public void setYaw(double yaw) { if(null == rpy) { rpy = new PmRpy(); } rpy.y = yaw; }
public void setRoll(double roll) { if(null == rpy) { rpy = new PmRpy(); } rpy.r = roll; }
public void setPitch(double pitch) { if(null == rpy) { rpy = new PmRpy(); } rpy.p = pitch; }
public PmXyzWpr() { cart = new PmCartesian(); rpy = new PmRpy(); }
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); }
PmRpy rpy = new PmRpy(Math.toRadians(Double.parseDouble(fa[5])), Math.toRadians(Double.parseDouble(fa[4])), Math.toRadians(Double.parseDouble(fa[3])+Math.PI)); PoseType pose = CRCLPosemath.toPoseType(cart, rcs.posemath.Posemath.toRot(rpy)); JbrPose jbrPose = new JbrPose("", index, pose);
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 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; }
public static Track getTest1Track() { // this.view3DPlotJPanel1.clear(); Track t = new Track(); double x = 0; double y = 0; double z = 0; PmRpy rpy = new PmRpy(); t.setData(new ArrayList<>()); for (; x < 2.0; x += 0.25) { t.getData().add(new TrackPoint(x, y, z, rpy)); } for (; y < 2.0; y += 0.25) { t.getData().add(new TrackPoint(x, y, z, rpy)); } for (; z < 2.0; z += 0.25) { t.getData().add(new TrackPoint(x, y, z, rpy)); } for (; y > 0.0; y -= 0.25) { t.getData().add(new TrackPoint(x, y, z, rpy)); } for (; z < 6.0; z += 0.25) { rpy.y = 2.0 * Math.PI * (z - 2.0) / 3.0; x = 2.0 * Math.cos(rpy.y); y = 2.0 * Math.sin(rpy.y); t.getData().add(new TrackPoint(x, y, z, rpy)); } t.cur_time_index = t.getData().size(); return t; }
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());
IXyzWpr pos = com4jobj_pos.queryInterface(IXyzWpr.class); 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);