public double getY() { return originPose.getY(); }
public void getPose2dIncludingFrame(FramePose2d framePose2dToPack) { framePose2dToPack.setPoseIncludingFrame(referenceFrame, getX(), getY(), getYaw()); }
public static void packFramePoseInJMEVector(FramePose original, Vector3f target) { target.set((float) original.getX(), (float) original.getY(), (float) original.getZ()); }
public static FramePose2d pose2dFormPose(FramePose pose) { FramePose2d pose2d = new FramePose2d(pose.getReferenceFrame()); pose2d.setYaw(pose.getYaw()); pose2d.setX(pose.getX()); pose2d.setY(pose.getY()); return pose2d; }
public void setPose(FramePose pose) { pose.getOrientation(tempYawPitchRoll); setYawPitchRoll(tempYawPitchRoll); x.set(pose.getX()); y.set(pose.getY()); z.set(pose.getZ()); }
private ContactableSelectableBoxRobot createDebrisRobot(FramePose debrisPose) { debrisPose.checkReferenceFrameMatch(constructionWorldFrame); ContactableSelectableBoxRobot debris = ContactableSelectableBoxRobot.createContactable2By4Robot(debrisName + String.valueOf(id++), debrisDepth, debrisWidth, debrisLength, debrisMass); debris.setPosition(debrisPose.getX(), debrisPose.getY(), debrisPose.getZ()); debris.setYawPitchRoll(debrisPose.getYaw(), debrisPose.getPitch(), debrisPose.getRoll()); return debris; }
private void updateTangentialCircleFrame() { if (controlledFrame != null) { tangentialCircleFramePose.setToZero(controlledFrame); } else { tangentialCircleFramePose.setToZero(currentPosition.getReferenceFrame()); tangentialCircleFramePose.setPosition(currentPosition); } tangentialCircleFramePose.changeFrame(circleFrame); double x = tangentialCircleFramePose.getX(); double y = tangentialCircleFramePose.getY(); double yaw = trimAngleMinusPiToPi(Math.PI / 2.0 + Math.atan2(y, x)); tangentialCircleFramePose.setYawPitchRoll(yaw, 0.0, 0.0); tangentialCircleFrame.setPoseAndUpdate(tangentialCircleFramePose); yoTangentialCircleFramePose.setAndMatchFrame(tangentialCircleFramePose); }
public void setConstantPose(FramePose constantPose) { position.checkReferenceFrameMatch(constantPose); position.set(constantPose.getX(), constantPose.getY(), constantPose.getZ()); orientation.set(constantPose.getYaw(), constantPose.getPitch(), constantPose.getRoll()); }
private void updateTangentialCircleFrame() { if (controlledFrame != null) { tangentialSteeringFramePose.setToZero(controlledFrame); } else { tangentialSteeringFramePose.setToZero(currentPosition.getReferenceFrame()); tangentialSteeringFramePose.setPosition(currentPosition); } tangentialSteeringFramePose.changeFrame(steeringWheelFrame); double x = tangentialSteeringFramePose.getX(); double y = tangentialSteeringFramePose.getY(); double yaw = trimAngleMinusPiToPi(Math.PI / 2.0 + Math.atan2(y, x)); tangentialSteeringFramePose.setYawPitchRoll(yaw, 0.0, 0.0); tangentialSteeringFrame.setPoseAndUpdate(tangentialSteeringFramePose); yoTangentialSteeringFramePose.setAndMatchFrame(tangentialSteeringFramePose); }
private void setXYVectorFromPoseToPoseNormalize(YoFrameVector2d vectorToPack, FramePose fromPose, FramePose toPose) { if (fromPose.epsilonEquals(toPose, 1e-7, Double.MAX_VALUE)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2d frameTuple2d = vectorToPack.getFrameTuple2d(); frameTuple2d.setByProjectionOntoXYPlane(toPose.getFramePointCopy()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.setWithoutChecks(frameTuple2d); } }
private void calculateInitialFootPoseAndOffsets() { SideDependentList<Footstep> currentFootsteps; if (priorStanceFeetSpecified) currentFootsteps = priorStanceFeet; else currentFootsteps = createFootstepsFromBipedFeet(); Footstep left = currentFootsteps.get(RobotSide.LEFT); Footstep right = currentFootsteps.get(RobotSide.RIGHT); FramePose leftPose = new FramePose(); FramePose rightPose = new FramePose(); left.getSolePose(leftPose); right.getSolePose(rightPose); FramePose2d leftPose2d = new FramePose2d(); FramePose2d rightPose2d = new FramePose2d(); leftPose.getPose2dIncludingFrame(leftPose2d); rightPose.getPose2dIncludingFrame(rightPose2d); startPose.interpolate(leftPose2d, rightPose2d, 0.5); Pose2dReferenceFrame startFramePose = new Pose2dReferenceFrame("StartPoseFrame", startPose); leftPose.changeFrame(startFramePose); rightPose.changeFrame(startFramePose); FrameOrientation2d leftOrientation = new FrameOrientation2d(); FrameOrientation2d rightOrientation = new FrameOrientation2d(); leftPose.getOrientation2dIncludingFrame(leftOrientation); rightPose.getOrientation2dIncludingFrame(rightOrientation); initialDeltaFeetYaw = leftOrientation.sub(rightOrientation); initialDeltaFeetY = leftPose.getY() - rightPose.getY(); initialDeltaFeetX = leftPose.getX() - rightPose.getX(); }
public void setGoalPose(FramePose goalPose) { goalPose.getPosition2dIncludingFrame(this.goalPosition); Tuple3d goalPosition = new Point3d(); goalPose.getPosition(goalPosition); FootstepPlannerGoal footstepPlannerGoal = new FootstepPlannerGoal(); footstepPlannerGoal.setGoalPoseBetweenFeet(goalPose); Point2d xyGoal = new Point2d(); xyGoal.setX(goalPose.getX()); xyGoal.setY(goalPose.getY()); double distanceFromXYGoal = 1.0; footstepPlannerGoal.setXYGoal(xyGoal, distanceFromXYGoal); footstepPlannerGoal.setFootstepPlannerGoalType(FootstepPlannerGoalType.CLOSE_TO_XY_POSITION); FramePose leftFootPose = new FramePose(); leftFootPose.setToZero(referenceFrames.getSoleFrame(RobotSide.LEFT)); leftFootPose.changeFrame(ReferenceFrame.getWorldFrame()); sendPacketToUI(new UIPositionCheckerPacket(new Point3d(xyGoal.getX(), xyGoal.getY(), leftFootPose.getZ()), new Quat4d())); footstepPlanner.setGoal(footstepPlannerGoal); footstepPlanner.setInitialStanceFoot(leftFootPose, RobotSide.LEFT); }
xyGoal.setY(goalPose.getY()); double distanceFromXYGoal = 1.0; footstepPlannerGoal.setXYGoal(xyGoal, distanceFromXYGoal);
xyGoal.setY(goalPose.getY()); double distanceFromXYGoal = 1.0; footstepPlannerGoal.setXYGoal(xyGoal, distanceFromXYGoal);