@Override public void getJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(jointAcceleration); }
private void computeSuccessorAcceleration(InverseDynamicsJoint joint) { RigidBody predecessor = joint.getPredecessor(); RigidBody successor = joint.getSuccessor(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); joint.getPredecessorTwist(tempJointTwist); if (!doVelocityTerms) tempJointTwist.setToZero(); if (useDesireds) joint.getDesiredSuccessorAcceleration(tempJointAcceleration); else joint.getSuccessorAcceleration(tempJointAcceleration); if (!doAccelerationTerms) tempJointAcceleration.setToZero(); twistCalculator.getTwistOfBody(tempTwistFromWorld, predecessor); if (!doVelocityTerms) tempTwistFromWorld.setToZero(); SpatialAccelerationVector successorAcceleration = accelerations.get(successor); successorAcceleration.set(accelerations.get(predecessor)); successorAcceleration.changeFrame(successorFrame, tempJointTwist, tempTwistFromWorld); successorAcceleration.add(tempJointAcceleration); }
@Override public void getDesiredJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(jointAccelerationDesired); }
public void getAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(acceleration); } }
public void getUnitJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitJointAcceleration); }
public void setSpatialAcceleration(SpatialAccelerationVector spatialAcceleration) { this.spatialAcceleration.set(spatialAcceleration); }
/** * Packs the spatial acceleration of the given {@code rigidBody}. * The resulting spatial acceleration is the acceleration of the {@code rigidBody.getBodyFixedFrame()} * with respect to {@code inertialFrame}, expressed in {@code rigidBody.getBodyFixedFrame()}. * * @param spatialAccelerationToPack the spatial acceleration of the {@code rigidBody} to pack. Modified. * @param rigidBody the rigid-body to get the acceleration of. */ // FIXME Change the method signature to have spatialAccelerationToPack as the last argument. public void getAccelerationOfBody(SpatialAccelerationVector spatialAccelerationToPack, RigidBody rigidBody) { spatialAccelerationToPack.set(accelerations.get(rigidBody)); }
@Override public void getDesiredSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitSuccessorAcceleration); accelerationToPack.scale(qddDesired); }
@Override public void getJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitJointAcceleration); accelerationToPack.scale(qdd); }
@Override public void getDesiredJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitJointAcceleration); accelerationToPack.scale(qddDesired); }
@Override public void getDesiredPredecessorAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitPredecessorAcceleration); accelerationToPack.scale(qddDesired); }
@Override public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitSuccessorAcceleration); accelerationToPack.scale(qdd); }
/** * Updates the internal memory to the new system state (configuration, velocity, and acceleration). * This method has to be called once every time the system state has changed, and before calling * the methods {@link #getAccelerationOfBody(SpatialAccelerationVector, RigidBody)}, * {@link #getRelativeAcceleration(SpatialAccelerationVector, RigidBody, RigidBody)}, * {@link #getLinearAccelerationOfBodyFixedPoint(FrameVector, RigidBody, FramePoint)}, * or {@link #getLinearAccelerationOfBodyFixedPoint(FrameVector, RigidBody, RigidBody, FramePoint)}. * <p> * Starting from the root which has a known acceleration {@code rootAcceleration}, * this method updates the spatial accelerations by using the relation: * <br> A<sup>s, s</sup><sub>i</sub> = A<sup>p, s</sup><sub>i</sub> + A<sup>s, s</sup><sub>p</sub> </br> * where 's' is the {@code successorFrame}, 'p' the {@code predecessorFrame}, and * 'i' the {@code inertialFrame}. * </p> */ public void compute() { accelerations.get(rootBody).set(rootAcceleration); for (int jointIndex = 0; jointIndex < allJoints.size(); jointIndex++) { InverseDynamicsJoint joint = allJoints.get(jointIndex); computeSuccessorAcceleration(joint); } }
/** * Copy constructor */ public SpatialAccelerationVector(SpatialAccelerationVector other) { this.angularPart = new Vector3d(); this.linearPart = new Vector3d(); set(other); }
@Override public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart) { jointAccelerationDesired.set(jointAccelerationDesired.getBodyFrame(), jointAccelerationDesired.getBaseFrame(), jointAccelerationDesired.getExpressedInFrame(), matrix, rowStart); }
/** * Sets this spatial acceleration vector so that it is the same as another spatial acceleration vector */ public void checkAndSet(SpatialAccelerationVector other) { this.bodyFrame.checkReferenceFrameMatch(other.bodyFrame); this.baseFrame.checkReferenceFrameMatch(other.baseFrame); this.expressedInFrame.checkReferenceFrameMatch(other.expressedInFrame); set(other); }
/** * Changes the spatial acceleration of the root. * Even though the root is assumed to be non-moving, the {@code rootAcceleration} is usually * set to the opposite of the gravitational acceleration, such the effect of the gravity is * naturally propagated to the entire system. * * @param newRootAcceleration the new spatial acceleration of the root. * @throws ReferenceFrameMismatchException if any of the reference frames of {@code newRootAcceleration} does * not match this calculator's root spatial acceleration's frames. */ public void setRootAcceleration(SpatialAccelerationVector newRootAcceleration) { rootAcceleration.checkReferenceFramesMatch(newRootAcceleration.getBodyFrame(), newRootAcceleration.getBaseFrame(), newRootAcceleration.getExpressedInFrame()); this.rootAcceleration.set(newRootAcceleration); }
@Override public void set(SpatialAccelerationCommand other) { hasWeight = other.hasWeight; setWeights(other.getWeightVector()); alphaTaskPriority = other.alphaTaskPriority; spatialAcceleration.set(other.getSpatialAcceleration()); selectionMatrix.set(other.getSelectionMatrix()); base = other.getBase(); endEffector = other.getEndEffector(); baseName = other.baseName; endEffectorName = other.endEffectorName; optionalPrimaryBase = other.optionalPrimaryBase; optionalPrimaryBaseName = other.optionalPrimaryBaseName; }
private void computeRootJointAcceleration(FloatingInverseDynamicsJoint rootJoint, SpatialAccelerationVector rootJointAcceleration, FrameVector rootJointAngularAcceleration, FrameVector rootJointLinearAcceleration) { rootJointAngularAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointAcceleration.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearAcceleration.getVector(), rootJointAngularAcceleration.getVector()); }
private void transformDesiredsFromSoleFrameToControlFrame() { desiredSoleFrame.setPoseAndUpdate(desiredPosition, desiredOrientation); // change pose desiredPose.setToZero(desiredControlFrame); desiredPose.changeFrame(worldFrame); desiredPose.getPosition(desiredPosition.getPoint()); desiredPose.getOrientation(desiredOrientation.getQuaternion()); // change twist desiredLinearVelocity.changeFrame(desiredSoleFrame); desiredAngularVelocity.changeFrame(desiredSoleFrame); desiredTwist.set(desiredSoleFrame, worldFrame, desiredSoleFrame, desiredLinearVelocity, desiredAngularVelocity); desiredTwist.changeFrame(desiredControlFrame); desiredTwist.getLinearPart(desiredLinearVelocity); desiredTwist.getAngularPart(desiredAngularVelocity); desiredLinearVelocity.changeFrame(worldFrame); desiredAngularVelocity.changeFrame(worldFrame); // change spatial acceleration desiredLinearAcceleration.changeFrame(desiredSoleFrame); desiredAngularAcceleration.changeFrame(desiredSoleFrame); desiredSpatialAcceleration.set(desiredSoleFrame, worldFrame, desiredSoleFrame, desiredLinearAcceleration, desiredAngularAcceleration); desiredSpatialAcceleration.changeFrameNoRelativeMotion(desiredControlFrame); desiredSpatialAcceleration.getLinearPart(desiredLinearAcceleration); desiredSpatialAcceleration.getAngularPart(desiredAngularAcceleration); desiredLinearAcceleration.changeFrame(worldFrame); desiredAngularAcceleration.changeFrame(worldFrame); }