public void getLinearVelocity(FixedFrameVector3DBasics linearVelocityToPack) { linearVelocityToPack.set(linearVelocity); }
/** * Sets this motion vector from a given motion measured at a different position. * <p> * Effectively, this motion is updated as follow: * * <pre> * ω<sub>this</sub> = ω<sub>new</sub> * ν<sub>this</sub> = ν<sub>new</sub> + P × ω<sub>new</sub> * </pre> * * where ω and ν represent the angular and linear parts respectively, and {@code P} is * the {@code observerPosition}. * </p> * * @param angularPart the angular part of the motion. Not modified. * @param linearPart the linear part of the motion measured at the observer position. Not * modified. * @param observerPosition the location at which the motion is measured. Not modified. */ default void set(Vector3DReadOnly angularPart, Vector3DReadOnly linearPart, Point3DReadOnly observerPosition) { getAngularPart().set(angularPart); double linearPartX = linearPart.getX(); double linearPartY = linearPart.getY(); double linearPartZ = linearPart.getZ(); getLinearPart().cross(observerPosition, angularPart); getLinearPart().add(linearPartX, linearPartY, linearPartZ); }
/** {@inheritDoc} */ @Override default void applyInverseTransform(Transform transform) { getAngularPart().applyInverseTransform(transform); getLinearPart().applyInverseTransform(transform); } }
cumulatedAngularVelocity.scaleAdd(joint.getQd(), jointAxis, cumulatedAngularVelocity); cumulatedAngularVelocity.changeFrame(bodyFrame); expectedTwist.getAngularPart().set(cumulatedAngularVelocity); assertTrue(expectedTwist.getAngularPart().epsilonEquals(actualTwist.getAngularPart(), 1.0e-12));
/** * Third and last pass that can be done iteratively and each iteration is independent. * <p> * Computes the Jacobian block corresponding to this joint. * </p> * * @param inverseOfTotalMass the inverse of the total system mass. */ public void passThree(double inverseOfTotalMass) { if (isRoot()) return; // Compute the Jacobian matrix block corresponding to the current joint. JointReadOnly joint = getJoint(); for (int i = 0; i < joint.getDegreesOfFreedom(); i++) { jointUnitTwist.setIncludingFrame(joint.getUnitTwists().get(i)); jointUnitTwist.changeFrame(jacobianFrame); jacobianColumn.cross(jointUnitTwist.getAngularPart(), centerOfMassTimesMass); jacobianColumn.scaleAdd(subTreeMass, jointUnitTwist.getLinearPart(), jacobianColumn); jacobianColumn.get(0, i, jacobianJointBlock); } CommonOps.scale(inverseOfTotalMass, jacobianJointBlock); for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) { int column = jointIndices[dofIndex]; CommonOps.extract(jacobianJointBlock, 0, 3, dofIndex, dofIndex + 1, jacobianMatrix, 0, column); } }
/** * Sets this frame vector to the cross product of {@code this} and {@code other}. * <p> * this = this × other * </p> * * @param other the second frame vector in the cross product. Not modified. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default void cross(FrameVector3DReadOnly other) { cross((FrameTuple3DReadOnly) other); }
/** * Transforms this spatial inertia using the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> */ @Override public void applyTransform(Transform transform) { if (transform instanceof RigidBodyTransform) { applyTransform((RigidBodyTransform) transform); } else { translation.setToZero(); translation.applyTransform(transform); // Let's first apply the rotation onto the CoM and the mass moment of inertia: momentOfInertia.applyTransform(transform); centerOfMassOffset.applyTransform(transform); // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(mass, centerOfMassOffset, false, translation, momentOfInertia); centerOfMassOffset.add(translation); } }
/** * Transforms this spatial inertia by the inverse of the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> */ @Override public void applyInverseTransform(Transform transform) { if (transform instanceof RigidBodyTransform) { applyInverseTransform((RigidBodyTransform) transform); } else { translation.setToZero(); translation.applyInverseTransform(transform); // Let's first apply the rotation onto the CoM and the mass moment of inertia: momentOfInertia.applyInverseTransform(transform); centerOfMassOffset.applyInverseTransform(transform); // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(mass, centerOfMassOffset, true, translation, momentOfInertia); centerOfMassOffset.add(translation); } }
/** * Transforms this spatial inertia by the inverse of the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> * * @param transform the transform to use on this. Not modified. */ default void applyInverseTransform(RigidBodyTransform transform) { if (transform.hasTranslation()) { // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(getMass(), getCenterOfMassOffset(), true, transform.getTranslationVector(), getMomentOfInertia()); getCenterOfMassOffset().sub(transform.getTranslationVector()); } if (transform.hasRotation()) { // Let's first apply the rotation onto the CoM and the mass moment of inertia: MecanoTools.inverseTransformSymmetricMatrix3D(transform.getRotationMatrix(), getMomentOfInertia()); getCenterOfMassOffset().applyInverseTransform(transform); } } }
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testprovidedMassMatrixToolRigidBody() { FullHumanoidRobotModel fullRobotModel = getFullRobotModel(); ReferenceFrame elevatorFrame = fullRobotModel.getElevatorFrame(); ProvidedMassMatrixToolRigidBody providedMassMatrixToolRigidBody = new ProvidedMassMatrixToolRigidBody(robotSide, fullRobotModel, gravity, registry, null); providedMassMatrixToolRigidBody.setMass(mass); SpatialAcceleration handSpatialAccelerationVector = new SpatialAcceleration(elevatorFrame, elevatorFrame, elevatorFrame); Wrench toolWrench = new Wrench(); providedMassMatrixToolRigidBody.control(handSpatialAccelerationVector, toolWrench); toolWrench.changeFrame(ReferenceFrame.getWorldFrame()); assertTrue(toolWrench.getLinearPart().epsilonEquals(new Vector3D(0.0, 0.0, -mass * gravity), 10e-5)); }
/** * Adds the given vectors to this vector angular and linear parts. * <p> * {@code this.angularPart += angular}<br> * {@code this.linearPart += linear} * </p> * * @param angular the vector to add to this vector's angular part. Not modified. * @param linear the vector to add to this vector's linear part. Not modified. */ default void add(Vector3DReadOnly angular, Vector3DReadOnly linear) { getAngularPart().add(angular); getLinearPart().add(linear); }
expectedTwist.getAngularPart().set(cumulatedAngularVelocity); assertTrue(expectedTwist.getAngularPart().epsilonEquals(actualTwist.getAngularPart(), 1.0e-12));
/** * Sets this frame vector to the cross product of {@code frameVector1} and {@code frameVector2}. * <p> * this = frameVector1 × frameVector2 * </p> * * @param frameVector1 the first frame vector in the cross product. Not modified. * @param frameVector2 the second frame vector in the cross product. Not modified. * @throws ReferenceFrameMismatchException if either {@code frameVector1} or {@code frameVector2} is * not expressed in the same reference frame as {@code this}. */ default void cross(FrameVector3DReadOnly frameVector1, FrameVector3DReadOnly frameVector2) { cross((FrameTuple3DReadOnly) frameVector1, (FrameTuple3DReadOnly) frameVector2); }
/** * Transforms this spatial inertia using the given transform. * <p> * See the Word™ document located in the document folder of this project for more * information about the transformation rule for spatial inertia. Also see Duindam, <i>Port-Based * Modeling and Control for Efficient Bipedal Walking Robots</i>, page 40, equation (2.57) from * which the equations here were derived. * </p> * * @param transform the transform to use on this. Not modified. */ default void applyTransform(RigidBodyTransform transform) { if (transform.hasRotation()) { // Let's first apply the rotation onto the CoM and the mass moment of inertia: MecanoTools.transformSymmetricMatrix3D(transform.getRotationMatrix(), getMomentOfInertia()); getCenterOfMassOffset().applyTransform(transform); } if (transform.hasTranslation()) { // Now we can simply apply the translation on the CoM and mass moment of inertia: MecanoTools.translateMomentOfInertia(getMass(), getCenterOfMassOffset(), false, transform.getTranslationVector(), getMomentOfInertia()); getCenterOfMassOffset().add(transform.getTranslationVector()); } }
/** * Adds the given vectors to this vector angular and linear parts. * <p> * {@code this.angularPart += angular}<br> * {@code this.linearPart += linear} * </p> * * @param angular the vector to add to this vector's angular part. Not modified. * @param linear the vector to add to this vector's linear part. Not modified. * @throws ReferenceFrameMismatchException if any of the arguments are not expressed in the same * reference frame as {@code this}. */ default void add(FrameVector3DReadOnly angular, FrameVector3DReadOnly linear) { getAngularPart().add(angular); getLinearPart().add(linear); }
/** {@inheritDoc} */ @Override default void setJointTorque(Vector3DReadOnly jointTorque) { getJointTorque().set(jointTorque); }
getAngularPart().applyInverseTransform(transform); getLinearPart().applyInverseTransform(transform);