/** * 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); }
/** * 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); }
/** * 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); }
/** * 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); } }
double z = getAngularPartZ(); getAngularPart().cross(spatialInertia.getCenterOfMassOffset(), twist.getLinearPart()); getAngularPart().scale(spatialInertia.getMass()); getAngularPart().add(x, y, z);