private void computeRootJointTwist(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointAngularVelocity, FrameVector rootJointLinearVelocity) { rootJointAngularVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), rootJointAngularVelocity.getVector()); }
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 computeRootJointTwistLinearPart(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointLinearVelocity) { rootJoint.getJointTwist(tempRootJointTwistExisting); tempRootJointTwistExisting.checkReferenceFramesMatch(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint()); tempRootJointTwistExisting.getAngularPart(tempRootJointTwistExistingAngularPart); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), tempRootJointTwistExistingAngularPart.getVector()); }
private void computeRootJointToWorldTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); }
private void computeRootJointToWorldTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); }
private void computeRootJointTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); } }
private void computeRootJointAngularVelocity(TwistCalculator twistCalculator, FrameVector rootJointAngularVelocityToPack, FrameVector angularVelocityEstimationLink) { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); RigidBody estimationLink = inverseDynamicsStructure.getEstimationLink(); tempEstimationLinkAngularVelocity.setIncludingFrame(angularVelocityEstimationLink); // T_{root}^{root, estimation} twistCalculator.getRelativeTwist(tempRootToEstimationTwist, estimationLink, rootJoint.getSuccessor()); tempRootToEstimationTwist.changeFrame(rootJoint.getFrameAfterJoint()); // omega_{root}^{root, estimation} tempRootToEstimationAngularVelocity.setToZero(rootJoint.getFrameAfterJoint()); tempRootToEstimationTwist.getAngularPart(tempRootToEstimationAngularVelocity); // omega_{estimation}^{root, world} tempEstimationLinkAngularVelocity.changeFrame(rootJoint.getFrameAfterJoint()); // omega_{root}^{root, world} = omega_{estimation}^{root, world} + omega_{root}^{root, estimation} rootJointAngularVelocityToPack.setToZero(rootJoint.getFrameAfterJoint()); rootJointAngularVelocityToPack.add(tempEstimationLinkAngularVelocity, tempRootToEstimationAngularVelocity); }
public PositionStateRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionOutputPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityOutputPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionOutputPort = centerOfMassPositionOutputPort; this.centerOfMassVelocityOutputPort = centerOfMassVelocityOutputPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); }
public void recordCurrentState() { FloatingInverseDynamicsJoint rootJoint = fullRobotModel.getRootJoint(); FloatingInverseDynamicsJointReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint(); rootJointFrame.getTraslation(rootJointTranslation); rootJointFrame.getRotation(rootJointRotation); yoRootJointTranslation.set(rootJointTranslation); yoRootJointRotation.set(rootJointRotation); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); updateRootJointRotation(inverseDynamicsStructure.getRootJoint(), orientationPort.getData(), estimationFrame); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); updateRootJointTwistAngularPart(twistCalculator, rootJoint, angularVelocityPort.getData()); rootJoint.getFrameAfterJoint().update(); twistCalculator.compute(); }
private void getCorrectionVelocityForMeasurementFrameOffset(FrameVector correctionTermToPack) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); measurementOffset.setToZero(measurementFrame); measurementOffset.changeFrame(rootJoint.getFrameAfterJoint()); correctionTermToPack.setToZero(tempRootJointAngularVelocity.getReferenceFrame()); correctionTermToPack.cross(tempRootJointAngularVelocity, measurementOffset); } }
public void computePelvisTrajectoryMessage() { checkIfDataHasBeenSet(); Point3d desiredPosition = new Point3d(); Quat4d desiredOrientation = new Quat4d(); ReferenceFrame pelvisFrame = fullRobotModelToUseForConversion.getRootJoint().getFrameAfterJoint(); FramePose desiredPelvisPose = new FramePose(pelvisFrame); desiredPelvisPose.changeFrame(worldFrame); desiredPelvisPose.getPose(desiredPosition, desiredOrientation); PelvisTrajectoryMessage pelvisTrajectoryMessage = new PelvisTrajectoryMessage(trajectoryTime, desiredPosition, desiredOrientation); output.setPelvisTrajectoryMessage(pelvisTrajectoryMessage); }
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); }
public YoRootJointDesiredConfigurationData(FloatingInverseDynamicsJoint rootJoint, YoVariableRegistry parentRegistry) { YoVariableRegistry registry = new YoVariableRegistry("RootJointDesiredConfigurationData"); parentRegistry.addChild(registry); ReferenceFrame frameAfterJoint = rootJoint.getFrameAfterJoint(); String namePrefix = "rootJointLowLevel"; orientation = new YoFrameQuaternion(namePrefix + "DesiredOrientation", worldFrame, registry); position = new YoFramePoint(namePrefix + "DesiredPosition", worldFrame, registry); linearVelocity = new YoFrameVector(namePrefix + "DesiredLinearVelocity", frameAfterJoint, registry); angularVelocity = new YoFrameVector(namePrefix + "DesiredAngularVelocity", frameAfterJoint, registry); linearAcceleration = new YoFrameVector(namePrefix + "DesiredLinearAcceleration", frameAfterJoint, registry); angularAcceleration = new YoFrameVector(namePrefix + "DesiredAngularAcceleration", frameAfterJoint, registry); clear(); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); centerOfMassCalculator.compute(); updateRootJointPosition(rootJoint, estimationFrame, centerOfMassPositionOutputPort.getData()); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); updateRootJointTwistLinearPart(centerOfMassVelocityOutputPort.getData(), rootJoint); twistCalculator.compute(); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); centerOfMassCalculator.compute(); updateRootJointConfiguration(rootJoint, estimationFrame); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); updateRootJointTwistAndSpatialAcceleration(twistCalculator, spatialAccelerationCalculator); twistCalculator.compute(); spatialAccelerationCalculator.compute(); }
private void loadScriptFileIfNecessary() { FramePoint soleInRootJointFrame = new FramePoint(bipedFeet.get(RobotSide.LEFT).getSoleFrame()); soleInRootJointFrame.changeFrame(fullRobotModel.getRootJoint().getFrameAfterJoint()); if (loadedScriptFile) return; // TODO: Get to work for more than just left foot frame. ReferenceFrame leftFootFrame = bipedFeet.get(RobotSide.LEFT).getSoleFrame(); RigidBodyTransform transformFromLeftFootPlaneFrameToWorldFrame = leftFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame()); ArrayList<ScriptObject> scriptObjectsList = scriptFileLoader.readIntoList(transformFromLeftFootPlaneFrameToWorldFrame); scriptObjects.addAll(scriptObjectsList); setupTimesForNewScriptEvent(0.1); loadedScriptFile = true; }
private void loadScriptFileIfNecessary() { FramePoint soleInRootJointFrame = new FramePoint(bipedFeet.get(RobotSide.LEFT).getSoleFrame()); soleInRootJointFrame.changeFrame(fullRobotModel.getRootJoint().getFrameAfterJoint()); if (loadedScriptFile) return; // TODO: Get to work for more than just left foot frame. ReferenceFrame leftFootFrame = bipedFeet.get(RobotSide.LEFT).getSoleFrame(); RigidBodyTransform transformFromLeftFootPlaneFrameToWorldFrame = leftFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame()); ArrayList<ScriptObject> scriptObjectsList = scriptFileLoader.readIntoList(transformFromLeftFootPlaneFrameToWorldFrame); scriptObjects.addAll(scriptObjectsList); setupTimesForNewScriptEvent(0.1); loadedScriptFile = true; }
@Override public void onBehaviorEntered() { hasTargetBeenProvided.set(false); haveFootstepsBeenGenerated.set(false); hasInputBeenSet.set(false); footstepListBehavior.initialize(); robotPose.setToZero(fullRobotModel.getRootJoint().getFrameAfterJoint()); robotPose.changeFrame(worldFrame); robotYoPose.set(robotPose); robotPose.getPosition(robotLocation); robotPose.getOrientation(robotOrientation); this.targetLocation.set(robotLocation); this.targetOrientation.set(robotOrientation); }
private void submitDesiredPelvisHeight(boolean parallelize, double offsetHeight) { FloatingInverseDynamicsJointReferenceFrame frameAfterRootJoint = fullRobotModel.getRootJoint().getFrameAfterJoint(); FramePoint desiredPelvisPosition = new FramePoint(frameAfterRootJoint); desiredPelvisPosition.setZ(offsetHeight); desiredPelvisPosition.changeFrame(worldFrame); PelvisHeightTrajectoryTask comHeightTask = new PelvisHeightTrajectoryTask(desiredPelvisPosition.getZ(), pelvisHeightTrajectoryBehavior, trajectoryTime.getDoubleValue()); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior, comHeightTask); pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior,createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(comHeightTask); pipeLine.submitSingleTaskStage(createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } }