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 extractJointsAcceleration(List<? extends JointReadOnly> joints, int startIndex, DenseMatrix64F matrixToPack) { for (int jointIndex = 0; jointIndex < joints.size(); jointIndex++) { JointReadOnly joint = joints.get(jointIndex); startIndex = joint.getJointAcceleration(startIndex, matrixToPack); } return startIndex; }
/** * 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 spatial acceleration (the 3D angular and linear accelerations) of this joint's * {@code predecessor} with respect to this joint's {@code successor} resulting from the desired * acceleration of this joint. The reference frames of the resulting spatial acceleration are as * follows: * <ul> * <li>{@code bodyFrame} is {@code predecessorFrame = predecessor.getBodyFixedFrame()}. * <li>{@code baseFrame} is {@code successorFrame = successor.getBodyFixedFrame()}. * <li>{@code expressedInFrame} is {@code predecessorFrame}. * </ul> * * @param predecessorAccelerationToPack the object in which the acceleration of this joint's * {@code predecessor} resulting from this joint desired acceleration is stored. * Modified. */ default void getPredecessorAcceleration(SpatialAccelerationBasics predecessorAccelerationToPack) { predecessorAccelerationToPack.setIncludingFrame(getJointAcceleration()); ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame(); predecessorAccelerationToPack.setBaseFrame(predecessorFrame); predecessorAccelerationToPack.setBodyFrame(successorFrame); predecessorAccelerationToPack.invert(); predecessorAccelerationToPack.changeFrame(predecessorFrame); }