public void setPositionWithoutChecksAndUpdate(Point3d position) { originPose.setPosition(position); this.update(); }
public void setPositionWithoutChecksAndUpdate(double x, double y, double z) { originPose.setPosition(x, y, z); this.update(); }
public void setPose(FramePoint position, FrameOrientation orientation) { setPosition(position); setOrientation(orientation); }
public void setPositionAndUpdate(FramePoint framePoint) { framePoint.checkReferenceFrameMatch(parentFrame); originPose.setPosition(framePoint); this.update(); }
public void getFramePose(FramePose framePoseToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); framePoseToPack.setPosition(tempFramePoint); framePoseToPack.setOrientation(tempFrameOrientation); }
public void getFramePose(FramePose framePoseToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); framePoseToPack.setPosition(tempFramePoint); framePoseToPack.setOrientation(tempFrameOrientation); }
public Footstep createFootstep(RobotSide robotSide, Point3d position, double[] orientationYawPitchRoll) { FramePose footstepPose = new FramePose(); footstepPose.setPosition(position); footstepPose.setYawPitchRoll(orientationYawPitchRoll); return createFootstep(robotSide, footstepPose); }
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 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 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 void getPose(FramePose poseToPack) { checkReferenceFrameMatch(poseToPack); SE3Waypoint waypointData = geometryObject.waypointData; poseToPack.setPosition(waypointData.getEuclideanWaypoint().getPosition()); poseToPack.setOrientation(waypointData.getSO3Waypoint().getOrientation()); }
@Override public void getReportedGoalPoseWorldFrame(FramePose framePoseToPack) { ReferenceFrame midFeetZUpFrame = referenceFrames.getMidFeetZUpFrame(); FramePoint goalPosition = new FramePoint(midFeetZUpFrame, constantGoalLocationInMidFeetZUpFrame); goalPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameOrientation goalOrientation = new FrameOrientation(midFeetZUpFrame); goalOrientation.changeFrame(ReferenceFrame.getWorldFrame()); framePoseToPack.setPosition(goalPosition); framePoseToPack.setOrientation(goalOrientation); }
public void setInitialLeadOut(FramePoint initialPosition, FrameVector initialDirection, double leaveDistance) { this.initialPosition.set(initialPosition); this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); initialDistortionPose.setToZero(this.initialPosition.getReferenceFrame()); initialDistortionPose.setPosition(initialPosition); initialDistortionPose.setOrientation(tempAxisAngle); this.leaveDistance.set(leaveDistance); }
public void setFinalLeadIn(FramePoint finalPosition, FrameVector finalDirection, double approachDistance) { this.finalPosition.set(finalPosition); this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); finalDistortionPose.setToZero(this.finalPosition.getReferenceFrame()); finalDistortionPose.setPosition(finalPosition); finalDistortionPose.setOrientation(tempAxisAngle); this.approachDistance.set(approachDistance); }
public void rotatePoseAboutAxis(ReferenceFrame rotationAxisFrame, Axis rotationAxis, double angle, boolean lockPosition, boolean lockOrientation) { ReferenceFrame initialFrame = this.referenceFrame; this.changeFrame(rotationAxisFrame); RigidBodyTransform axisRotationTransform = new RigidBodyTransform(); TransformTools.rotate(axisRotationTransform, angle, rotationAxis); if (!lockPosition) { Vector3d tempVector = new Vector3d(); this.getPosition(tempVector); axisRotationTransform.transform(tempVector); this.setPosition(tempVector); } if(!lockOrientation) this.pose.getOrientation().applyTransform(axisRotationTransform); this.changeFrame(initialFrame); }
private void kraneKick(RobotSide robotSide) { ReferenceFrame ankleZUpFrame = ankleZUpFrames.get(robotSide.getOppositeSide()); // First Lift up the foot submitFootPosition(false, robotSide, new FramePoint(ankleZUpFrame, 0.1, robotSide.negateIfRightSide(0.25), 0.2)); submitSymmetricHumanoidArmPose(HumanoidArmPose.KARATE_KID_KRANE_KICK); pipeLine.requestNewStage(); // Go back to stand prep but don't put the foot on the ground yet submitSymmetricHumanoidArmPose(HumanoidArmPose.STAND_PREP); FramePose footPose = new FramePose(); footPose.setToZero(ankleZUpFrame); footPose.setPosition(0.0, robotSide.negateIfRightSide(0.25), 0.1); footPose.setYawPitchRoll(0.0, 0.0, 0.0); submitFootPose(true, robotSide, footPose); submitChestHomeCommand(true); // Put the foot back on the ground submitFootPosition(false, robotSide, new FramePoint(ankleZUpFrame, 0.0, robotSide.negateIfRightSide(0.25), -0.3)); }