public void getPose(Point3d pointToPack, Quat4d quaternionToPack) { originPose.getPose(pointToPack, quaternionToPack); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { originPose.checkReferenceFrameMatch(parentFrame); originPose.getPose(transformToParent); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { currentDistortionPose.changeFrame(parentFrame); currentDistortionPose.getPose(transformToParent); } };
public void reportFootstepStarted(RobotSide robotSide, FramePose desiredFootPoseInWorld, FramePose actualFootPoseInWorld) { desiredFootPoseInWorld.getPose(desiredFootPositionInWorld, desiredFootOrientationInWorld); actualFootPoseInWorld.getPose(actualFootPositionInWorld, actualFootOrientationInWorld); statusOutputManager.reportStatusMessage(new FootstepStatus(FootstepStatus.Status.STARTED, currentFootstepIndex.getIntegerValue(), desiredFootPositionInWorld, desiredFootOrientationInWorld, actualFootPositionInWorld, actualFootOrientationInWorld, robotSide)); }
public RigidBodyTransform getTransformFromThisToThat(FramePose thatPose) { checkReferenceFrameMatch(thatPose); RigidBodyTransform transformToThis = new RigidBodyTransform(); this.getPose(transformToThis); RigidBodyTransform transformToThat = new RigidBodyTransform(); thatPose.getPose(transformToThat); return TransformTools.getTransformFromA2toA1(transformToThat, transformToThis); }
@Override public final void setInitialStanceFoot(FramePose stanceFootPose, RobotSide initialSide) { stanceFootPose.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); this.initialSide = initialSide; stanceFootPose.getPose(initialFootPose); initialStanceFootWasSet = true; }
public void setControlFrameFixedInEndEffector(FramePose pose) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); pose.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); pose.getPose(controlFrameOriginInEndEffectorFrame, controlFrameOrientationInEndEffectorFrame); }
private void changeControlFrame(ReferenceFrame oldControlFrame, ReferenceFrame newControlFrame, FramePose framePoseToModify) { if (oldControlFrame == newControlFrame) return; framePoseToModify.getPose(oldTrackingFrameDesiredTransform); newControlFrame.getTransformToDesiredFrame(transformFromNewTrackingFrameToOldTrackingFrame, oldControlFrame); newTrackingFrameDesiredTransform.multiply(oldTrackingFrameDesiredTransform, transformFromNewTrackingFrameToOldTrackingFrame); framePoseToModify.setPose(newTrackingFrameDesiredTransform); }
private void changeDesiredPoseBodyFrame(ReferenceFrame oldBodyFrame, ReferenceFrame newBodyFrame, FramePose framePoseToModify) { if (oldBodyFrame == newBodyFrame) return; framePoseToModify.getPose(oldBodyFrameDesiredTransform); newBodyFrame.getTransformToDesiredFrame(transformFromNewBodyFrameToOldBodyFrame, oldBodyFrame); newBodyFrameDesiredTransform.multiply(oldBodyFrameDesiredTransform, transformFromNewBodyFrameToOldBodyFrame); framePoseToModify.setPose(newBodyFrameDesiredTransform); }
public double getEffectiveDistanceToFramePose(FramePose framePose, double radiusOfRotation) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); transformThis.invert(); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); transformThat.invert(); return TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformThis, transformThat, radiusOfRotation); }
public void changeFrameAndSetControlFrameFixedInEndEffector(FramePose pose) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); pose.changeFrame(endEffector.getBodyFixedFrame()); pose.getPose(controlFrameOriginInEndEffectorFrame, controlFrameOrientationInEndEffectorFrame); }
public void reportFootstepCompleted(RobotSide robotSide, FramePose actualFootPoseInWorld) { actualFootPoseInWorld.getPose(actualFootPositionInWorld, actualFootOrientationInWorld); statusOutputManager.reportStatusMessage(new FootstepStatus(FootstepStatus.Status.COMPLETED, currentFootstepIndex.getIntegerValue(), actualFootPositionInWorld, actualFootOrientationInWorld, robotSide)); // reusableSpeechPacket.setTextToSpeak(TextToSpeechPacket.FOOTSTEP_COMPLETED); // statusOutputManager.reportStatusMessage(reusableSpeechPacket); }
public void setSolePose(FramePose newSolePoseInWorldFrame) { newSolePoseInWorldFrame.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); if (transformFromSoleToWorld == null) transformFromSoleToWorld = new RigidBodyTransform(); newSolePoseInWorldFrame.getPose(transformFromSoleToWorld); setSolePose(transformFromSoleToWorld); }
public FootTrajectoryTask(E stateEnum,RobotSide robotSide, FramePose pose, FootTrajectoryBehavior behavior, double trajectoryTime) { super(stateEnum,behavior); this.footPoseBehavior = behavior; pose.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); Point3d position = new Point3d(); Quat4d orientation = new Quat4d(); pose.getPose(position, orientation); footTrajectoryMessage = new FootTrajectoryMessage(robotSide, trajectoryTime, position, orientation); }
private void submitFootPose(RobotSide robotSide, FramePose desiredFootPose) { desiredFootPose.changeFrame(worldFrame); Point3d desiredFootPosition = new Point3d(); Quat4d desiredFootOrientation = new Quat4d(); desiredFootPose.getPose(desiredFootPosition, desiredFootOrientation); FootTrajectoryTask task = new FootTrajectoryTask(robotSide, desiredFootPosition, desiredFootOrientation, footTrajectoryBehavior, trajectoryTime.getDoubleValue()); pipeLine.submitSingleTaskStage(task); }
public void setSoleFrame(FramePose newSolePoseInWorldFrame) { newSolePoseInWorldFrame.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); RigidBodyTransform transformFromSoleToWorld = new RigidBodyTransform(); newSolePoseInWorldFrame.getPose(transformFromSoleToWorld); RigidBodyTransform transformFromShinToWorld = new RigidBodyTransform(); transformFromShinToWorld.multiply(transformFromSoleToWorld, shinFrame.getTransformToDesiredFrame(soleFrame)); shinFrame.setPoseAndUpdate(transformFromShinToWorld); }
public void computeFootTrajectoryMessage(RobotSide robotSide) { checkIfDataHasBeenSet(); Point3d desiredPosition = new Point3d(); Quat4d desiredOrientation = new Quat4d(); ReferenceFrame footFrame = fullRobotModelToUseForConversion.getEndEffectorFrame(robotSide, LimbName.LEG); FramePose desiredFootPose = new FramePose(footFrame); desiredFootPose.changeFrame(worldFrame); desiredFootPose.getPose(desiredPosition, desiredOrientation); FootTrajectoryMessage footTrajectoryMessage = new FootTrajectoryMessage(robotSide, trajectoryTime, desiredPosition, desiredOrientation); output.setFootTrajectoryMessage(footTrajectoryMessage); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { midFeetPose.setToZero(midFeetZUpWalkDirectionFrame); pelvisPose.setToZero(getPelvisFrame()); pelvisPose.changeFrame(midFeetZUpWalkDirectionFrame); midFeetPose.setX(pelvisPose.getX()); midFeetPose.changeFrame(ReferenceFrame.getWorldFrame()); midFeetPose.getPose(transformToParent); } };
public void computeHandTrajectoryMessage(RobotSide robotSide) { checkIfDataHasBeenSet(); BaseForControl baseForControl = BaseForControl.WORLD; Point3d desiredPosition = new Point3d(); Quat4d desiredOrientation = new Quat4d(); ReferenceFrame handControlFrame = fullRobotModelToUseForConversion.getHandControlFrame(robotSide); FramePose desiredHandPose = new FramePose(handControlFrame); desiredHandPose.changeFrame(worldFrame); desiredHandPose.getPose(desiredPosition, desiredOrientation); HandTrajectoryMessage handTrajectoryMessage = new HandTrajectoryMessage(robotSide, baseForControl, trajectoryTime, desiredPosition, desiredOrientation); output.setHandTrajectoryMessage(handTrajectoryMessage); }
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); }