/** * 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(); }
/** * 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; }
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); }
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; }
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; }
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); } }
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); }
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(); }
@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); }
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); } }
@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); }
@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); }
@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); }
@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); }
@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); }
@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); }
@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); }
@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); }
@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); }
@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); }