us.ihmc.mecano.multiBodySystem.interfaces.JointBasics.getSuccessor()方法的使用及代码示例

x33g5p2x  于2022-01-22 转载在 其他  
字(16.3k)|赞(0)|评价(0)|浏览(74)

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

JointBasics.getSuccessor介绍

暂无

代码示例

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

/**
* Returns the end-effector {@code RigidBody} of this Jacobian. The end-effector is the successor
* of the last joint this Jacobian considers.
* 
* @return the end-effector of this jacobian.
*/
public RigidBodyBasics getEndEffector()
{
 return joints[joints.length - 1].getSuccessor();
}

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

/**
* Writes the computed acceleration into the given {@code joint}.
* <p>
* Any joint that is not considered by this calculator remains unchanged.
* </p>
* 
* @param joint the joint to retrieve the acceleration of and to store it. Modified.
* @return whether the calculator handles the given joint or not.
*/
public boolean writeComputedJointAcceleration(JointBasics joint)
{
 ArticulatedBodyRecursionStep recursionStep = rigidBodyToRecursionStepMap.get(joint.getSuccessor());
 if (recursionStep == null)
   return false;
 joint.setJointAcceleration(0, recursionStep.qdd);
 return true;
}

代码示例来源：origin: us.ihmc/simulation-construction-set-tools

public InverseDynamicsMechanismReferenceFrameVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry,
   double length)
{
 YoGraphicsList yoGraphicsList = new YoGraphicsList(name);
 List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints());
 while (!jointStack.isEmpty())
 {
   JointBasics joint = jointStack.get(0);
   ReferenceFrame referenceFrame = joint.getSuccessor().getBodyFixedFrame();
   YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame, registry, false, length);
   yoGraphicsList.add(yoGraphicReferenceFrame);
   yoGraphicReferenceFrames.add(yoGraphicReferenceFrame);
   List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints();
   jointStack.addAll(childrenJoints);
   jointStack.remove(joint);
 }
 yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
}

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

public boolean writeComputedJointInstanteneousVelocityChange(JointBasics joint)
{
 ImpulseRecursionStep recursionStep = rigidBodyToRecursionStepMap.get(joint.getSuccessor());
 if (recursionStep == null)
   return false;
 jointVelocityMatrix.reshape(joint.getDegreesOfFreedom(), 1);
 joint.getJointVelocity(0, jointVelocityMatrix);
 CommonOps.addEquals(jointVelocityMatrix, recursionStep.delta_qd);
 return true;
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public static double computeSubTreeMass(RigidBodyBasics rootBody)
{
 SpatialInertiaBasics inertia = rootBody.getInertia();
 double ret = inertia == null ? 0.0 : inertia.getMass();
 for (JointBasics childJoint : rootBody.getChildrenJoints())
 {
   ret += computeSubTreeMass(childJoint.getSuccessor());
 }
 return ret;
}

代码示例来源：origin: us.ihmc/simulation-construction-set-tools

private void addInverseDynamicsJoints(List<? extends JointBasics> joints, GraphicsJoint parentJoint, GraphicsObjectsHolder graphicsObjectsHolder,
                   boolean useCollisionMeshes)
{
 for (JointBasics joint : joints)
 {
   GraphicsJoint graphicsJoint = createJoint(joint, Graphics3DNodeType.JOINT, graphicsObjectsHolder, useCollisionMeshes);
   parentJoint.addChild(graphicsJoint);
   addInverseDynamicsJoints(joint.getSuccessor().getChildrenJoints(), graphicsJoint, graphicsObjectsHolder, useCollisionMeshes);
 }
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public static NumericalInverseKinematicsCalculator createIKCalculator(JointBasics[] jointsToControl, int maxIterations)
{
 JointBasics[] cloneOfControlledJoints = MultiBodySystemFactories.cloneKinematicChain(jointsToControl);
 int numberOfDoFs = cloneOfControlledJoints.length;
 RigidBodyBasics cloneOfEndEffector = cloneOfControlledJoints[numberOfDoFs - 1].getSuccessor();
 ReferenceFrame cloneOfEndEffectorFrame = cloneOfEndEffector.getBodyFixedFrame();
 GeometricJacobian jacobian = new GeometricJacobian(cloneOfControlledJoints, cloneOfEndEffectorFrame);
 double lambdaLeastSquares = 0.0009;
 double tolerance = 0.001;
 double maxStepSize = 0.2;
 double minRandomSearchScalar = 0.02;
 double maxRandomSearchScalar = 0.8;
 return new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar,
    maxRandomSearchScalar);
}

代码示例来源：origin: us.ihmc/simulation-construction-set-tools

public GraphicsIDRobot(String name, RigidBodyBasics rootBody, GraphicsObjectsHolder graphicsObjectsHolder, boolean useCollisionMeshes)
{
 super(new Graphics3DNode(name, Graphics3DNodeType.TRANSFORM));
 for (JointBasics joint : rootBody.getChildrenJoints())
 {
   GraphicsJoint rootGraphicsJoint = createJoint(joint, Graphics3DNodeType.ROOTJOINT, graphicsObjectsHolder, useCollisionMeshes);
   getRootNode().addChild(rootGraphicsJoint);
   addInverseDynamicsJoints(joint.getSuccessor().getChildrenJoints(), rootGraphicsJoint, graphicsObjectsHolder, useCollisionMeshes);
 }
 update();
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.6)
@Test(timeout = 30000)
public void testVMC()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame and no selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 submitAndCheckVMC(pelvis, foot, desiredWrench, null);
}

代码示例来源：origin: us.ihmc/ihmc-avatar-interfaces-test

private void getExternalWrenchesFromSCS()
{
  calculator.setExternalWrenchesToZero();
  for (ExternalForcePoint efp : robot.getAllGroundContactPoints())
  {
   String parentJointName = efp.getParentJoint().getName();
   RigidBodyBasics body = nameToJointMap.get(parentJointName).getSuccessor();
   FrameVector3DReadOnly moment = efp.getYoMoment();
   FrameVector3DReadOnly force = efp.getYoForce();
   FramePoint3D pointOfApplication = new FramePoint3D(efp.getYoPosition());
   pointOfApplication.changeFrame(body.getBodyFixedFrame());
   SpatialVector vector6D = new SpatialVector(moment, force);
   vector6D.changeFrame(body.getBodyFixedFrame());
   Wrench externalWrench = new Wrench(body.getBodyFixedFrame(), body.getBodyFixedFrame());
   externalWrench.set(vector6D.getAngularPart(), vector6D.getLinearPart(), pointOfApplication);
   calculator.getExternalWrench(body).add(externalWrench);
  }
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.6)
@Test(timeout = 30000)
public void testVMCSelectAll()
{
   RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.6)
@Test(timeout = 30000)
public void testVMCSelectForceZ()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.clearSelection();
 selectionMatrix.selectLinearZ(true);
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.6)
@Test(timeout = 30000)
public void testVMCSelectForceX()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.clearSelection();
 selectionMatrix.selectLinearX(true);
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.6)
@Test(timeout = 30000)
public void testVMCSelectForceY()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.clearSelection();
 selectionMatrix.selectLinearY(true);
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.5)
@Test(timeout = 30000)
public void testVMCSelectTorqueX()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.clearSelection();
 selectionMatrix.selectAngularX(true);
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.5)
@Test(timeout = 30000)
public void testVMCSelectTorqueZ()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.clearSelection();
 selectionMatrix.selectAngularZ(true);
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.5)
@Test(timeout = 30000)
public void testVMCSelectTorque()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 // select only torque
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.clearSelection();
 selectionMatrix.setToAngularSelectionOnly();
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.5)
@Test(timeout = 30000)
public void testVMCSelectForceXTorqueY()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 // select only torque
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.selectLinearX(true);
 selectionMatrix.selectAngularY(true);
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.6)
@Test(timeout = 30000)
public void testVMCWrongExpressedInFrame()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), pelvis.getBodyFixedFrame(), desiredTorque, desiredForce);
 // select only torque
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.clearSelection();
 selectionMatrix.setToAngularSelectionOnly();
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.5)
@Test(timeout = 30000)
public void testVMCSelectForceXTorqueXZ()
{
 RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity);
 RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT);
 RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor();
 RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor();
 // send in the correct frame with identity selection matrix
 FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame());
 Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce);
 SelectionMatrix6D selectionMatrix = new SelectionMatrix6D();
 selectionMatrix.selectLinearX(true);
 selectionMatrix.selectAngularX(true);
 selectionMatrix.selectAngularZ(true);
 submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix);
}

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

JointBasics

相关文章

    查看更多
    微信公众号

    热门标签

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

    最新文章

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