public void setZ(double z) { originPose.setZ(z); }
public static FramePose poseFormPose2d(FramePose2d pose2d, double z) { FramePose pose = poseFormPose2d(pose2d); pose.setZ(z); return pose; }
public void addVerticalDebrisLeaningAgainstAWall(double xRelativeToRobot, double yRelativeToRobot, double debrisYaw, double debrisPitch) { Point3d tempPosition = new Point3d(xRelativeToRobot, yRelativeToRobot, debrisLength / 2.0 * Math.cos(debrisPitch)); FramePose debrisPose = generateDebrisPose(tempPosition, debrisYaw, debrisPitch, 0.0); debrisRobots.add(createDebrisRobot(debrisPose)); double supportWidth = 0.1; double supportLength = 0.2; double supportHeight = 1.05*debrisLength; RigidBodyTransform debrisTransform = new RigidBodyTransform(); debrisPose.getPose(debrisTransform ); TransformTools.rotate(debrisTransform, -debrisPitch, Axis.Y); debrisPose.setPose(debrisTransform); debrisPose.setZ(0.0); PoseReferenceFrame debrisReferenceFrame = new PoseReferenceFrame("debrisReferenceFrame", debrisPose); FramePose supportPose = new FramePose(debrisReferenceFrame); double x = supportWidth / 2.0 + debrisLength/2.0 * Math.sin(debrisPitch); double y = 0.0; double z = supportHeight / 2.0; supportPose.setPosition(x, y, z); supportPose.changeFrame(constructionWorldFrame); RigidBodyTransform supportTransform = new RigidBodyTransform(); supportPose.getPose(supportTransform); combinedTerrainObject.addRotatableBox(supportTransform, supportWidth, supportLength, supportHeight, YoAppearance.AliceBlue()); }
@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); } };
TransformTools.rotate(debrisTransform, -debrisRoll, Axis.X); debrisPose.setPose(debrisTransform); debrisPose.setZ(0.0); PoseReferenceFrame debrisReferenceFrame = new PoseReferenceFrame("debrisReferenceFrame", debrisPose);
footstep.getSolePose(footstepSolePose); footstepSolePose.setZ(footstepSolePose.getZ() + finalSwingHeightOffset.getDoubleValue()); finalConfigurationProvider.setPose(footstepSolePose); orientationTrajectoryGenerator.setFinalOrientation(footstepSolePose);
solePose.setZ(0.0);