public Footstep createFootstep(RobotSide robotSide, Point3d position, double[] orientationYawPitchRoll) { FramePose footstepPose = new FramePose(); footstepPose.setPosition(position); footstepPose.setYawPitchRoll(orientationYawPitchRoll); return createFootstep(robotSide, footstepPose); }
public static FramePose poseFormPose2d(FramePose2d pose2d) { FramePose pose = new FramePose(pose2d.getReferenceFrame()); pose.setYawPitchRoll(pose2d.getYaw(), 0.0, 0.0); pose.setX(pose2d.getX()); pose.setY(pose2d.getY()); return pose; }
public void setInterpolatorInputs(FramePose startOffsetError, FramePose goalOffsetError, double alphaFilterPosition) { startOffsetErrorPose.setPoseIncludingFrame(startOffsetError); goalOffsetErrorPose.setPoseIncludingFrame(goalOffsetError); if (!isRotationCorrectionEnabled.getBooleanValue()) { startOffsetErrorPose.setYawPitchRoll(0.0, 0.0, 0.0); goalOffsetErrorPose.setYawPitchRoll(0.0, 0.0, 0.0); } //scs feedback only yoStartOffsetErrorPose_InWorldFrame.set(startOffsetErrorPose); yoGoalOffsetErrorPose_InWorldFrame.set(goalOffsetErrorPose); stateEstimatorReferenceFrame.update(); updateStartAndGoalOffsetErrorTranslation(); updateStartAndGoalOffsetErrorRotation(); ///////////////////// initializeAlphaFilter(alphaFilterPosition); updateMaxTranslationAlphaVariationSpeed(); updateMaxRotationAlphaVariationSpeed(); }
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 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); }
@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); } };
footPose.setYawPitchRoll(0.0, 0.8 * Math.PI / 2.0, 0.0); submitFootPose(true, robotSide, footPose); footPose.setYawPitchRoll(0.0, 0.0, robotSide.negateIfRightSide(Math.toRadians(40.0))); submitFootPose(true, robotSide, footPose); submitChestHomeCommand(true); footPose.setYawPitchRoll(0.0, 0.0, 0.0); submitFootPose(true, robotSide, footPose);
public static void main(String[] args) throws IOException { FramePose framePose = new FramePose(); framePose.setYawPitchRoll(1.0, 0.8, -1.1); framePose.setPosition(3.1, 0.1, 1.0); System.out.println(framePose.getFrameOrientationCopy().toStringAsQuaternion()); RigidBodyTransform transformToParent = new RigidBodyTransform(); framePose.getPose(transformToParent); ReferenceFrame gridFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("blop", ReferenceFrame.getWorldFrame(), transformToParent); SphereVoxelShape sphereVoxelShape = new SphereVoxelShape(gridFrame, 0.1, 10, 12, SphereVoxelType.graspOrigin); Voxel3DGrid voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, 10, 0.1); RobotArm robot = new RobotArm(); OneDoFJoint[] armJoints = ScrewTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJoint.class); ReachabilityMapFileWriter reachabilityMapFileWriter = new ReachabilityMapFileWriter(robot.getName(), armJoints, voxel3dGrid, ReachabilityMapFileWriter.class); for (int i = 0; i < 70000; i++) reachabilityMapFileWriter.registerReachablePose(0, 1, 2, 3, 4); reachabilityMapFileWriter.exportAndClose(); } }
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)); }
footPose.setToZero(ankleZUpFrame); footPose.setPosition(0.75, robotSide.negateIfRightSide(0.25), 0.25); footPose.setYawPitchRoll(0.0, -halfPi / 2.0, 0.0); submitFootPose(true, robotSide, footPose); footPose.setYawPitchRoll(0.0, 0.8 * halfPi, 0.0); submitFootPose(true, robotSide, footPose); footPose.setYawPitchRoll(0.0, 0.0, robotSide.negateIfRightSide(Math.toRadians(40.0))); submitFootPose(true, robotSide, footPose); footPose.setYawPitchRoll(0.0, 0.0, 0.0); submitFootPose(true, robotSide, footPose);
private void submitDesiredPelvisPositionOffsetAndOrientation(boolean parallelize, double dx, double dy, double dz, double yaw, double pitch, double roll) { FloatingInverseDynamicsJointReferenceFrame frameAfterRootJoint = fullRobotModel.getRootJoint().getFrameAfterJoint(); FramePose desiredPelvisPose = new FramePose(frameAfterRootJoint); desiredPelvisPose.setPosition(dx, dy, dz); desiredPelvisPose.setYawPitchRoll(yaw, pitch, roll); desiredPelvisPose.changeFrame(worldFrame); Point3d position = new Point3d(); Quat4d orientation = new Quat4d(); desiredPelvisPose.getPose(position, orientation); PelvisTrajectoryMessage message = new PelvisTrajectoryMessage(trajectoryTime.getDoubleValue(), position, orientation); PelvisTrajectoryTask task = new PelvisTrajectoryTask(message, pelvisTrajectoryBehavior); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisTrajectoryBehavior, task); pipeLine.submitTaskForPallelPipesStage(pelvisTrajectoryBehavior,createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(task); pipeLine.submitSingleTaskStage(createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } }