private void updateRootJointTwistLinearPart(FrameVector centerOfMassVelocityWorld, FloatingInverseDynamicsJoint rootJoint) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); computeRootJointLinearVelocity(centerOfMassVelocityWorld, tempRootJointLinearVelocity, tempRootJointAngularVelocity, rootJoint); computeRootJointTwistLinearPart(rootJoint, tempRootJointTwist, tempRootJointLinearVelocity); rootJoint.setJointTwist(tempRootJointTwist); }
private void updateRootJointTwistAngularPart(TwistCalculator twistCalculator, FloatingInverseDynamicsJoint rootJoint, FrameVector estimationLinkAngularVelocity) { rootJoint.getJointTwist(tempRootJointTwist); computeRootJointAngularVelocity(twistCalculator, tempRootJointAngularVelocity, estimationLinkAngularVelocity); tempRootJointTwist.setAngularPart(tempRootJointAngularVelocity.getVector()); rootJoint.setJointTwist(tempRootJointTwist); }
@Override public void updateRootJointOrientationAndAngularVelocity() { yoRootJointFrameOrientation.getQuaternion(rootJointOrientation); rootJoint.setRotation(rootJointOrientation); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
public static void setRandomVelocity(FloatingInverseDynamicsJoint rootJoint, Random random) { Twist jointTwist = new Twist(); rootJoint.getJointTwist(jointTwist); jointTwist.setAngularPart(RandomTools.generateRandomVector(random)); jointTwist.setLinearPart(RandomTools.generateRandomVector(random)); rootJoint.setJointTwist(jointTwist); }
private void updateKinematicsNewTwist() { rootJoint.getJointTwist(tempRootBodyTwist); rootJointLinearVelocityNewTwist.getFrameTupleIncludingFrame(tempFrameVector); tempFrameVector.changeFrame(tempRootBodyTwist.getExpressedInFrame()); tempRootBodyTwist.setLinearPart(tempFrameVector); rootJoint.setJointTwist(tempRootBodyTwist); twistCalculator.compute(); for (int i = 0; i < feetRigidBodies.size(); i++) { RigidBody foot = feetRigidBodies.get(i); Twist footTwistInWorld = footTwistsInWorld.get(foot); YoFrameVector footVelocityInWorld = footVelocitiesInWorld.get(foot); twistCalculator.getTwistOfBody(footTwistInWorld, foot); footTwistInWorld.changeBodyFrameNoRelativeTwist(soleFrames.get(foot)); footTwistInWorld.changeFrame(soleFrames.get(foot)); this.copsFilteredInFootFrame.get(foot).getFrameTuple2dIncludingFrame(tempCoP2d); tempCoP.setXYIncludingFrame(tempCoP2d); footTwistInWorld.changeFrame(footTwistInWorld.getBaseFrame()); tempCoP.changeFrame(footTwistInWorld.getExpressedInFrame()); footTwistInWorld.getLinearVelocityOfPointFixedInBodyFrame(tempFrameVector, tempCoP); tempFrameVector.changeFrame(worldFrame); footVelocityInWorld.set(tempFrameVector); } }
private void updateRootJointTwistAndSpatialAcceleration(TwistCalculator twistCalculator, SpatialAccelerationCalculator spatialAccelerationCalculator) { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); computeRootJointAngularVelocityAndAcceleration(twistCalculator, spatialAccelerationCalculator, tempRootJointAngularVelocity, tempRootJointAngularAcceleration); computeRootJointLinearVelocityAndAcceleration(tempRootJointLinearVelocity, tempRootJointLinearAcceleration, tempRootJointAngularVelocity, tempRootJointAngularAcceleration); computeRootJointTwist(rootJoint, tempRootJointTwist, tempRootJointAngularVelocity, tempRootJointLinearVelocity); rootJoint.setJointTwist(tempRootJointTwist); computeRootJointAcceleration(rootJoint, tempRootJointAcceleration, tempRootJointAngularAcceleration, tempRootJointLinearAcceleration); rootJoint.setAcceleration(tempRootJointAcceleration); }
public void updateForFrozenState() { // Keep setting the position so the localization updater works properly. yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); // Set the rootJoint twist to zero. rootJoint.getJointTwist(rootJointTwist); rootJointTwist.setToZero(); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); updateCoMState(); }
private void updateRootJoint() { yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); tempVelocity.setIncludingFrame(rootJointVelocity); rootJoint.getJointTwist(rootJointTwist); tempVelocity.changeFrame(rootJointTwist.getExpressedInFrame()); rootJointTwist.setLinearPart(tempVelocity); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); }
private void readAndUpdateRootJointAngularAndLinearVelocity() { ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = rootJoint.getFrameAfterJoint(); FrameVector linearVelocity = robot.getRootJointVelocity(); linearVelocity.changeFrame(pelvisFrame); FrameVector angularVelocity = robot.getRootJointAngularVelocityInRootJointFrame(pelvisFrame); angularVelocity.changeFrame(pelvisFrame); Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, linearVelocity.getVector(), angularVelocity.getVector()); rootJoint.setJointTwist(bodyTwist); }
@Override public void updateForFrozenState() { // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); yoRootJointFrameQuaternion.getYawPitchRoll(lastComputedYawPitchRoll); double currentYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); double yawDifference = AngleTools.computeAngleDifferenceMinusPiToPi(lastComputedYawPitchRoll[0], currentYaw); rootJointYawOffsetFromFrozenState.set(yawDifference); rotationFrozenOffset.rotZ(yawDifference); // Keep setting the orientation so that the localization updater works properly. yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); updateViz(); }
@Override public void initializeForFrozenState() { rotationFrozenOffset.setIdentity(); // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); double initialYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); rootJointYawOffsetFromFrozenState.set(initialYaw); rotationFrozenOffset.rotZ(initialYaw); yoRootJointFrameQuaternion.setToZero(); yoRootJointFrameOrientation.setToZero(); yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
private void updateRootJointTwistAngularPart() { // T_{rootBody}^{rootBody, measurementLink} twistCalculator.getRelativeTwist(twistRootJointFrameRelativeToMeasurementLink, measurementLink, rootJoint.getSuccessor()); // T_{rootBody}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeFrame(rootJointFrame); // T_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeBodyFrameNoRelativeTwist(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.getAngularPart(angularVelocityRootJointFrameRelativeToMeasurementLink); // omega_{measurementLink}^{measurementFrame, world} imuProcessedOutput.getAngularVelocityMeasurement(angularVelocityMeasurement); if (imuBiasProvider != null) { imuBiasProvider.getAngularVelocityBiasInIMUFrame(imuProcessedOutput, angularVelocityMeasurementBias); angularVelocityMeasurement.sub(angularVelocityMeasurementBias); } angularVelocityMeasurementLinkRelativeToWorld.setIncludingFrame(measurementFrame, angularVelocityMeasurement); // omega_{measurementLink}^{rootJointFrame, world} angularVelocityMeasurementLinkRelativeToWorld.changeFrame(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, world} = omega_{rootJointFrame}^{rootJointFrame, measurementLink} + omega_{measurementLink}^{rootJointFrame, world} angularVelocityRootJointFrameRelativeToWorld.add(angularVelocityRootJointFrameRelativeToMeasurementLink, angularVelocityMeasurementLinkRelativeToWorld); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setAngularPart(angularVelocityRootJointFrameRelativeToWorld); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); twistCalculator.compute(); yoRootJointAngularVelocity.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityMeasFrame.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityInWorld.setAndMatchFrame(angularVelocityRootJointFrameRelativeToWorld); }
sixDoFJoint.setJointTwist(bodyTwist);