public void saveProperties() { if (null != this.propertiesFile) { Properties props = new Properties(); props.put("keepMoveToLog", Boolean.valueOf(keepMoveToLog)); try (FileWriter fw = new FileWriter(propertiesFile)) { props.store(fw, ""); } catch (IOException exception) { exception.printStackTrace(); } } saveCartLimits(new PmCartesian(xMin, yMin, zMin), new PmCartesian(xMax, yMax, zMax)); saveJointLimits(this.lowerJointLimits, this.upperJointLimits); }
/** * 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 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 PmXyzWpr() { cart = new PmCartesian(); rpy = new PmRpy(); }
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 double distTransFrom(PoseType pose) { PmCartesian cart = CRCLPosemath.toPmCartesian(CRCLPosemath.getPoint(status)); return cart.distFrom(CRCLPosemath.toPmCartesian(pose.getPoint())); }
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); }
private void updateCartLimits(boolean force) { if (force || this.jCheckBoxEditCartesianLimits.isSelected()) { PmCartesian min = new PmCartesian(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY); PmCartesian max = new PmCartesian(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); TableModel model = this.jTableCartesianLimits.getModel(); min.x = Double.parseDouble(model.getValueAt(0, 1).toString()); max.x = Double.parseDouble(model.getValueAt(0, 3).toString()); min.y = Double.parseDouble(model.getValueAt(1, 1).toString()); max.y = Double.parseDouble(model.getValueAt(1, 3).toString()); min.z = Double.parseDouble(model.getValueAt(2, 1).toString()); max.z = Double.parseDouble(model.getValueAt(2, 3).toString()); if (null != main) { main.applyAdditionalCartLimits(min, max); main.saveCartLimits(min, max); } else { logError("Can not set cart limits: main is null."); } } }
public void readAndApplyUserCartLimits() { PmCartesian min = new PmCartesian(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY); PmCartesian max = new PmCartesian(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); if (null != cartLimitsFile && cartLimitsFile.exists()) { try (BufferedReader br = new BufferedReader(new FileReader(cartLimitsFile))) { String line = null; while ((line = br.readLine()) != null) { findString(line, "min.x=", t -> min.x = Double.parseDouble(t)); findString(line, "max.x=", t -> max.x = Double.parseDouble(t)); findString(line, "min.y=", t -> min.y = Double.parseDouble(t)); findString(line, "max.y=", t -> max.y = Double.parseDouble(t)); findString(line, "min.z=", t -> min.z = Double.parseDouble(t)); findString(line, "max.z=", t -> max.z = Double.parseDouble(t)); } } catch (IOException ex) { Logger.getLogger(FanucCRCLMain.class.getName()).log(Level.SEVERE, null, ex); } } applyAdditionalCartLimits(min, max); }
/** * 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()); }
System.out.println("fa = " + Arrays.toString(fa)); PmCartesian cart = new PmCartesian(Double.parseDouble(fa[0]), Double.parseDouble(fa[1]), Double.parseDouble(fa[2])); 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));
/** * 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 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()); }
PmCartesian ac = new PmCartesian(); ac.x = (a1.x + a2.x) * 0.5; ac.y = (a1.y + a2.y) * 0.5; PmCartesian bc = new PmCartesian(); bc.x = (b1.x + b2.x) * 0.5; bc.y = (b1.y + b2.y) * 0.5; bc.z = 0; PmCartesian ac_out = new PmCartesian();
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); } }
IConfig conf = curXyzWpr.config(); confText = conf.text(); cartPos = new PmCartesian(curXyzWpr.x(), curXyzWpr.y(), curXyzWpr.z());
PointType moveCmdEndPt = moveCmd.getEndPosition().getPoint(); PmCartesian cart = CRCLPosemath.toPmCartesian(moveCmdEndPt); PmCartesian endCart = new PmCartesian(cart.x * lengthScale, cart.y * lengthScale, cart.z * lengthScale); PmRpy rpy = CRCLPosemath.toPmRpy(moveCmd.getEndPosition()); updatePosReg98();
PmCartesian cart = new PmCartesian(posXyzWpr.x(), posXyzWpr.y(), posXyzWpr.z()); float xMinEffective = xMin + border1; float xMaxEffective = xMax - border1; PmCartesian newcart = new PmCartesian(posXyzWpr.x(), posXyzWpr.y(), posXyzWpr.z());
PmCartesian cart = new PmCartesian(pos.x() / lengthScale, pos.y() / lengthScale, pos.z() / lengthScale); PmEulerZyx zyx = new PmEulerZyx(Math.toRadians(pos.rz()), Math.toRadians(pos.ry()), Math.toRadians(pos.rx()));
Com4jObject com4jobj_pos = icgp.formats(FRETypeCodeConstants.frXyzWpr); 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()));