@Override public RigidBodyTransform getOffsetTransform3D() { return new RigidBodyTransform(jointToWrap.getFrameBeforeJoint().getTransformToParent()); }
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); }
public KinematicsToolboxCommandConverter(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); }
public KinematicsPlanningToolboxCommandConverter(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); }
ReferenceFrame frameBeforeJoint = joint.getFrameBeforeJoint();
ReferenceFrame frameBeforeJoint = joint.getFrameBeforeJoint(); RigidBodyBasics successor = joint.getSuccessor();
/** * 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(); } } }
expectedTwist.setBodyFrame(child.getFrameBeforeJoint()); expectedTwist.changeFrame(child.getFrameBeforeJoint()); child.getFrameBeforeJoint().getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5);
/** * 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()); }