public TwistReadOnly getBodyTwist() { return getBodyFixedFrame().getTwistOfFrame(); }
/** * Packs the twist of this frame with respect the closest stationary frame, i.e. not moving with * respect to its root frame. * <p> * The returned twist the twist of {@code this} with respect to (usually) * {@link ReferenceFrame#getWorldFrame()} and expressed in {@code this}. * </p> * * @param twistToPack the twist in which the absolute velocity of this frame is stored. Modified. */ public void getTwistOfFrame(Twist twistToPack) { twistToPack.setIncludingFrame(getTwistOfFrame()); }
public TwistReadOnly getBodyTwist() { return getBodyFixedFrame().getTwistOfFrame(); }
public void get(FrameVector3D frameVectorToPack) { referenceFrame.getTwistOfFrame(twist); frameVectorToPack.setIncludingFrame(twist.getAngularPart()); } }
public void get(FrameVector3D frameVectorToPack) { referenceFrame.getTwistOfFrame(twist); frameVectorToPack.setIncludingFrame(twist.getLinearPart()); } }
public void getLinearVelocity(FrameVector3D linearVelocityToPack) { frameOfInterest.getTwistOfFrame(twist); linearVelocityToPack.setIncludingFrame(twist.getLinearPart()); }
public void getAngularVelocity(FrameVector3D angularVelocityToPack) { frameOfInterest.getTwistOfFrame(twist); angularVelocityToPack.setIncludingFrame(twist.getAngularPart()); } }
private void computeNetWrenches() { for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) { RigidBodyBasics body = allBodiesExceptRoot.get(bodyIndex); Wrench netWrench = netWrenches.get(body); body.getBodyFixedFrame().getTwistOfFrame(tempTwist); if (!doVelocityTerms) tempTwist.setToZero(); tempAcceleration.setIncludingFrame(spatialAccelerationCalculator.getAccelerationOfBody(body)); body.getInertia().computeDynamicWrenchFast(tempAcceleration, tempTwist, netWrench); } }
getTwistOfFrame(relativeTwistToPack); relativeTwistToPack.setBaseFrame(base); ((MovingReferenceFrame) base).getTwistOfFrame(relativeTwistToPack); relativeTwistToPack.changeFrame(this); relativeTwistToPack.sub(getTwistOfFrame()); relativeTwistToPack.invert();
private static void assertFrameTwistEquals(MovingReferenceFrame expected, MovingReferenceFrame actual, double epsilon) { EuclidCoreTestTools.assertRigidBodyTransformEquals(expected.getTransformToRoot(), actual.getTransformToRoot(), 1.0e-12); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); expected.getTwistOfFrame(expectedTwist); actual.getTwistOfFrame(actualTwist); expectedTwist.setBodyFrame(actual); expectedTwist.changeFrame(actual); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, epsilon); }
public void computeCentroidalConvectiveTerm() { if (!isRoot()) { SpatialInertiaReadOnly inertia = bodyInertia; coriolisBodyAcceleration.setIncludingFrame(parent.coriolisBodyAcceleration); getJoint().getPredecessorTwist(intermediateTwist); coriolisBodyAcceleration.changeFrame(getBodyFixedFrame(), intermediateTwist, parent.getBodyFixedFrame().getTwistOfFrame()); coriolisBodyAcceleration.setBodyFrame(getBodyFixedFrame()); inertia.computeDynamicWrench(coriolisBodyAcceleration, getBodyFixedFrame().getTwistOfFrame(), netCoriolisBodyWrench); netCoriolisBodyWrench.changeFrame(centroidalMomentumFrame); centroidalConvectiveTerm.add(netCoriolisBodyWrench); } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).computeCentroidalConvectiveTerm(); }
private void updateTwistOfFrame() { if (isTwistOfFrameUpToDateRecursive()) return; if (parentMovingFrame == null) { if (isFixedInParent) twistOfFrame.setToZero(this, getParent(), this); else twistOfFrame.setIncludingFrame(twistRelativeToParent); } else { twistOfFrame.setIncludingFrame(parentMovingFrame.getTwistOfFrame()); twistOfFrame.changeFrame(this); if (isFixedInParent) twistOfFrame.setBodyFrame(this); else twistOfFrame.add(twistRelativeToParent); } isTwistOfFrameUpToDate = true; for (int i = 0; i < childrenMovingFrames.size(); i++) childrenMovingFrames.get(i).isTwistOfFrameUpToDate = false; }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithPrismaticChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<PrismaticJoint> joints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstTwistCalculatorWithChainRobot() throws Exception { Random random = new Random(435423L); int numberOfJoints = 100; List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointChain(random, numberOfJoints); TwistCalculator twistCalculator = new TwistCalculator(worldFrame, joints.get(0).getPredecessor()); Twist actualTwist = new Twist(); Twist expectedTwist = new Twist(); for (int i = 0; i < 100; i++) { MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -1.0, 1.0, joints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -1.0, 1.0, joints); joints.get(0).getPredecessor().updateFramesRecursively(); twistCalculator.compute(); for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { OneDoFJointBasics joint = joints.get(jointIndex); RigidBodyBasics body = joint.getSuccessor(); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); twistCalculator.getTwistOfBody(body, expectedTwist); bodyFrame.getTwistOfFrame(actualTwist); MecanoTestTools.assertTwistEquals(expectedTwist, actualTwist, 1.0e-5); } } }
/** * First pass going from the root to the leaves. * <p> * Here the rigid-body Coriolis and centrifugal accelerations are updated to calculate the system's * bias force.. * </p> */ public void passOne() { if (isRoot()) return; ReferenceFrame inertiaFrame = bodyInertia.getReferenceFrame(); matrixFrame.getTransformToDesiredFrame(matrixFrameToBodyFixedFrameTransform, inertiaFrame); coriolisBodyAcceleration.setIncludingFrame(parent.coriolisBodyAcceleration); getJoint().getPredecessorTwist(intermediateTwist); coriolisBodyAcceleration.changeFrame(getBodyFixedFrame(), intermediateTwist, parent.getBodyFixedFrame().getTwistOfFrame()); coriolisBodyAcceleration.setBodyFrame(getBodyFixedFrame()); bodyInertia.computeDynamicWrench(coriolisBodyAcceleration, getBodyFixedFrame().getTwistOfFrame(), netCoriolisBodyWrench); netCoriolisBodyWrench.applyInverseTransform(matrixFrameToBodyFixedFrameTransform); netCoriolisBodyWrench.setReferenceFrame(matrixFrame); biasSpatialForce.add(netCoriolisBodyWrench); }
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { twistRelativeToParentToPack.setToZero(this, getParent(), this); TwistReadOnly twistOfFrameOne = frameOne.getTwistOfFrame(); TwistReadOnly twistOfFrameTwo = frameTwo.getTwistOfFrame(); linearVelocityOne.setIncludingFrame(twistOfFrameOne.getLinearPart()); linearVelocityTwo.setIncludingFrame(twistOfFrameTwo.getLinearPart()); linearVelocityOne.changeFrame(this); linearVelocityTwo.changeFrame(this); linearVelocity.setToZero(this); linearVelocity.interpolate(linearVelocityOne, linearVelocityTwo, 0.5); twistRelativeToParentToPack.getLinearPart().set(linearVelocity); angularVelocityOne.setIncludingFrame(twistOfFrameOne.getAngularPart()); angularVelocityTwo.setIncludingFrame(twistOfFrameTwo.getAngularPart()); computeAngularVelocityNumeric(); twistRelativeToParentToPack.getAngularPart().set(angularVelocity); }
public void computeAndPack(Momentum momentum) { momentum.getAngularPart().set(zero); momentum.getLinearPart().set(zero); for (RigidBodyBasics rigidBody : rigidBodiesInOrders) { SpatialInertiaBasics inertia = rigidBody.getInertia(); rigidBody.getBodyFixedFrame().getTwistOfFrame(tempTwist); tempMomentum.setReferenceFrame(inertia.getReferenceFrame()); tempMomentum.compute(inertia, tempTwist); tempMomentum.changeFrame(momentum.getReferenceFrame()); momentum.add(tempMomentum); } } }
@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; }
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) TwistReadOnly twistOfFrameOne = frameOne.getTwistOfFrame(); TwistReadOnly twistOfFrameTwo = frameTwo.getTwistOfFrame();
/** * 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(); }