public double getSwingHeight(FramePose startPose, FramePose endPose, double stanceHeight, HeightMapWithPoints groundProfile) { FramePoint startFramePoint = startPose.getFramePointCopy(); FramePoint endFramePoint = endPose.getFramePointCopy(); Point3d startPoint = startFramePoint.getPointCopy(); Point3d endPoint = endFramePoint.getPointCopy(); return getSwingHeight(startPoint, endPoint, stanceHeight, groundProfile); }
private void setVectorFromPoseToPose(YoFrameVector frameVectorToPack, FramePose fromPose, FramePose toPose) { frameVectorToPack.set(toPose.getFramePointCopy()); FrameVector frameTuple = frameVectorToPack.getFrameTuple(); frameTuple.sub(fromPose.getFramePointCopy()); frameVectorToPack.setWithoutChecks(frameTuple); }
private FootstepDataMessage createRelativeFootStep(PoseReferenceFrame frame, RobotSide side, Point3d location, Quat4d orientation) { FramePose pose = offsetPointFromFrameInWorldFrame(frame, location, orientation); FootstepDataMessage message = new FootstepDataMessage(side, pose.getFramePointCopy().getPoint(), pose.getFrameOrientationCopy().getQuaternion()); return message; }
private void moveHand(final double x, final double y, final double z, final double yaw, final double pitch, final double roll, final String description) { TextToSpeechPacket p1 = new TextToSpeechPacket(description); sendPacket(p1); // Vector3d orient = new Vector3d(); // referenceFrames.getHandFrame(RobotSide.RIGHT).getTransformToDesiredFrame(valvePose).getRotationEuler(orient); // 1.607778783110418,1.442441289823466,-3.1298946145335043` FramePose point = offsetPointFromValveInWorldFrame(x, y, z, yaw, pitch, roll); // System.out.println("-orient.x,orient.y, orient.z " + (-orient.x) + "," + orient.y + "," + orient.z); sendPacketToUI(new UIPositionCheckerPacket(point.getFramePointCopy().getPoint())); HandTrajectoryMessage handTrajectoryMessage = new HandTrajectoryMessage(RobotSide.RIGHT, 2, point.getFramePointCopy().getPoint(), point.getFrameOrientationCopy().getQuaternion()); atlasPrimitiveActions.rightHandTrajectoryBehavior.setInput(handTrajectoryMessage); }
@Override protected void setBehaviorInput() { TextToSpeechPacket p1 = new TextToSpeechPacket("rotate Valve"); sendPacket(p1); FramePose point = offsetPointFromValveInWorldFrame(0.0, valveRadius + valveRadiusfinalOffset, distanceFromValve, 1.5708, 1.5708, -3.14159); point.rotatePoseAboutAxis(valvePose, Axis.Z, degrees); sendPacketToUI(new UIPositionCheckerPacket(point.getFramePointCopy().getPoint())); HandTrajectoryMessage handTrajectoryMessage = new HandTrajectoryMessage(RobotSide.RIGHT, 2, point.getFramePointCopy().getPoint(), point.getFrameOrientationCopy().getQuaternion()); atlasPrimitiveActions.rightHandTrajectoryBehavior.setInput(handTrajectoryMessage); } };
private void moveHand(final double x, final double y, final double z, final double yaw, final double pitch, final double roll, final String description) { TextToSpeechPacket p1 = new TextToSpeechPacket(description); sendPacket(p1); FramePose point = offsetPointFromChestInWorldFrame(x, y, z, yaw, pitch, roll); HandTrajectoryMessage handTrajectoryMessage = new HandTrajectoryMessage(RobotSide.RIGHT, 2, point.getFramePointCopy().getPoint(), point.getFrameOrientationCopy().getQuaternion()); atlasPrimitiveActions.rightHandTrajectoryBehavior.setInput(handTrajectoryMessage); }
@Override public void doTransitionOutOfAction() { FramePose goalPose = new FramePose(); locateGoalBehavior.getReportedGoalPoseWorldFrame(goalPose); setGoalPose(goalPose); super.doTransitionOutOfAction(); sendPacketToUI(new UIPositionCheckerPacket(goalPose.getFramePointCopy().getPoint(), goalPose.getFrameOrientationCopy().getQuaternion())); PrintTools.info("got the goal: \n" + goalPose + " \n requesting planar regions..."); } };
@Override public void variableChanged(YoVariable<?> v) { if (userDesiredSetPelvisHeightToActual.getBooleanValue()) { ReferenceFrame pelvisFrame = fullRobotModel.getPelvis().getBodyFixedFrame(); FramePose currentPose = new FramePose(pelvisFrame); currentPose.changeFrame(ReferenceFrame.getWorldFrame()); userDesiredPelvisHeight.set(currentPose.getFramePointCopy().getZ()); userDesiredSetPelvisHeightToActual.set(false); } } });
@Override public void variableChanged(YoVariable<?> v) { if (userDesiredSetHandPoseToActual.getBooleanValue()) { ReferenceFrame referenceFrame = getReferenceFrameToUse(); ReferenceFrame wristFrame = fullRobotModel.getEndEffectorFrame(userHandPoseSide.getEnumValue(), LimbName.ARM); FramePose currentPose = new FramePose(wristFrame); currentPose.changeFrame(referenceFrame); userDesiredHandPose.setPosition(currentPose.getFramePointCopy().getPointCopy()); userDesiredHandPose.setOrientation(currentPose.getFrameOrientationCopy().getQuaternionCopy()); userDesiredSetHandPoseToActual.set(false); } } });
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); } }
trajectoryPoint.setPosition(framePose.getFramePointCopy()); trajectoryPoint.setOrientation(framePose.getFrameOrientationCopy()); trajectoryPoint.setLinearVelocity(new Vector3d());
FramePoint startFramePoint = startPose.getFramePointCopy(); FramePoint endFramePoint = endPose.getFramePointCopy(); Point3d startPoint = startFramePoint.getPointCopy(); Point3d endPoint = endFramePoint.getPointCopy();
public void variableChanged(YoVariable<?> v) { if (userDoFootPose.getBooleanValue()) { userDesiredFootPose.getFramePose(framePose); ReferenceFrame soleZUpFrame = ankleZUpFrames.get(userFootPoseSide.getEnumValue()); soleZUpFrame.update(); framePose.setIncludingFrame(soleZUpFrame, framePose.getGeometryObject()); framePose.changeFrame(ReferenceFrame.getWorldFrame()); System.out.println("framePose " + framePose); FootTrajectoryCommand footTrajectoryControllerCommand = new FootTrajectoryCommand(); FrameSE3TrajectoryPoint trajectoryPoint = new FrameSE3TrajectoryPoint(ReferenceFrame.getWorldFrame()); trajectoryPoint.setTime(userDesiredFootPoseTrajectoryTime.getDoubleValue()); trajectoryPoint.setPosition(framePose.getFramePointCopy()); trajectoryPoint.setOrientation(framePose.getFrameOrientationCopy()); trajectoryPoint.setLinearVelocity(new Vector3d()); trajectoryPoint.setAngularVelocity(new Vector3d()); footTrajectoryControllerCommand.addTrajectoryPoint(trajectoryPoint); footTrajectoryControllerCommand.setRobotSide(userFootPoseSide.getEnumValue()); System.out.println("Submitting " + footTrajectoryControllerCommand); controllerCommandInputManager.submitCommand(footTrajectoryControllerCommand); userDoFootPose.set(false); } } });