public PmXyzWpr() { cart = new PmCartesian(); rpy = new PmRpy(); }
public void setYaw(double yaw) { if(null == rpy) { rpy = new PmRpy(); } rpy.y = yaw; }
public static PoseType toPose(PmPose pose) throws PmException { PmHomogeneous hom = new PmHomogeneous(); Posemath.pmPoseHomConvert(pose, hom); return toPose(hom.toMatdd()); }
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; }
/** * 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; }
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); } }
/** * Convert the crcl.VectorType to a PmCartesian. crcl.VectorType is * generally used as a unit vector for rotation. * * @param v Vector to convert * @return PmCartesian equivalent of v */ public static PmCartesian vectorToPmCartesian(VectorType v) { return new PmCartesian(v.getI(), v.getJ(), v.getK()); }
/** * 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)); }
/** * Compute the cartesian distance between two points. * * @param pt1 first Point * @param pt2 second Point * @return distance between pt1 and pt2 */ public static double diffPoints(PointType pt1, PointType pt2) { return toPmCartesian(pt1).distFrom(toPmCartesian(pt2)); }
public double distRotFrom(PoseType pose) throws PmException { PmRotationVector rotvCurrent = CRCLPosemath.toPmRotationVector(getPose()); PmRotationVector rotvArg = CRCLPosemath.toPmRotationVector(pose); PmRotationVector rotvDiff = rotvArg.multiply(rotvCurrent.inv()); return Math.toDegrees(rotvDiff.s); }
public static PmPose toPmPose(CRCLStatusType stat) throws CRCLException { if (stat == null) { throw new IllegalArgumentException("Can not convert null status to PmPose"); } try { PoseType pose = getPose(stat); if (pose != null) { PointType pt = getPoint(stat); if (null != pt) { PmCartesian cart = toPmCartesian(pt); PmRotationMatrix mat = toPmRotationMatrix(pose); if (null != mat) { return new PmPose(cart, new PmQuaternion(mat)); } } } } catch (PmException pmException) { throw new CRCLException(pmException); } throw new IllegalArgumentException("stat has null pose components"); }
public PmXyzWpr(double x, double y, double z, double w, double p, double r) { rpy = new PmRpy(r, p, w); cart = new PmCartesian(x, y, z); }
public void setRoll(double roll) { if(null == rpy) { rpy = new PmRpy(); } rpy.r = roll; }
/** * Convert crcl.PointType to rcs.posemath.PmCartesian * * @param pt Point to be converted * @return PmCartesian equivalent */ public static PmCartesian toPmCartesian(final PointType pt) { return new PmCartesian( pt.getX(), pt.getY(), pt.getZ()); }
public double distTransFrom(PoseType pose) { PmCartesian cart = CRCLPosemath.toPmCartesian(CRCLPosemath.getPoint(status)); return cart.distFrom(CRCLPosemath.toPmCartesian(pose.getPoint())); }
public PmXyzWpr(IXyzWpr xyzwpr) { rpy = new PmRpy(Math.toRadians(xyzwpr.w()), Math.toRadians(xyzwpr.p()), Math.toRadians(xyzwpr.r())); cart = new PmCartesian(xyzwpr.x(), xyzwpr.y(), xyzwpr.z()); }
public void setPitch(double pitch) { if(null == rpy) { rpy = new PmRpy(); } rpy.p = pitch; }