us.ihmc.commonWalkingControlModules.bipedSupportPolygons.YoPlaneContactState类的使用及代码示例

x33g5p2x  于2022-02-05 转载在 其他  
字(11.4k)|赞(0)|评价(0)|浏览(501)

本文整理了Java中us.ihmc.commonWalkingControlModules.bipedSupportPolygons.YoPlaneContactState类的一些代码示例，展示了YoPlaneContactState类的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。YoPlaneContactState类的具体详情如下：
包路径：us.ihmc.commonWalkingControlModules.bipedSupportPolygons.YoPlaneContactState
类名称：YoPlaneContactState

YoPlaneContactState介绍

暂无

代码示例

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setPlaneContactStateFree(ContactablePlaneBody contactableBody)
{
 YoPlaneContactState yoPlaneContactState = yoPlaneContactStates.get(contactableBody);
 if (yoPlaneContactState != null)
   yoPlaneContactState.clear();
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

@Override
  public String toString()
  {
   return "Body: " + rigidBody.getName() + ", in contact: " + inContact() + ", nunber of CPs: " + getTotalNumberOfContactPoints();
  }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setPlaneContactStateFullyConstrained(ContactablePlaneBody contactableBody, double coefficientOfFriction, FrameVector normalContactVector)
{
 YoPlaneContactState yoPlaneContactState = yoPlaneContactStates.get(contactableBody);
 yoPlaneContactState.setFullyConstrained();
 yoPlaneContactState.setCoefficientOfFriction(coefficientOfFriction);
 yoPlaneContactState.setContactNormalVector(normalContactVector);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

private void setFootPlaneContactPoints(RobotSide robotSide, List<Point2d> predictedContactPoints)
{
 ContactablePlaneBody foot = feet.get(robotSide);
 YoPlaneContactState footContactState = yoPlaneContactStates.get(foot);
 footContactState.getContactFramePoint2dsInContact(previousFootContactPoints.get(robotSide));
 footContactState.setContactPoints(predictedContactPoints);
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

@Before
public void setUp()
{
 parentRegistry = new YoVariableRegistry("parentRegistryTEST");
 for (RobotSide robotSide : RobotSide.values)
 {
   String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression();
   double xToAnkle = 0.0;
   double yToAnkle = 0.0;
   double zToAnkle = 0.0;
   List<Point2D> contactPointsInSoleFrame = new ArrayList<>();
   contactPointsInSoleFrame.add(new Point2D(footLengthForControl / 2.0, toeWidthForControl / 2.0));
   contactPointsInSoleFrame.add(new Point2D(footLengthForControl / 2.0, -toeWidthForControl / 2.0));
   contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, -footWidthForControl / 2.0));
   contactPointsInSoleFrame.add(new Point2D(-footLengthForControl / 2.0, footWidthForControl / 2.0));
   FootSpoof contactableFoot = new FootSpoof(sidePrefix + "Foot", xToAnkle, yToAnkle, zToAnkle, contactPointsInSoleFrame, 0.0);
   FramePose3D startingPose = new FramePose3D();
   startingPose.setToZero(worldFrame);
   startingPose.setY(robotSide.negateIfRightSide(0.20));
   contactableFoot.setSoleFrame(startingPose);
   contactableFeet.put(robotSide, contactableFoot);
   RigidBodyBasics foot = contactableFoot.getRigidBody();
   ReferenceFrame soleFrame = contactableFoot.getSoleFrame();
   List<FramePoint2D> contactFramePoints = contactableFoot.getContactPoints2d();
   double coefficientOfFriction = contactableFoot.getCoefficientOfFriction();
   YoPlaneContactState yoPlaneContactState = new YoPlaneContactState(sidePrefix + "Foot", foot, soleFrame, contactFramePoints, coefficientOfFriction, parentRegistry);
   yoPlaneContactState.setFullyConstrained();
   contactStates.put(robotSide, yoPlaneContactState);
 }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void computeMatrices(double rhoWeight, double rhoRateWeight, Vector2d desiredCoPWeight, Vector2d copRateWeight)
 int numberOfContactPointsInContact = yoPlaneContactState.getNumberOfContactPointsInContact();
 if (numberOfContactPointsInContact > maxNumberOfContactPoints)
   throw new RuntimeException("Unhandled number of contact points: " + numberOfContactPointsInContact);
 List<YoContactPoint> contactPoints = yoPlaneContactState.getContactPoints();
 for (int contactPointIndex = 0; contactPointIndex < yoPlaneContactState.getTotalNumberOfContactPoints(); contactPointIndex++)
 boolean isFootholdAreaLargeEnough = yoPlaneContactState.getFootholdArea() > 1.0e-3;
 if (yoPlaneContactState.inContact() && !resetRequested.getBooleanValue() && isFootholdAreaLargeEnough)

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

@Override
public void getPlaneContactStateCommand(PlaneContactStateCommand planeContactStateCommandToPack)
{
 planeContactStateCommandToPack.setId(NameBasedHashCodeTools.combineHashCodes(getNumberOfContactPointsInContact(), rigidBody));
 planeContactStateCommandToPack.clearContactPoints();
 planeContactStateCommandToPack.setContactingRigidBody(rigidBody);
 planeContactStateCommandToPack.setCoefficientOfFriction(coefficientOfFriction.getDoubleValue());
 planeContactStateCommandToPack.setContactNormal(contactNormalFrameVector);
 if (!inContact())
   return;
 for (int i = 0; i < getTotalNumberOfContactPoints(); i++)
 {
   YoContactPoint contactPoint = contactPoints.get(i);
   if (contactPoint.isInContact())
   {
    contactPoint.getPosition(tempContactPointPosition);
    planeContactStateCommandToPack.addPointInContact(tempContactPointPosition);
   }
 }
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

private void updateContactState(int currentStepCount, double percentageOfPhase)
{
 if (inDoubleSupport.getBooleanValue())
 {
   for (RobotSide side : RobotSide.values)
    contactStates.get(side).setFullyConstrained();
 }
 else
 {
   RobotSide swingSide = footstepList.get(currentStepCount).getRobotSide();
   contactStates.get(swingSide).clear();
   contactStates.get(swingSide.getOppositeSide()).setFullyConstrained();
   if (currentStepCount > 0)
   {
    swingFootPose.set(footstepList.get(currentStepCount - 1).getFootstepPose());
   }
   else
   {
    swingFootPose.set(initialPose);
    swingFootPose.appendTranslation(0.0, swingSide.negateIfRightSide(stepWidth / 2.0), 0.0);
   }
   FramePose3D endOfSwing = footstepList.get(currentStepCount).getFootstepPose();
   swingFootPose.interpolate(endOfSwing, percentageOfPhase);
   FootSpoof foot = feet.get(swingSide);
   foot.setSoleFrame(swingFootPose);
 }
 bipedSupportPolygons.updateUsingContactStates(contactStates);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setPlaneContactStateFullyConstrained(ContactablePlaneBody contactableBody)
{
 YoPlaneContactState yoPlaneContactState = yoPlaneContactStates.get(contactableBody);
 yoPlaneContactState.setFullyConstrained();
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

yoPlaneContactState = new YoPlaneContactState(namePrefix, rigidBody, planeFrame, contactPoints2d, 0.0, registry);

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

@Override
public void updateFromPlaneContactStateCommand(PlaneContactStateCommand planeContactStateCommand)
{
 if (planeContactStateCommand.getContactingRigidBody() != rigidBody)
   throw new RuntimeException("The rigid body in the command does not match this rigid body: command.rigidBody = " + planeContactStateCommand.getContactingRigidBody() + ", contactState.rigidBody = " + rigidBody);
 coefficientOfFriction.set(planeContactStateCommand.getCoefficientOfFriction());
 planeContactStateCommand.getContactNormal(contactNormalFrameVector);
 if (planeContactStateCommand.isEmpty())
   clear();
 else
   inContact.set(true);
 for (int i = 0; i < planeContactStateCommand.getNumberOfContactPoints(); i++)
 {
   planeContactStateCommand.getContactPoint(i, tempContactPointPosition);
   YoContactPoint contactPoint = contactPoints.get(i);
   contactPoint.setPosition(tempContactPointPosition);
   contactPoint.setInContact(true);
 }
 for (int i = planeContactStateCommand.getNumberOfContactPoints(); i < getTotalNumberOfContactPoints(); i++)
 {
   contactPoints.get(i).setInContact(false);
 }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void computeWrenchFromRho(int startIndex, DenseMatrix64F allRobotRho)
{
 CommonOps.extract(allRobotRho, startIndex, startIndex + rhoSize, 0, 1, rhoMatrix, 0, 0);
 yoRho.set(rhoMatrix);
 if (yoPlaneContactState.inContact())
 {
   totalWrenchMatrix.zero();
   for (int rhoIndex = 0; rhoIndex < rhoSize; rhoIndex++)
   {
    double rho = rhoMatrix.get(rhoIndex, 0);
    CommonOps.extract(rhoJacobianMatrix, 0, SpatialForceVector.SIZE, rhoIndex, rhoIndex + 1, singleRhoWrenchMatrix, 0, 0);
    MatrixTools.addMatrixBlock(totalWrenchMatrix, 0, 0, singleRhoWrenchMatrix, 0, 0, SpatialForceVector.SIZE, 1, rho);
   }
   RigidBody rigidBody = yoPlaneContactState.getRigidBody();
   ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame();
   wrenchFromRho.set(bodyFixedFrame, centerOfMassFrame, totalWrenchMatrix);
   CommonOps.mult(copJacobianMatrix, rhoMatrix, previousCoPMatrix);
   previousCoP.setX(previousCoPMatrix.get(0, 0));
   previousCoP.setY(previousCoPMatrix.get(1, 0));
 }
 else
 {
   wrenchFromRho.setToZero();
 }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void resetFootSupportPolygon(RobotSide robotSide)
{
 YoPlaneContactState contactState = footContactStates.get(robotSide);
 List<YoContactPoint> contactPoints = contactState.getContactPoints();
 FrameConvexPolygon2d defaultSupportPolygon = defaultFootPolygons.get(robotSide);
 for (int i = 0; i < defaultSupportPolygon.getNumberOfVertices(); i++)
 {
   defaultSupportPolygon.getFrameVertexXY(i, tempPosition);
   contactPoints.get(i).setPosition(tempPosition);
 }
 contactState.notifyContactStateHasChanged();
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setPlaneContactStateCommand(PlaneContactStateCommand command)
{
 yoPlaneContactState.updateFromPlaneContactStateCommand(command);
 yoPlaneContactState.computeSupportPolygon();
 if (lastCommandId.getLongValue() != command.getId())
 {
   resetRequested.set(true);
   lastCommandId.set(command.getId());
 }
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

private void updateCurrentFootsteps()
{
 for (RobotSide side : RobotSide.values)
 {
   YoFramePoseUsingYawPitchRoll footPose = currentFootLocations.get(side);
   if (contactStates.get(side).inContact())
   {
    footPose.setFromReferenceFrame(feet.get(side).getSoleFrame());
   }
   else
   {
    footPose.setToNaN();
   }
 }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

if (footContactStates != null && footContactStates.get(leadingLeg).getTotalNumberOfContactPoints() > 0)
  footContactStates.get(leadingLeg).getContactFramePointsInContact(contactStatePoints);
  leadingFootSupportPolygon.setIncludingFrameByProjectionOntoXYPlaneAndUpdate(worldFrame, contactStatePoints);

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void getContactPoints(ContactablePlaneBody contactablePlaneBody, List<FramePoint> contactPointListToPack)
{
 yoPlaneContactStates.get(contactablePlaneBody).getContactFramePointsInContact(contactPointListToPack);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

private void computeBasisVector(int basisVectorIndex, Matrix3d normalContactVectorRotationMatrix, FrameVector basisVectorToPack)
{
 double angle = basisVectorIndex * basisVectorAngleIncrement;
 double mu = yoPlaneContactState.getCoefficientOfFriction();
 // Compute the linear part considering a normal contact vector pointing z-up
 basisVectorToPack.setIncludingFrame(planeFrame, Math.cos(angle) * mu, Math.sin(angle) * mu, 1.0);
 // Transforming the result to consider the actual normal contact vector
 normalContactVectorRotationMatrix.transform(basisVectorToPack.getVector());
 basisVectorToPack.normalize();
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

private void computeNormalContactVectorRotation(Matrix3d normalContactVectorRotationMatrixToPack)
{
 yoPlaneContactState.getContactNormalFrameVector(contactNormalVector);
 contactNormalVector.changeFrame(planeFrame);
 contactNormalVector.normalize();
 GeometryTools.getAxisAngleFromZUpToVector(contactNormalVector.getVector(), normalContactVectorRotation);
 normalContactVectorRotationMatrixToPack.set(normalContactVectorRotation);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

List<YoContactPoint> contactPoints = contactStateToModify.getContactPoints();
for (int i = 0; i < controllerFootPolygon.getNumberOfVertices(); i++)

内容来源于网络，如有侵权，请联系作者删除！

YoPlaneContactState

相关文章

    查看更多

    热门标签

    更多
    JavaquerypythonNode开发语言requestUtil数据库Table后端算法LoggerMessageElementParser

    最新文章

    更多
    • 蜀ICP备13028337号-1 大数据知识库 https://www.saoniuhuo.com © All rights reserved
    • 本站内容来源互联网,如果侵犯您的权益请联系我们删除, 联系方式：448109455@qq.com