documenting

src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java

@@ -8,14 +8,14 @@
 
 import javax.swing.*;
 import javax.swing.text.NumberFormatter;
-import javax.vecmath.Matrix3d;
-import javax.vecmath.Matrix4d;
 import java.awt.*;
 import java.text.NumberFormat;
 import java.util.List;
 
 /**
- * {@link HingeJoint} is a joint that can rotate around the local Z axis.
+ * <p>a {@link HingeJoint} is a joint that can rotate around the local Z axis.</p>
+ * <p>a {@link HingeJoint} should be attached to a child {@link Pose} referenced as the axle.  In this way the axle's
+ * parent {@link Pose} can be thought of as the initial pose at zero degrees.  This helps prevent drift over time.</p>
  */
 public class HingeJoint extends Node {
     private double angle = 0;  // degrees
@@ -101,11 +101,7 @@ public void update(double dt) {
 
         if(axle!=null) {
             // set the axle's location in space.
-            Matrix3d rotZ = new Matrix3d();
-            rotZ.rotZ(Math.toRadians(angle));
-            Matrix4d local = new Matrix4d(axle.getLocal());
-            local.set(rotZ);
-            axle.setLocal(local);
+            axle.getLocal().rotZ(Math.toRadians(angle));
         }
     }
 

src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java

@@ -11,7 +11,8 @@
 import java.util.List;
 
 /**
- * a {@link Motor} is a {@link Node} that can be attached to a {@link HingeJoint}.
+ * A {@link Motor} is a {@link Node} that can be attached to a {@link HingeJoint}.  It will then drive the joint
+ * according to the motor's settings.
  */
 public class Motor extends Node {
     private HingeJoint hinge;

src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java

@@ -372,7 +372,7 @@ public void moveEndEffectorInCartesianDirection(double[] cartesianVelocity) {
         double sum = sumCartesianVelocityComponents(cartesianVelocity);
         if(sum<0.0001) return;
         if(sum <= 1) {
-            moveEndEffectorInCartesianDirectionALittle(cartesianVelocity);
+            setMotorVelocitiesFromCartesianVelocity(cartesianVelocity);
             return;
         }
 
@@ -382,45 +382,21 @@ public void moveEndEffectorInCartesianDirection(double[] cartesianVelocity) {
         double[] cartesianVelocityUnit = Arrays.stream(cartesianVelocity)
                 .map(v -> v / total)
                 .toArray();
-        /*
-        // option 1, set joints directly.
-        for (int i = 0; i < total; ++i) {
-            moveEndEffectorInCartesianDirectionALittle(cartesianVelocityUnit);
-        }
-        */
-        // option 2, set joint velocities.
-        setJointVelocitiesFromCartesianVelocity(cartesianVelocityUnit);
+        // set motor velocities.
+        setMotorVelocitiesFromCartesianVelocity(cartesianVelocityUnit);
     }
 
     /**
-     * <p>Attempts to move the robot arm such that the end effector travels in the direction of the cartesian
-     * velocity.</p>
-     * <p>This method will only move the robot arm a small amount.  It is intended to be called repeatedly to move the
-     * arm in an iterative fashion.</p>
+     * <p>Attempts to move the robot arm such that the end effector travels in the cartesian direction.  This is
+     * achieved by setting the velocity of the motors.</p>
      * @param cartesianVelocity three linear forces (mm) and three angular forces (degrees).
      * @throws RuntimeException if the robot cannot be moved in the direction of the cartesian force.
      */
-    private void moveEndEffectorInCartesianDirectionALittle(double[] cartesianVelocity) {
+    private void setMotorVelocitiesFromCartesianVelocity(double[] cartesianVelocity) {
         ApproximateJacobian aj = getJacobian();
         try {
-            double[] jointVelocity = aj.getJointForceFromCartesianForce(cartesianVelocity);  // uses inverse jacobian
-            // do not make moves for impossible velocities
+            double[] jointVelocity = aj.getJointFromCartesian(cartesianVelocity);  // uses inverse jacobian
             if(impossibleVelocity(jointVelocity)) return;  // TODO: throw exception instead?
-
-            double[] angles = getAllJointAngles();  // # dof long
-            for (int i = 0; i < angles.length; ++i) {
-                angles[i] += jointVelocity[i];
-            }
-            setAllJointAngles(angles);
-        } catch (Exception e) {
-            throw new RuntimeException(e);
-        }
-    }
-
-    private void setJointVelocitiesFromCartesianVelocity(double[] cartesianVelocity) {
-        ApproximateJacobian aj = getJacobian();
-        try {
-            double[] jointVelocity = aj.getJointForceFromCartesianForce(cartesianVelocity);  // uses inverse jacobian
             setAllJointVelocities(jointVelocity);
         } catch (Exception e) {
             throw new RuntimeException(e);

src/main/java/com/marginallyclever/ro3/render/Viewport.java

@@ -15,6 +15,7 @@
 import javax.vecmath.Vector3d;
 import java.awt.*;
 import java.awt.event.MouseEvent;
+import java.awt.event.MouseWheelEvent;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -187,9 +188,10 @@ private void renderAllPasses() {
     }
 
     private void updateAllNodes(double dt) {
+        // option 1, recursively
         Registry.getScene().update(dt);
         /*
-        // linear way, which is less recursive.
+        // option 2, in a linear way.
         List<Node> toScan = new ArrayList<>(Registry.getScene().getChildren());
         while(!toScan.isEmpty()) {
             Node node = toScan.remove(0);
@@ -229,6 +231,12 @@ public void mouseDragged(MouseEvent e) {
         }
     }
 
+    @Override
+    public void mouseWheelMoved(MouseWheelEvent e) {
+        super.mouseWheelMoved(e);
+        int dz = e.getWheelRotation();
+    }
+
     private void panTiltCamera(int dx, int dy) {
         Matrix4d local = camera.getLocal();
         double [] panTiltAngles = getPanTiltFromMatrix(local);

src/main/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/ApproximateJacobian.java

@@ -21,24 +21,6 @@ protected ApproximateJacobian(int DOF) {
         jacobian = new double[6][DOF];
     }
 
-    /**
-     * Use the jacobian to get the cartesian velocity from the joint velocity.
-     * @param jointForce joint velocity in degrees.
-     * @return 6 doubles containing the XYZ translation and UVW rotation forces on the end effector.
-     */
-    public double[] getCartesianForceFromJointForce(final double[] jointForce) {
-        // vector-matrix multiplication (y = x^T A)
-        double[] cartesianVelocity = new double[DOF];
-        for (int j = 0; j < DOF; ++j) {
-            double sum = 0;
-            for (int k = 0; k < 6; ++k) {
-                sum += jacobian[k][j] * Math.toRadians(jointForce[j]);
-            }
-            cartesianVelocity[j] = sum;
-        }
-        return cartesianVelocity;
-    }
-
     /**
      * See <a href="https://stackoverflow.com/a/53028167/1159440">5 DOF Inverse kinematics for Jacobian Matrices</a>.
      * @return the inverse Jacobian matrix.
@@ -94,10 +76,11 @@ private double[][] getInverseDampedLeastSquares(double lambda) {
     /**
      * Use the Jacobian to get the joint velocity from the cartesian velocity.
      * @param cartesianVelocity 6 doubles - the XYZ translation and UVW rotation forces on the end effector.
-     * @return jointVelocity joint velocity in degrees. Will be filled with the new velocity.
+     *                          The rotation component is in radians.
+     * @return joint velocity in degrees.  Will be filled with the new velocity.
      * @throws Exception if joint velocities have NaN values
      */
-    public double[] getJointForceFromCartesianForce(final double[] cartesianVelocity) throws Exception {
+    public double[] getJointFromCartesian(final double[] cartesianVelocity) throws Exception {
         double[][] inverseJacobian = getInverseJacobian();
         double[] jointVelocity = new double[DOF];
 
@@ -116,6 +99,25 @@ public double[] getJointForceFromCartesianForce(final double[] cartesianVelocity
         return jointVelocity;
     }
 
+    /**
+     * Use the jacobian to convert joint velocity to cartesian velocity.
+     * @param joint joint velocity in degrees.
+     * @return 6 doubles containing the XYZ translation and UVW rotation forces on the end effector.
+     * The rotation component is in radians.
+     */
+    public double[] getCartesianFromJoint(final double[] joint) {
+        // vector-matrix multiplication (y = x^T A)
+        double[] cartesianVelocity = new double[DOF];
+        for (int j = 0; j < DOF; ++j) {
+            double sum = 0;
+            for (int k = 0; k < 6; ++k) {
+                sum += jacobian[k][j] * Math.toRadians(joint[j]);
+            }
+            cartesianVelocity[j] = sum;
+        }
+        return cartesianVelocity;
+    }
+
     public double[][] getJacobian() {
         return jacobian;
     }

src/main/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/RobotArmSystem.java

@@ -229,7 +229,7 @@ private void applySmallCartesianForceToEndEffector(RobotComponent robotComponent
         ApproximateJacobian aj = new ApproximateJacobianFiniteDifferences(robotComponent);
         //ApproximateJacobian aj = new ApproximateJacobianScrewTheory(robotComponent);
         try {
-            double[] jointVelocity = aj.getJointForceFromCartesianForce(cartesianVelocity);  // uses inverse jacobian
+            double[] jointVelocity = aj.getJointFromCartesian(cartesianVelocity);  // uses inverse jacobian
             // do not make moves for impossible velocities
             if(impossibleVelocity(robotComponent,jointVelocity)) return;
 

src/test/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/ApproximateJacobianTest.java

@@ -130,8 +130,8 @@ public void compare() throws Exception {
         }
 
         double [] v = new double[] {1,2,3,4,5,6};
-        double [] vFinite = finite.getJointForceFromCartesianForce(v);
-        double [] vScrew = screw.getJointForceFromCartesianForce(v);
+        double [] vFinite = finite.getJointFromCartesian(v);
+        double [] vScrew = screw.getJointFromCartesian(v);
         System.out.println(Arrays.toString(vFinite));
         System.out.println(Arrays.toString(vScrew));
         for(int i=0;i<vFinite.length;++i) {