private void printJointInformation(JointBasics joint, StringBuffer buffer) String jointName = joint.getName(); JointBasics parentJoint = joint.getPredecessor().getParentJoint(); if (parentJoint == null) ReferenceFrame frameBeforeJoint = joint.getFrameBeforeJoint(); ReferenceFrame frameAfterParentJoint = parentJoint.getFrameAfterJoint(); RigidBodyTransform comOffsetTransform = frameBeforeJoint.getTransformToDesiredFrame(frameAfterParentJoint); comOffsetTransform.getTranslation(jointOffset); RigidBodyBasics linkRigidBody = joint.getSuccessor(); printLinkInformation(linkRigidBody, buffer);
private void storeJointState() { MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.ACCELERATION, storedJointDesiredAccelerations); MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.VELOCITY, storedJointVelocities); for (JointBasics joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getJointTau(0, tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForce.SIZE, 1); DenseMatrix64F motionSubspace = new DenseMatrix64F(6, joint.getDegreesOfFreedom()); joint.getMotionSubspace(motionSubspace); CommonOps.mult(motionSubspace, tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getFrameAfterJoint()); } }
private void setDesiredAccelerationsToZero() { for (JointBasics joint : jointsInOrder) { joint.setJointAccelerationToZero(); joint.setJointVelocity(0, new DenseMatrix64F(joint.getDegreesOfFreedom(), 1)); } }
/** * 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 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; }
private static final void areJointsTheSame(JointBasics originalJoint, JointBasics targetJoint) { if(!(originalJoint.getClass().equals(targetJoint.getClass()) && originalJoint.getName().equals(targetJoint.getName()))) { throw new RuntimeException(originalJoint.getName() + " differs from " + targetJoint); } } }
private void addJoint(CollisionBoxProvider collissionBoxProvider, JointBasics joint) { List<CollisionShape> collisionMesh = collissionBoxProvider.getCollisionMesh(joint.getName()); if (collisionMesh != null) { trackingCollisionShapes.add(new TrackingCollisionShape(joint.getFrameAfterJoint(), collisionMesh)); } else { System.err.println(joint + " does not have a collission mesh"); } }
public MovingReferenceFrame getFrameAfterParentJoint() { return ankle.getFrameAfterJoint(); }
/** * 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(); }
public static Joint addSCSJointUsingIDJoint(JointBasics idJoint, Robot scsRobot, boolean isRootJoint) String jointName = idJoint.getName(); RigidBodyTransform offsetTransform = new RigidBodyTransform(); idJoint.getJointOffset(offsetTransform); Vector3D offsetVector = new Vector3D(); offsetTransform.getTranslation(offsetVector); RigidBodyBasics idRigidBody = idJoint.getSuccessor(); SpatialInertiaBasics idInertia = idRigidBody.getInertia(); Vector3D comOffset = new Vector3D(); FramePoint3D centerOfMassOffset = new FramePoint3D(idInertia.getCenterOfMassOffset()); centerOfMassOffset.changeFrame(idJoint.getFrameAfterJoint()); comOffset.set(centerOfMassOffset); double mass = idInertia.getMass(); if (currentSCSJoint.getName().equals(idJoint.getPredecessor().getParentJoint().getName()))
/** * Updates {@code afterJointFrame} of this joint to take into consideration the new joint * configuration. Then calls {@link RigidBody#updateFramesRecursively()} which in its turn updates * its {@code bodyFixedFrame} and then {@link #updateFramesRecursively()} for all of its * {@link JointBasics} child. * <p> * As a result, this method will update all the reference frames of the subtree starting from this * joint. * </p> * <p> * In addition to updating their respective poses, the reference frame also updates their velocity * based on the joint velocities. * </p> */ default void updateFramesRecursively() { getFrameBeforeJoint().update(); getFrameAfterJoint().update(); if (getSuccessor() != null) { getSuccessor().updateFramesRecursively(); } } }
double b_vel_limit = 500.0; String firstAnkleName = fullRobotModel.getFoot(robotSide).getParentJoint().getName(); if (simulatedRobot.getOneDegreeOfFreedomJoint(firstAnkleName) instanceof PinJoint) String secondAnkleName = fullRobotModel.getFoot(robotSide).getParentJoint().getPredecessor().getParentJoint().getName(); if (simulatedRobot.getOneDegreeOfFreedomJoint(secondAnkleName) instanceof PinJoint)
public WholeBodyTrajectoryToolboxCommandConverter(RigidBodyBasics rootBody) { List<ReferenceFrame> referenceFrames = new ArrayList<>(); for (JointBasics joint : rootBody.childrenSubtreeIterable()) { referenceFrames.add(joint.getFrameAfterJoint()); referenceFrames.add(joint.getFrameBeforeJoint()); } for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) { referenceFrames.add(rigidBody.getBodyFixedFrame()); } referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(referenceFrames); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
/** * Sets this joint current force/torque to the given wrench. * <p> * As for the other spatial setters, this method's common implementation is to project the given * wrench to this joint's motion subspace. For instance, a {@code RevoluteJoint} that can rotate * around the y-axis will extract the torque around the y-axis from the given wrench to update its * torque. * </p> * * @param jointWrench the new wrench for this joint. Not modified. * @throws ReferenceFrameMismatchException if the given wrench does not have the following frames: * <ul> * <li>{@code bodyFrame} is {@code successor.getBodyFixedFrame()}. * <li>{@code expressedInFrame} is {@code frameAfterJoint}. * </ul> */ default void setJointWrench(WrenchReadOnly jointWrench) { jointWrench.checkBodyFrameMatch(getSuccessor().getBodyFixedFrame()); jointWrench.checkExpressedInFrameMatch(getFrameAfterJoint()); setJointTorque((Vector3DReadOnly) jointWrench.getAngularPart()); setJointForce((Vector3DReadOnly) jointWrench.getLinearPart()); }
/** * Sets this joint current acceleration to the given spatial acceleration. * <p> * As for configuration setters, the common implementation here is to project the given acceleration * onto the motion subspace of this joint such that not all the components of the given spatial * acceleration may necessarily be used. * </p> * * @param jointAcceleration the new acceleration for this joint. Not modified. * @throws ReferenceFrameMismatchException if the given spatial acceleration does not have the * following frames: * <ul> * <li>{@code bodyFrame} is {@code afterJointFrame}. * <li>{@code baseFrame} is {@code beforeJointFrame}. * <li>{@code expressedInFrame} is {@code afterJointFrame}. * </ul> */ default void setJointAcceleration(SpatialAccelerationReadOnly jointAcceleration) { jointAcceleration.checkBodyFrameMatch(getFrameAfterJoint()); jointAcceleration.checkBaseFrameMatch(getFrameBeforeJoint()); jointAcceleration.checkExpressedInFrameMatch(getFrameAfterJoint()); setJointAngularAcceleration((Vector3DReadOnly) jointAcceleration.getAngularPart()); setJointLinearAcceleration((Vector3DReadOnly) jointAcceleration.getLinearPart()); }
/** * Sets this joint current velocity to the given twist. * <p> * As for configuration setters, the common implementation here is to project the given twist onto * the motion subspace of this joint such that not all the components of the given twist may * necessarily be used. * </p> * <p> * When updating the velocity the joints of a robot, it is important to then call * {@link #updateFramesRecursively()} on the root joint to update all the reference frames to the * new robot motion. * </p> * * @param jointTwist the new twist for this joint. Not modified. * @throws ReferenceFrameMismatchException if the given twist does not have the following frames: * <ul> * <li>{@code bodyFrame} is {@code afterJointFrame}. * <li>{@code baseFrame} is {@code beforeJointFrame}. * <li>{@code expressedInFrame} is {@code afterJointFrame}. * </ul> */ default void setJointTwist(TwistReadOnly jointTwist) { jointTwist.checkBodyFrameMatch(getFrameAfterJoint()); jointTwist.checkBaseFrameMatch(getFrameBeforeJoint()); jointTwist.checkExpressedInFrameMatch(getFrameAfterJoint()); setJointAngularVelocity((Vector3DReadOnly) jointTwist.getAngularPart()); setJointLinearVelocity((Vector3DReadOnly) jointTwist.getLinearPart()); }
private RigidBody(String bodyName, JointBasics parentJoint, RigidBodyTransform inertiaPose) { if (bodyName == null) throw new IllegalArgumentException("Name can not be null"); name = bodyName; this.parentJoint = parentJoint; ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); bodyFixedFrame = MovingReferenceFrame.constructFrameFixedInParent(bodyName + "CoM", frameAfterJoint, inertiaPose); inertia = new SpatialInertia(bodyFixedFrame, bodyFixedFrame); // inertia should be expressed in body frame, otherwise it will change inertia.getBodyFrame().checkReferenceFrameMatch(inertia.getReferenceFrame()); parentJoint.setSuccessor(this); nameId = parentJoint.getPredecessor().getNameId() + NAME_ID_SEPARATOR + bodyName; }
public JointAxisVisualizer(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); if(joint instanceof OneDoFJointBasics) { FrameVector3DReadOnly jAxis=((OneDoFJointBasics)joint).getJointAxis(); ReferenceFrame referenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(joint.getName()+"JointAxis", new FramePoint3D(jAxis.getReferenceFrame()), new FrameVector3D(jAxis.getReferenceFrame(),jAxis)); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame , registry, false, length, YoAppearance.Gold()); yoGraphicsList.add(yoGraphicReferenceFrame); yoGraphicReferenceFrames.add(yoGraphicReferenceFrame); } List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints(); jointStack.addAll(childrenJoints); jointStack.remove(joint); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
/** * Sets this joint current force. * <p> * This method is ineffective for joints that cannot translate. * </p> * <p> * As for the other spatial setters, this method's common implementation is to project the given 3D * force to this joint's motion subspace. For instance, a {@code PrismaticJoint} that can translate * along the z-axis will extract the force along the z-axis from the given 3D force to update its * force. * </p> * * @param jointForce the new force for this joint. Unused if this joint cannot translate. Not * modified. * @throws ReferenceFrameMismatchException if the given vector is not expressed in * {@code frameAfterJoint}. */ default void setJointForce(FrameVector3DReadOnly jointForce) { jointForce.checkReferenceFrameMatch(getFrameAfterJoint()); setJointForce((Vector3DReadOnly) jointForce); }
/** * Sets the angular acceleration of this joint. * <p> * This method is ineffective for joints that cannot rotate. * </p> * <p> * As for configuration setters, the common implementation here is to project the given angular * acceleration onto the motion subspace of this joint such that not all the components of the given * acceleration may necessarily be used. * </p> * * @param jointAngularAcceleration the new angular acceleration for this joint. Unused if this joint * cannot rotate. Not modified. * @throws ReferenceFrameMismatchException if the given vector is not expressed in * {@code frameAfterJoint}. */ default void setJointAngularAcceleration(FrameVector3DReadOnly jointAngularAcceleration) { jointAngularAcceleration.checkReferenceFrameMatch(getFrameAfterJoint()); setJointAngularAcceleration((Vector3DReadOnly) jointAngularAcceleration); }