@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { currentDistortionPose.changeFrame(parentFrame); currentDistortionPose.getPose(transformToParent); } };
public void getDesiredEndEffectorPoseFromQDesireds(FramePose desiredPose, ReferenceFrame desiredFrame) { desiredPose.setToZero(localControlFrame); desiredPose.changeFrame(desiredFrame); }
public void getSolePose(FramePose soleFrameToPack) { soleFrameToPack.setToZero(soleReferenceFrame); soleFrameToPack.changeFrame(ReferenceFrame.getWorldFrame()); }
private void setControlFrameFixedInEndEffector(ReferenceFrame controlFrame) { controlFramePose.setToZero(controlFrame); controlFramePose.changeFrame(endEffectorFrame); spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(controlFramePose); this.controlFrame.setPoseAndUpdate(controlFramePose); }
public void setInitialPose(FramePose initialPose) { initialPose.changeFrame(trajectoryFrame); initialPose.getPoseIncludingFrame(initialPosition, initialOrientation); yoInitialPosition.set(initialPosition); yoInitialOrientation.set(initialOrientation); }
public void changeFrameAndSetControlFrameFixedInEndEffector(FramePose pose) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); pose.changeFrame(endEffector.getBodyFixedFrame()); pose.getPose(controlFrameOriginInEndEffectorFrame, controlFrameOrientationInEndEffectorFrame); }
@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); } };
private FramePose generateDebrisPose(Point3d positionWithRespectToRobot, double debrisYaw, double debrisPitch, double debrisRoll) { FramePose debrisPose = new FramePose(robotInitialPoseReferenceFrame); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(debrisYaw, debrisPitch, debrisRoll, orientation); debrisPose.setPose(positionWithRespectToRobot, orientation); debrisPose.changeFrame(constructionWorldFrame); return debrisPose; }
public void variableChanged(YoVariable<?> v) { if (userUpdateDesiredPelvisPose.getBooleanValue()) { framePose.setToZero(pelvisFrame); framePose.changeFrame(midFeetZUpFrame); userDesiredPelvisPose.set(framePose); userUpdateDesiredPelvisPose.set(false); } } });
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(position.getReferenceFrame()); framePoseToPack.setPosition(position.getFrameTuple()); orientation.get(temp); framePoseToPack.setOrientation(temp); }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public static Footstep generateStandingFootstep(RobotSide side, RigidBody foot, ReferenceFrame soleFrame) { FramePose footFramePose = new FramePose(foot.getParentJoint().getFrameAfterJoint()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose); Footstep footstep = new Footstep(foot, side, soleFrame, footstepPoseFrame, trustHeight); return footstep; }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(currentPosition.getReferenceFrame()); framePoseToPack.setPosition(currentPosition.getFrameTuple()); currentOrientation.get(temp); framePoseToPack.setOrientation(temp); }
public Footstep getFootstepAtCurrentLocation(RobotSide robotSide) { tempPose.setToZero(contactableFeet.get(robotSide).getFrameAfterParentJoint()); tempPose.changeFrame(worldFrame); Footstep footstep = footstepsAtCurrentLocation.get(robotSide); footstep.setPose(tempPose); return footstep; }
@Override public void doTransitionOutOfAction() { super.doTransitionOutOfAction(); balanceManager.minimizeAngularMomentumRateZ(false); actualFootPoseInWorld.setToZero(fullRobotModel.getEndEffectorFrame(swingSide, LimbName.LEG)); // changed Here Nicolas actualFootPoseInWorld.changeFrame(worldFrame); walkingMessageHandler.reportFootstepCompleted(swingSide, actualFootPoseInWorld); walkingMessageHandler.registerCompletedDesiredFootstep(nextFootstep); }
public static Footstep generateStandingFootstep(RobotSide side, SideDependentList<? extends ContactablePlaneBody> bipedFeet) { ContactablePlaneBody endEffector = bipedFeet.get(side); FramePose footFramePose = new FramePose(endEffector.getFrameAfterParentJoint()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose); Footstep footstep = new Footstep(endEffector.getRigidBody(), side, endEffector.getSoleFrame(), footstepPoseFrame, trustHeight); return footstep; }
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 Footstep createFootstepAtCurrentLocation(RobotSide robotSide) { ContactablePlaneBody foot = feet.get(robotSide); ReferenceFrame footReferenceFrame = foot.getRigidBody().getParentJoint().getFrameAfterJoint(); FramePose framePose = new FramePose(footReferenceFrame); framePose.changeFrame(worldFrame); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("poseReferenceFrame", framePose); boolean trustHeight = true; Footstep footstep = new Footstep(foot.getRigidBody(), robotSide, foot.getSoleFrame(), poseReferenceFrame, trustHeight); return footstep; }