public double getZ() { return originPose.getZ(); }
public static void packFramePoseInJMEVector(FramePose original, Vector3f target) { target.set((float) original.getX(), (float) original.getY(), (float) original.getZ()); }
@Override public void setInitialStanceFoot(FramePose stanceFootPose, RobotSide side) { this.initialStanceFootPose.set(FlatGroundPlanningUtils.pose2dFormPose(stanceFootPose)); this.initialStanceFootPose.changeFrame(ReferenceFrame.getWorldFrame()); this.initialStanceSide = side; this.lastStepSide = side; this.groundHeight = stanceFootPose.getZ(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { for (RobotSide robotSide : RobotSide.values) { ReferenceFrame footFrame = getFootFrame(robotSide); footFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame()); FramePose footPose = footPoses.get(robotSide); footPose.setPoseIncludingFrame(ReferenceFrame.getWorldFrame(), tempTransform); footPose.getOrientation(tempYawPitchRoll); tempYawPitchRoll[1] = 0.0; tempYawPitchRoll[2] = 0.0; footPose.setYawPitchRoll(tempYawPitchRoll); } midFootZUpPose.interpolate(leftFootPose, rightFootPose, 0.5); midFootZUpPose.setZ(Math.min(leftFootPose.getZ(), rightFootPose.getZ())); midFootZUpPose.getPose(transformToParent); } };
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; }
public void setPose(FramePose pose) { pose.getOrientation(tempYawPitchRoll); setYawPitchRoll(tempYawPitchRoll); x.set(pose.getX()); y.set(pose.getY()); z.set(pose.getZ()); }
public void setConstantPose(FramePose constantPose) { position.checkReferenceFrameMatch(constantPose); position.set(constantPose.getX(), constantPose.getY(), constantPose.getZ()); orientation.set(constantPose.getYaw(), constantPose.getPitch(), constantPose.getRoll()); }
footstep.getSolePose(footstepSolePose); footstepSolePose.setZ(footstepSolePose.getZ() + finalSwingHeightOffset.getDoubleValue()); finalConfigurationProvider.setPose(footstepSolePose); orientationTrajectoryGenerator.setFinalOrientation(footstepSolePose); double zDifference = Math.abs(footstepSolePose.getZ() - oldFootstepPosition.getZ()); boolean stepUpOrDown = zDifference > minHeightDifferenceForObstacleClearance.getDoubleValue();
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); }