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 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;
}