/** * Iterates through the given joints and sums their number degrees of freedom. * * @param joints the kinematic chain to compute the total number of degrees of freedom of. * @return the total number of degrees of freedom. */ public static int computeDegreesOfFreedom(JointReadOnly[] joints) { int numberOfDegreesOfFreedom = 0; for (int i = 0; i < joints.length; i++) { numberOfDegreesOfFreedom += joints[i].getDegreesOfFreedom(); } return numberOfDegreesOfFreedom; }
/** * Iterates through the given joints and sums their number degrees of freedom. * * @param joints the kinematic chain to compute the total number of degrees of freedom of. * @return the total number of degrees of freedom. */ public static int computeDegreesOfFreedom(List<? extends JointReadOnly> joints) { int numberOfDegreesOfFreedom = 0; for (int i = 0; i < joints.size(); i++) { numberOfDegreesOfFreedom += joints.get(i).getDegreesOfFreedom(); } return numberOfDegreesOfFreedom; }
public int getNumberOfDoFs() { return getJoint().getDegreesOfFreedom(); }
public static void computeIndexForJoint(JointReadOnly[] jointsInOrder, TIntArrayList listToPackIndices, JointReadOnly jointToComputeIndicesFor) { int startIndex = 0; for (int i = 0; i < jointsInOrder.length; i++) { int nDegreesOfFreedom = jointsInOrder[i].getDegreesOfFreedom(); if (jointsInOrder[i] == jointToComputeIndicesFor) { for (int k = startIndex; k < startIndex + nDegreesOfFreedom; k++) { listToPackIndices.add(k); } } startIndex += nDegreesOfFreedom; } }
public static void computeIndexForJoint(List<? extends JointReadOnly> jointsInOrder, TIntArrayList listToPackIndices, JointReadOnly jointToComputeIndicesFor) { int startIndex = 0; for (int i = 0; i < jointsInOrder.size(); i++) { JointReadOnly joint = jointsInOrder.get(i); int nDegreesOfFreedom = joint.getDegreesOfFreedom(); if (joint == jointToComputeIndicesFor) { for (int k = startIndex; k < startIndex + nDegreesOfFreedom; k++) { listToPackIndices.add(k); } } startIndex += nDegreesOfFreedom; } }
/** * 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); }
int[] dofIndices = new int[joint.getDegreesOfFreedom()]; for (int i = 0; i < joint.getDegreesOfFreedom(); i++)
/** * 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; }
/** * Calculates the number of degrees of freedom of the kinematic chain that starts from * {@code ancestor} to end to {@code descendant}. * * @param ancestor the base of the kinematic chain. * @param descendant the end-effector of the kinematic chain. * @return the number of degrees of freedom. * @throws RuntimeException if the given ancestor and descendant are swapped, or if the do not * belong to the same system. * @throws RuntimeException this method does not support in kinematic trees to go through different * branches. */ public static int computeDegreesOfFreedom(RigidBodyReadOnly ancestor, RigidBodyReadOnly descendant) { int nDoFs = 0; RigidBodyReadOnly currentBody = descendant; while (currentBody != ancestor) { JointReadOnly parentJoint = currentBody.getParentJoint(); if (parentJoint == null) throw new RuntimeException("Could not find the ancestor: " + ancestor.getName() + ", to the descendant: " + descendant.getName()); nDoFs += parentJoint.getDegreesOfFreedom(); currentBody = parentJoint.getPredecessor(); } return nDoFs; }
public RecursionStep(RigidBodyReadOnly rigidBody, int[] jointIndices) { this.rigidBody = rigidBody; this.jointIndices = jointIndices; if (isRoot()) { bodyInertia = null; jacobianJointBlock = null; } else { bodyInertia = new SpatialInertia(rigidBody.getInertia()); jacobianJointBlock = new DenseMatrix64F(3, getJoint().getDegreesOfFreedom()); } }
/** * 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); } }
public IterativeStep(RigidBodyReadOnly rigidBody, int[] jointIndices) { this.rigidBody = rigidBody; this.jointIndices = jointIndices; if (isRoot()) { bodyInertia = null; centroidalMomentumMatrixBlock = null; matrixFrameToBodyFixedFrameTransform = null; } else { bodyInertia = new SpatialInertia(rigidBody.getInertia()); centroidalMomentumMatrixBlock = new DenseMatrix64F(6, getJoint().getDegreesOfFreedom()); matrixFrameToBodyFixedFrameTransform = new RigidBodyTransform(); } }
/** * 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); } }
/** * Second pass going from leaves to the root. * <p> * The net wrenches are propagated upstream and summed up at each rigid-body to compute the joint * effort. * </p> */ public void passTwo() { for (int childIndex = 0; childIndex < children.size(); childIndex++) { children.get(childIndex).passTwo(); } if (isRoot()) return; jointWrench.sub(externalWrench); jointWrench.changeFrame(getFrameAfterJoint()); for (int childIndex = 0; childIndex < children.size(); childIndex++) { jointForceFromChild.setIncludingFrame(children.get(childIndex).jointWrench); jointForceFromChild.changeFrame(getFrameAfterJoint()); jointWrench.add(jointForceFromChild); } jointWrench.get(jointWrenchMatrix); CommonOps.multTransA(S, jointWrenchMatrix, tau); for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) { jointTauMatrix.set(jointIndices[dofIndex], 0, tau.get(dofIndex, 0)); } }
/** * 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); } }
public RecursionStep(RigidBodyReadOnly rigidBody, RecursionStep parent, int[] jointIndices) { this.rigidBody = rigidBody; this.parent = parent; this.jointIndices = jointIndices; if (isRoot()) { bodyInertia = null; coriolisBodyAcceleration = new SpatialAcceleration(getBodyFixedFrame(), input.getInertialFrame(), getBodyFixedFrame()); centroidalMomentumMatrixBlock = null; matrixFrameToBodyFixedFrameTransform = null; } else { parent.children.add(this); bodyInertia = new SpatialInertia(rigidBody.getInertia()); coriolisBodyAcceleration = new SpatialAcceleration(); centroidalMomentumMatrixBlock = new DenseMatrix64F(6, getJoint().getDegreesOfFreedom()); matrixFrameToBodyFixedFrameTransform = new RigidBodyTransform(); } }
ya = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, 1); int nDoFs = articulatedBodyRecursionStep.getJoint().getDegreesOfFreedom(); U_Dinv_ST = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, SpatialVectorReadOnly.SIZE); one_minus_U_Dinv_ST = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, SpatialVectorReadOnly.SIZE);
/** * The third and last pass calculates the joint acceleration and body spatial acceleration. */ public void passThree() { if (!isRoot()) { // Computing a'_i = a_{lambda(i)} + c_i parentAcceleration.setIncludingFrame(parent.rigidBodyAcceleration); parentAcceleration.applyInverseTransform(transformToParentJointFrame); parentAcceleration.setReferenceFrame(getFrameAfterJoint()); parentAcceleration.add((SpatialVectorReadOnly) biasAcceleration); parentAcceleration.get(aPrime); // Computing qdd_i = D_i^-1 * ( u_i - U_i^T * a'_i ) CommonOps.multTransA(-1.0, U, aPrime, qdd_intermediate); CommonOps.addEquals(qdd_intermediate, u); CommonOps.mult(Dinv, qdd_intermediate, qdd); // Computing a_i = a'_i + S_i * qdd_i CommonOps.mult(S, qdd, a); CommonOps.addEquals(a, aPrime); rigidBodyAcceleration.setIncludingFrame(getBodyFixedFrame(), parent.getBodyFixedFrame(), getFrameAfterJoint(), a); for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) { jointAccelerationMatrix.set(jointIndices[dofIndex], 0, qdd.get(dofIndex, 0)); } } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).passThree(); }
int nDoFs = getJoint().getDegreesOfFreedom();
int nDoFs = getJoint().getDegreesOfFreedom(); CommonOps.extract(jointAccelerationMatrix, jointIndices, nDoFs, qdd); CommonOps.mult(S, qdd, a);