/** * Extracts only the rotation part of a PoseType and converts it to a * PmRotationMatrix * * @param p Pose to convert * @return Rotation Vector from rotational component of pose. * @throws PmException if rotation vectors are invalid */ public static PmRotationVector toPmRotationVector(final PoseType p) throws PmException { return Posemath.toRot(toPmRotationMatrix(p)); }
/** * Extracts only the rotation part of a PoseType and converts it to a * roll-pitch-yaw * * @param p Pose to convert * @return Roll-pitch-yaw taken from rotational component of Pose * @throws PmException if rotation vectors are invalid */ public static PmRpy toPmRpy(PoseType p) throws PmException { return Posemath.toRpy(toPmRotationMatrix(p)); }
zyxout.y = 0; zyxout.x = 0; Posemath.pmZyxQuatConvert(zyxout, pout.rot); Posemath.pmQuatCartMult(pout.rot, ac, ac_out); Posemath.pmCartCartSub(bc, ac_out, pout.tran); pout.tran.z = (b2.z + b1.z - a1.z - a2.z) / 2.0; return pout;
public PoseType jointsToPose(double[] jv, PoseType _pose) throws PmException { PoseType pose = null; double[] sl = this.scaled_seglengths; PmSpherical sphereTran = new PmSpherical(Math.toRadians(jv[1]), Math.toRadians(jv[2] + 90.0), jv[0] * this.scale); PmCartesian endSeg = new PmCartesian(); Posemath.pmSphCartConvert(sphereTran, endSeg); PmRpy rpy = new PmRpy(Math.toRadians(jv[3]), Math.toRadians(jv[4]), Math.toRadians(jv[5])); PmRotationMatrix rmat = new PmRotationMatrix(); Posemath.pmRpyMatConvert(rpy, rmat); PmCartesian cart = endSeg.add(rmat.multiply(new PmCartesian(sl[0], 0, 0))); PmRotationVector rv = new PmRotationVector(); Posemath.pmMatRotConvert(rmat, rv); pose = CRCLPosemath.toPoseType(cart, rv, _pose); return pose; } private static final Logger LOG = Logger.getLogger(SimulatedKinematicsSimple.class.getName());
public double[] poseToJoints(double[] _jv, PoseType pose) throws PmException { double[] jv = _jv; if (null == jv || _jv.length != NUM_JOINTS) { jv = new double[NUM_JOINTS]; } double[] sl = this.scaled_seglengths; PmCartesian cart = CRCLPosemath.toPmCartesian(pose.getPoint()); // double r = cart.mag(); double endr = sl[0]; PmRotationMatrix rmat = CRCLPosemath.toPmRotationMatrix(pose); PmCartesian endSeg = cart.add(rmat.multiply(new PmCartesian(-endr, 0.0, 0.0))); PmSpherical sphereTran = new PmSpherical(); Posemath.pmCartSphConvert(endSeg, sphereTran); jv[0] = sphereTran.r / this.scale; jv[1] = Math.toDegrees(sphereTran.theta); jv[2] = Math.toDegrees(sphereTran.phi) - 90.0; PmRpy rpy = new PmRpy(); Posemath.pmMatRpyConvert(rmat, rpy); jv[3] = Math.toDegrees(rpy.r); jv[4] = Math.toDegrees(rpy.p); jv[5] = Math.toDegrees(rpy.y); return jv; }
PmRotationVector rv = Posemath.toRot(rotMat); PmRpy rpy = Posemath.toRpy(rotMat); Math.hypot(cmd.getEndPosition().getZAxis().getI(), cmd.getEndPosition().getZAxis().getJ())); PmEulerZyx zyx = new PmEulerZyx(); Posemath.pmMatZyxConvert(rotMat, zyx); double degreesRx = Math.toDegrees(zyx.x); double degreesRy = Math.toDegrees(zyx.y);
PmRotationMatrix mat = Posemath.toMat(zyx);//new PmRotationVector(rotMag, rx / rotMag, ry / rotMag, rz / rotMag); crclStatus.getPoseStatus().setPose(CRCLPosemath.toPoseType(cart, mat, crclStatus.getPoseStatus().getPose()));
public static PointType multiply(PoseType pose, PointType pt) throws CRCLException { try { PmCartesian cartOut = new PmCartesian(); Posemath.pmPoseCartMult(toPmPose(pose), toPmCartesian(pt), cartOut); return toPointType(cartOut); } catch (PmException pmException) { throw new CRCLException(pmException); } }
/** * Combine an rcslib Posemath translation and rotation(roll-pitch-yaw) in a * PoseType * * @param tran translational component of pose * @param v rotational component of pose in roll-pith-yaw format. * @param pose_in optional pose to be set instead of creating new Pose * @return new Pose creating from combining inputs or pose_in if not null * @throws PmException if rotation vector can not be converted to matrix */ static public PoseType toPoseType(PmCartesian tran, PmRpy v, /*@Nullable*/ PoseType pose_in) throws PmException { PoseType pose = pose_in; if (pose == null) { pose = new PoseType(); } pose.setPoint(toPointType(tran)); PmRotationMatrix mat = Posemath.toMat(v); VectorType xVec = new VectorType(); xVec.setI(mat.x.x); xVec.setJ(mat.x.y); xVec.setK(mat.x.z); pose.setXAxis(xVec); VectorType zVec = new VectorType(); zVec.setI(mat.z.x); zVec.setJ(mat.z.y); zVec.setK(mat.z.z); pose.setZAxis(zVec); return pose; }
PoseType pose = CRCLPosemath.toPoseType(cart, rcs.posemath.Posemath.toRot(rpy)); JbrPose jbrPose = new JbrPose("", index, pose); System.out.println("jbrPose = " + jbrPose);
public static Track toTrack(List<? extends PmPose> l) { if (null == l) { return null; } Track track = new Track(); track.setData(new ArrayList<TrackPoint>()); List<TrackPoint> data = track.getData(); boolean pmErrorOccurred = false; for (PmPose pose : l) { TrackPoint tp = new TrackPoint(); tp.x = pose.tran.x; tp.y = pose.tran.y; tp.z = pose.tran.z; if (!pmErrorOccurred) { try { tp.setRpy(Posemath.toRpy(pose.rot)); } catch (PmException ex) { pmErrorOccurred = true; Logger.getLogger(MainJFrame.class.getName()).log(Level.SEVERE, null, ex); } } data.add(tp); } return track; }
/** * Combine a translation and rotation in a PoseType * * @param tran translational component of pose * @param v rotational component of pose * @param pose_in optional pose to be set instead of creating new Pose * @return new Pose creating from combining inputs or pose_in if not null * @throws PmException if rotation vector can not be converted to matrix */ static public PoseType toPoseType(PmCartesian tran, PmRotationVector v, /*@Nullable*/ PoseType pose_in) throws PmException { PoseType pose = pose_in; if (pose == null) { pose = new PoseType(); } pose.setPoint(toPointType(tran)); PmRotationMatrix mat = Posemath.toMat(v); VectorType xVec = new VectorType(); xVec.setI(mat.x.x); xVec.setJ(mat.x.y); xVec.setK(mat.x.z); pose.setXAxis(xVec); VectorType zVec = new VectorType(); zVec.setI(mat.z.x); zVec.setJ(mat.z.y); zVec.setK(mat.z.z); pose.setZAxis(zVec); return pose; }
/** * Compute the magnitude of a rotation vector between the two poses in * radians. * * @param pose1 first pose to compare * @param pose2 second pose to compare * @return magnitude of rotation between poses. * @throws PmException if either pose has an invalid rotation. */ public static double diffPosesRot(PoseType pose1, PoseType pose2) throws PmException { PmRotationMatrix m1 = toPmRotationMatrix(pose1); PmRotationMatrix m2 = toPmRotationMatrix(pose2); PmRotationVector r = Posemath.toRot(m1.multiply(m2.inv())); return r.s; }
PmRpy rpy2 = Posemath.toRpy(rv); System.out.println("rpy2 = " + rpy2);
PoseType pose = CRCLPosemath.toPoseType(posxyzwpr.cart, Posemath.toRot(posxyzwpr.rpy)); if (parts[2].endsWith("mm/sec")) { SetTransSpeedType setSpeedCmd = new SetTransSpeedType();
PmCartesian cart = new PmCartesian(pos.x() / lengthScale, pos.y() / lengthScale, pos.z() / lengthScale); PmRpy rpy = new PmRpy(Math.toRadians(pos.w()), Math.toRadians(pos.p()), Math.toRadians(pos.r())); setPose(CRCLPosemath.toPoseType(cart, rcs.posemath.Posemath.toRot(rpy), getPose())); Com4jObject com4jobj_joint_pos = icgp.formats(FRETypeCodeConstants.frJoint); IJoint joint_pos = com4jobj_joint_pos.queryInterface(IJoint.class);