MultiBodySystemTools.extractJointsState(rootJacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); randomEndEffector.getBodyFixedFrame().getTwistRelativeToOther(rootBody.getBodyFixedFrame(), expectedTwist); rootJacobian.getTwist(jointVelocitiesMatrix, actualTwist); MultiBodySystemTools.extractJointsState(jacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); randomEndEffector.getBodyFixedFrame().getTwistRelativeToOther(randomBase.getBodyFixedFrame(), expectedTwist); jacobian.getTwist(jointVelocitiesMatrix, actualTwist);
predecessorBodyFrame.getTwistRelativeToOther(baseFrame, bodyTwistRelativeToBase);
@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; }
MultiBodySystemTools.extractJointsState(rootJacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); randomEndEffector.getBodyFixedFrame().getTwistRelativeToOther(rootBody.getBodyFixedFrame(), expectedTwist); rootJacobian.getTwist(jointVelocitiesMatrix, actualTwist); MultiBodySystemTools.extractJointsState(jacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); randomEndEffector.getBodyFixedFrame().getTwistRelativeToOther(randomBase.getBodyFixedFrame(), expectedTwist); jacobian.getTwist(jointVelocitiesMatrix, actualTwist);
@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) { twistRelativeToParentToPack.setToZero(this, getParent(), this); TwistReadOnly twistOfMidFootFrame = midFootZUpGroundFrame.getTwistOfFrame(); angularVelocity.setIncludingFrame(twistOfMidFootFrame.getAngularPart()); linearVelocity.setIncludingFrame(twistOfMidFootFrame.getLinearPart()); pelvisFrame.getTwistRelativeToOther(midFootZUpGroundFrame, twistOfPelvisRelativeToMidFootFrame); linearPelvisVelocity.setIncludingFrame(twistOfPelvisRelativeToMidFootFrame.getLinearPart()); linearPelvisVelocity.changeFrame(this); angularVelocity.changeFrame(this); linearVelocity.changeFrame(this); linearVelocity.add(linearPelvisVelocity.getX(), 0.0, 0.0); offset.set(pelvisPosition.getX(), 0.0, 0.0); linearVelocityOffset.cross(angularVelocity, offset); linearVelocity.add(linearVelocityOffset); twistRelativeToParentToPack.getAngularPart().set(angularVelocity); twistRelativeToParentToPack.getLinearPart().set(linearVelocity); } }