private void setOrientationFromPoseToPose(YoFrameOrientation frameOrientationToPack, FramePose fromPose, FramePose toPose) { FrameOrientation toOrientation = toPose.getFrameOrientationCopy(); FrameOrientation fromOrientation = fromPose.getFrameOrientationCopy(); frameOrientationToPack.getFrameOrientation().setOrientationFromOneToTwo(fromOrientation, toOrientation); }
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); 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 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); // 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 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); } } });
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(); } }
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); } } });
trajectoryPoint.setTime(userDesiredHandPoseTrajectoryTime.getDoubleValue()); trajectoryPoint.setPosition(framePose.getFramePointCopy()); trajectoryPoint.setOrientation(framePose.getFrameOrientationCopy()); trajectoryPoint.setLinearVelocity(new Vector3d()); trajectoryPoint.setAngularVelocity(new Vector3d());