/** * Packs the motion subspace of this joint into the given matrix. * <p> * The resulting matrix is a 6-by-N matrix, where N is equal to the number of DoFs this joint * has, see {@link #getDegreesOfFreedom()}. The motion subspace is equivalent to the geometric * Jacobian of this joint only expressed in {@code frameAfterJoint}. * </p> * * @param matrixToPack the matrix used to store the motion subspace. It is reshaped to the proper * size. Modified. */ default void getMotionSubspace(DenseMatrix64F matrixToPack) { matrixToPack.reshape(SpatialVectorReadOnly.SIZE, getDegreesOfFreedom()); for (int dofIndex = 0; dofIndex < getDegreesOfFreedom(); dofIndex++) getUnitTwists().get(dofIndex).get(0, dofIndex, matrixToPack); }
/** * Updates the values of the Jacobian matrix. * <p> * The base, end-effector, and jacobian frame, have all to be properly set before calling this * method. * </p> * * @throws RuntimeException if either the base or the end-effector has not been provided beforehand. */ private void updateJacobianMatrix() { if (isJacobianUpToDate) return; if (base == null || endEffector == null) throw new RuntimeException("The base and end-effector have to be set first."); jacobianMatrix.reshape(SpatialVectorReadOnly.SIZE, numberOfDegreesOfFreedom); int column = 0; for (int jointIndex = 0; jointIndex < jointsFromBaseToEndEffector.size(); jointIndex++) { JointReadOnly joint = jointsFromBaseToEndEffector.get(jointIndex); for (int dofIndex = 0; dofIndex < joint.getDegreesOfFreedom(); dofIndex++) { jointUnitTwist.setIncludingFrame(joint.getUnitTwists().get(dofIndex)); jointUnitTwist.changeFrame(jacobianFrame); jointUnitTwist.get(0, column++, jacobianMatrix); } } isJacobianUpToDate = true; }
public CompositeRigidBodyInertia(RigidBodyReadOnly rigidBody, CompositeRigidBodyInertia parent, int[] jointIndices) { this.rigidBody = rigidBody; this.parent = parent; this.jointIndices = jointIndices; if (parent == null) { bodyInertia = null; unitMomenta = null; compositeInertia = null; unitTwists = null; coriolisBodyAcceleration = new SpatialAcceleration(getBodyFixedFrame(), input.getInertialFrame(), getBodyFixedFrame()); } else { parent.children.add(this); int nDoFs = getNumberOfDoFs(); bodyInertia = new SpatialInertia(rigidBody.getInertia()); compositeInertia = new SpatialInertia(); unitMomenta = IntStream.range(0, nDoFs).mapToObj(i -> new Momentum()).toArray(Momentum[]::new); unitTwists = new Twist[nDoFs]; for (int i = 0; i < nDoFs; i++) { unitTwists[i] = new Twist(getJoint().getUnitTwists().get(i)); unitTwists[i].changeFrame(getFrameAfterJoint()); } coriolisBodyAcceleration = new SpatialAcceleration(); } }
/** * Second pass that can be done iteratively and each iteration is independent. * <p> * Computes the block of the centroidal momentum matrix for this parent joint. * </p> */ public void passTwo() { if (isRoot()) return; for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) { unitMomentum.setToZero(); jointUnitTwist.setIncludingFrame(getJoint().getUnitTwists().get(dofIndex)); jointUnitTwist.changeFrame(matrixFrame); addToUnitMomentumRecursively(jointUnitTwist, unitMomentum); unitMomentum.get(0, dofIndex, centroidalMomentumMatrixBlock); int column = jointIndices[dofIndex]; CommonOps.extract(centroidalMomentumMatrixBlock, 0, 6, dofIndex, dofIndex + 1, centroidalMomentumMatrix, 0, column); } }
/** * Second pass that can be done iteratively and each iteration is independent. * <p> * Computes the block of the centroidal momentum matrix for this parent joint. * </p> */ public void passTwo() { if (isRoot()) return; for (int i = 0; i < getJoint().getDegreesOfFreedom(); i++) { unitMomentum.setToZero(); jointUnitTwist.setIncludingFrame(getJoint().getUnitTwists().get(i)); jointUnitTwist.changeFrame(matrixFrame); addToUnitMomentumRecursively(jointUnitTwist, unitMomentum); unitMomentum.get(0, i, centroidalMomentumMatrixBlock); } for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) { int column = jointIndices[dofIndex]; CommonOps.extract(centroidalMomentumMatrixBlock, 0, 6, dofIndex, dofIndex + 1, centroidalMomentumMatrix, 0, column); } }
/** * 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); } }