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());
/** * 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; }
double cry = Math.cos(Math.toRadians(ry)); double crz = Math.cos(Math.toRadians(rz)); PmRotationMatrix mat2 = new PmRotationMatrix( Math.signum(cry*crz)*Math.sqrt(1 - srz*cry*srz*cry-sry*crz*sry*crz), srz*cry,sry*crx, srz*crx,Math.signum(crx*crz)*Math.sqrt(1 - srz*crx*srz*crx-srx*crz*srx*crz),sry*crz,
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; }
/** * Extracts only the rotation part of a PoseType and converts it to a * PmRotationMatrix * * @param p Pose to be converted * @return Rotation matrix from rotational component of pose * @throws PmException if rotation vectors are invalid */ public static PmRotationMatrix toPmRotationMatrix(PoseType p) throws PmException { PmCartesian cx = vectorToPmCartesian(p.getXAxis()); PmCartesian cz = vectorToPmCartesian(p.getZAxis()); //PmCartesian cy = Posemath.cross(cz, cx); PmCartesian cy = vectorToPmCartesian(cross(p.getZAxis(), p.getXAxis())); return new PmRotationMatrix(cx, cy, cz); }