public MovingReferenceFrame getBodyFixedFrame() { return rigidBody.getBodyFixedFrame(); } }
/** * Returns the body fixed frame of the base {@code RigidBody} of the current Jacobian. The base is * the predecessor of the first joint that the Jacobian considers. * * @return the body fixed frame of the base. */ public ReferenceFrame getBaseFrame() { return base.getBodyFixedFrame(); }
private MovingReferenceFrame getBodyFixedFrame() { return rigidBody.getBodyFixedFrame(); }
public MovingReferenceFrame getBodyFixedFrame() { return rigidBody.getBodyFixedFrame(); }
public MovingReferenceFrame getBodyFixedFrame() { return rigidBody.getBodyFixedFrame(); }
/** * Returns the body fixed frame of the end-effector {@code RigidBody} of the current Jacobian. The * end-effector is the successor of the last joint the Jacobian considers. * * @return the body fixed frame of the end-effector. */ public ReferenceFrame getEndEffectorFrame() { return endEffector.getBodyFixedFrame(); }
/** * Creates a new calculator for the given {@code input}. * * @param input the definition of the system to be evaluated by this calculator. */ public CompositeRigidBodyMassMatrixCalculator(MultiBodySystemReadOnly input) { this(input, input.getRootBody().getBodyFixedFrame()); }
/** * Gets the inertial frame to use with this multi-body system. * <p> * The inertial frame is used as the principal referential when measuring the motion of an object in * space. It is usually set to the root frame of the reference frame tree this system is attached * to. * </p> * * @return the inertial frame. */ default ReferenceFrame getInertialFrame() { return getRootBody().getBodyFixedFrame().getRootFrame(); }
public void includeIgnoredSubtreeInertia() { if (!isRoot() && children.size() != rigidBody.getChildrenJoints().size()) { for (JointReadOnly childJoint : rigidBody.getChildrenJoints()) { if (input.getJointsToIgnore().contains(childJoint)) { SpatialInertia subtreeIneria = MultiBodySystemTools.computeSubtreeInertia(childJoint); subtreeIneria.changeFrame(rigidBody.getBodyFixedFrame()); bodyInertia.add(subtreeIneria); } } } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).includeIgnoredSubtreeInertia(); }
public void includeIgnoredSubtreeInertia() { if (!isRoot() && children.size() != rigidBody.getChildrenJoints().size()) { for (JointReadOnly childJoint : rigidBody.getChildrenJoints()) { if (input.getJointsToIgnore().contains(childJoint)) { SpatialInertia subtreeIneria = MultiBodySystemTools.computeSubtreeInertia(childJoint); subtreeIneria.changeFrame(rigidBody.getBodyFixedFrame()); bodyInertia.add(subtreeIneria); } } } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).includeIgnoredSubtreeInertia(); }
private static RigidBodyTransform getCloneJointTransformToParent(JointReadOnly original) { if (original.getFrameBeforeJoint() == original.getPredecessor().getBodyFixedFrame()) return null; else return original.getFrameBeforeJoint().getTransformToParent(); }
/** * Creates a new spatial acceleration that can be used to describe the gravitational acceleration * that a multi-system is under. * * @param rootBody the very first body of the multi-body system. * @param gravity the magnitude of the gravitational acceleration along the z-axis. * @return the spatial acceleration representing the gravitational acceleration. */ public static SpatialAccelerationReadOnly newGravitationalSpatialAcceleration(RigidBodyReadOnly rootBody, double gravity) { Vector3D gravitationalAcceleration = new Vector3D(0.0, 0.0, gravity); Vector3D zero = new Vector3D(); MovingReferenceFrame bodyFixedFrame = rootBody.getBodyFixedFrame(); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); return new SpatialAcceleration(bodyFixedFrame, worldFrame, bodyFixedFrame, zero, gravitationalAcceleration); }
/** * 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); }
/** * 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 predecessor} * with respect to this joint's {@code successor}. The reference frames of the resulting twist * 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 predecessorTwistToPack the object in which the velocity of this joint's * {@code predecessor} is stored. Modified. */ default void getPredecessorTwist(TwistBasics predecessorTwistToPack) { predecessorTwistToPack.setIncludingFrame(getJointTwist()); ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame(); predecessorTwistToPack.setBaseFrame(predecessorFrame); predecessorTwistToPack.setBodyFrame(successorFrame); predecessorTwistToPack.invert(); predecessorTwistToPack.changeFrame(predecessorFrame); }
/** * 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); }
@Override public SpatialAccelerationReadOnly getRelativeAcceleration(RigidBodyReadOnly base, RigidBodyReadOnly body) { MovingReferenceFrame baseFrame = base.getBodyFixedFrame(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); baseAcceleration.setIncludingFrame(getAccelerationOfBody(base)); acceleration.setIncludingFrame(getAccelerationOfBody(body)); if (areVelocitiesConsidered()) { baseFrame.getTwistRelativeToOther(bodyFrame, deltaTwist); baseAcceleration.changeFrame(bodyFrame, deltaTwist, baseFrame.getTwistOfFrame()); } else { baseAcceleration.changeFrame(bodyFrame); } acceleration.sub(baseAcceleration); return acceleration; }
/** * 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); }
/** * Sums the inertia of all the rigid-bodies composing the subtree that originates at * {@code rootBody} including {@code rootBody}. * <p> * WARNING: This method generates garbage. * </p> * * @param rootBody the root of the subtree. * @return the subtree total inertia. */ public static SpatialInertia computeSubtreeInertia(RigidBodyReadOnly rootBody) { MovingReferenceFrame bodyFixedFrame = rootBody.getBodyFixedFrame(); SpatialInertia subtreeInertia = new SpatialInertia(bodyFixedFrame, bodyFixedFrame); SpatialInertia bodyInertia = new SpatialInertia(); for (RigidBodyReadOnly subtreeBody : rootBody.subtreeList()) { bodyInertia.setIncludingFrame(subtreeBody.getInertia()); bodyInertia.changeFrame(bodyFixedFrame); subtreeInertia.add(bodyInertia); } return subtreeInertia; }
@Override public FrameVector3DReadOnly getLinearAccelerationOfBodyFixedPoint(RigidBodyReadOnly base, RigidBodyReadOnly body, FramePoint3DReadOnly bodyFixedPoint) { SpatialAccelerationReadOnly accelerationToUse = base != null ? getRelativeAcceleration(base, body) : getAccelerationOfBody(body); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); ReferenceFrame baseFrame = accelerationToUse.getBaseFrame(); localBodyFixedPoint.setIncludingFrame(bodyFixedPoint); localBodyFixedPoint.changeFrame(bodyFrame); if (areVelocitiesConsidered()) bodyFrame.getTwistRelativeToOther(baseFrame, twistForLinearAcceleration); else twistForLinearAcceleration.setToZero(bodyFrame, baseFrame, bodyFrame); accelerationToUse.getLinearAccelerationAt(twistForLinearAcceleration, localBodyFixedPoint, linearAcceleration); linearAcceleration.changeFrame(baseFrame); return linearAcceleration; }