/** * 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; }
public MovingReferenceFrame getFrameAfterJoint() { return getJoint().getFrameAfterJoint(); }
private static RigidBodyTransform getCloneJointTransformToParent(JointReadOnly original) { if (original.getFrameBeforeJoint() == original.getPredecessor().getBodyFixedFrame()) return null; else return original.getFrameBeforeJoint().getTransformToParent(); }
/** * 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); }
/** * Creates the reference frame that is at the joint and rigidly attached to the joint's successor. * * @param joint the joint to which the new frame is for. Not modified. * @return the new frame after joint. */ public static MovingReferenceFrame newFrameAfterJoint(JointReadOnly joint) { MovingReferenceFrame frameBeforeJoint = joint.getFrameBeforeJoint(); if (frameBeforeJoint == null) throw new NullPointerException("The frameBeforeJoint has to be created before the frameAfterJoint."); return new MovingReferenceFrame("after" + MecanoTools.capitalize(joint.getName()), frameBeforeJoint) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { joint.getJointConfiguration(transformToParent); } @Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { twistRelativeToParentToPack.setIncludingFrame(joint.getJointTwist()); } }; }
RigidBodyReadOnly originalAncestor = originalJoints[0].getPredecessor(); RigidBodyBasics cloneAncestor; if (originalAncestor.isRootBody()) cloneAncestor = new RigidBody(originalAncestor.getName() + cloneSuffix, chainRootFrame); else cloneAncestor = new RigidBody(originalAncestor.getName() + cloneSuffix, originalAncestor.getParentJoint().getFrameAfterJoint()); RigidBodyReadOnly originalPredecessor = originalJoint.getPredecessor(); RigidBodyReadOnly originalSuccessor = originalJoint.getSuccessor(); RigidBodyBasics cloneSuccessor = cloneRigidBody(originalSuccessor, null, cloneSuffix, cloneJoint, null); originalToCloneBodyMap.put(originalSuccessor, cloneSuccessor);
/** * Packs the spatial acceleration (the 3D angular and linear accelerations) of this joint's * {@code successor} with respect to this joint's {@code predecessor}. The reference frames of * the resulting spatial acceleration are as follows: * <ul> * <li>{@code bodyFrame} is {@code successorFrame = successor.getBodyFixedFrame()}. * <li>{@code baseFrame} is {@code predecessorFrame = predecessor.getBodyFixedFrame()}. * <li>{@code expressedInFrame} is {@code successorFrame}. * </ul> * * @param successorAccelerationToPack the object in which the acceleration of this joint's * {@code successor} is stored. Modified. */ default void getSuccessorAcceleration(SpatialAccelerationBasics successorAccelerationToPack) { successorAccelerationToPack.setIncludingFrame(getJointAcceleration()); ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame(); successorAccelerationToPack.setBaseFrame(predecessorFrame); successorAccelerationToPack.setBodyFrame(successorFrame); successorAccelerationToPack.changeFrame(successorFrame); }
/** * Packs the twist (the 3D angular and linear velocities) of this joint's {@code successor} with * respect to this joint's {@code predecessor}. The reference frames of the resulting twist are * as follows: * <ul> * <li>{@code bodyFrame} is {@code successorFrame = successor.getBodyFixedFrame()}. * <li>{@code baseFrame} is {@code predecessorFrame = predecessor.getBodyFixedFrame()}. * <li>{@code expressedInFrame} is {@code successorFrame}. * </ul> * * @param successorTwistToPack the object in which the velocity of this joint's {@code successor} * is stored. Modified. */ default void getSuccessorTwist(TwistBasics successorTwistToPack) { successorTwistToPack.setIncludingFrame(getJointTwist()); ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame(); successorTwistToPack.setBaseFrame(predecessorFrame); successorTwistToPack.setBodyFrame(successorFrame); successorTwistToPack.changeFrame(successorFrame); }
int nDoFs = getJoint().getDegreesOfFreedom(); tau = new DenseMatrix64F(nDoFs, 1); jointWrenchMatrix = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, 1); getJoint().getMotionSubspace(S);
if (robotState.isFloating()) ReferenceFrame rootFrame = robotJacobian.getJointsFromBaseToEndEffector().get(0).getFrameAfterJoint(); ReferenceFrame baseFrame = robotJacobian.getJointsFromBaseToEndEffector().get(0).getFrameBeforeJoint(); rootFrame.getTransformToDesiredFrame(rootToMeasurement, measurementFrame); baseFrame.getTransformToDesiredFrame(rootTransform, rootFrame);
int[] dofIndices = new int[joint.getDegreesOfFreedom()]; for (int i = 0; i < joint.getDegreesOfFreedom(); i++) int[] configurationIndices = new int[joint.getConfigurationMatrixSize()]; for (int i = 0; i < joint.getConfigurationMatrixSize(); i++)
/** * 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; }
/** * Creates the reference frame that is at the joint and rigidly attached to the joint's predecessor. * * @param joint the joint to which the new frame is for. Not modified. * @param transformToParent the transform from the new frame to the frame after the parent joint. * Not modified. * @param beforeJointName the name of the new frame. * @return the new frame before joint. */ public static MovingReferenceFrame newJointFrame(JointReadOnly joint, RigidBodyTransform transformToParent, String beforeJointName) { MovingReferenceFrame parentFrame; RigidBodyReadOnly parentBody = joint.getPredecessor(); if (parentBody.isRootBody()) { parentFrame = parentBody.getBodyFixedFrame(); /* * TODO Special case to keep the beforeJointFrame of the SixDoFJoint to be the elevatorFrame. This * should probably removed, might cause reference frame exceptions though. */ if (transformToParent == null) return parentFrame; } else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); return MovingReferenceFrame.constructFrameFixedInParent(beforeJointName, parentFrame, transformToParent); }
/** * The first pass consists in calculating the bias wrench resulting from external and Coriolis * forces, and the bias acceleration resulting from the Coriolis acceleration. */ public void passOne() { if (!isRoot()) { MovingReferenceFrame frameAfterJoint = getFrameAfterJoint(); MovingReferenceFrame frameBeforeJoint = getJoint().getFrameBeforeJoint(); if (parent.isRoot()) frameAfterJoint.getTransformToDesiredFrame(transformToParentJointFrame, parent.getBodyFixedFrame()); else frameAfterJoint.getTransformToDesiredFrame(transformToParentJointFrame, parent.getFrameAfterJoint()); bodyInertia.computeDynamicWrench(null, getBodyTwist(), biasWrench); biasWrench.sub(externalWrench); biasWrench.changeFrame(frameAfterJoint); biasAcceleration.setToZero(frameAfterJoint, input.getInertialFrame(), frameBeforeJoint); biasAcceleration.changeFrame(frameAfterJoint, getJoint().getJointTwist(), frameAfterJoint.getTwistOfFrame()); biasAcceleration.get(c); } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).passOne(); }
getJoint().getPredecessorTwist(localJointTwist); rigidBodyAcceleration.changeFrame(getBodyFixedFrame(), localJointTwist, parent.getBodyFixedFrame().getTwistOfFrame()); bodyTwistToUse = getBodyTwist(); int nDoFs = getJoint().getDegreesOfFreedom(); CommonOps.extract(jointAccelerationMatrix, jointIndices, nDoFs, qdd); CommonOps.mult(S, qdd, a);
/** * Packs the offset from the frame before this joint to the frame after this parent joint. * * @param jointOffsetTransformToPack the transform in which this joint's offset is stored. * Modified. */ default void getJointOffset(RigidBodyTransform jointOffsetTransformToPack) { getFrameBeforeJoint().getTransformToParent(jointOffsetTransformToPack); }
private static int extractJointsConfiguration(JointReadOnly[] joints, int startIndex, DenseMatrix64F matrixToPack) { for (int jointIndex = 0; jointIndex < joints.length; jointIndex++) { JointReadOnly joint = joints[jointIndex]; startIndex = joint.getJointConfiguration(startIndex, matrixToPack); } return startIndex; }
private static int extractJointsVelocity(JointReadOnly[] joints, int startIndex, DenseMatrix64F matrixToPack) { for (int jointIndex = 0; jointIndex < joints.length; jointIndex++) { JointReadOnly joint = joints[jointIndex]; startIndex = joint.getJointVelocity(startIndex, matrixToPack); } return startIndex; }
private static int extractJointsAcceleration(JointReadOnly[] joints, int startIndex, DenseMatrix64F matrixToPack) { for (int jointIndex = 0; jointIndex < joints.length; jointIndex++) { JointReadOnly joint = joints[jointIndex]; startIndex = joint.getJointAcceleration(startIndex, matrixToPack); } return startIndex; }
private static int extractJointsTau(JointReadOnly[] joints, int startIndex, DenseMatrix64F matrixToPack) { for (int jointIndex = 0; jointIndex < joints.length; jointIndex++) { JointReadOnly joint = joints[jointIndex]; startIndex = joint.getJointTau(startIndex, matrixToPack); } return startIndex; }