cleaning

src/main/java/com/marginallyclever/ro3/node/nodes/odenode/Brain.java

@@ -13,7 +13,9 @@ public class Brain {
     private final List<Matrix4d> matrices = new ArrayList<>();
     // build a list of contacts for each body
     private final List<Boolean> isTouching = new ArrayList<>();
-    private final static int memoryLength = 30*3;  // fps * seconds
+    public static int FPS = 30;
+    public static int MEMORY_SECONDS = 3;
+    private final static int MEMORY_FRAMES = FPS * MEMORY_SECONDS;  // fps * seconds
     private int k=0;
 
     private final List<Neuron> neurons = new ArrayList<>();
@@ -34,9 +36,10 @@ public void setNumInputs(int size) {
             matrices.add(new Matrix4d());
         }
 
-        // every matrix is 4 inputs
-        // every is touching is 1 input.
-        image = new BufferedImage(size * 5, memoryLength, BufferedImage.TYPE_INT_ARGB);
+        // every matrix is 12 inputs and can be expressed as 4 RGB pixels with 3 colors each.
+        // every is touching is 1 input.  use a single color chanel of each pixel.
+        // force data from the hinges is 1 matrix each.
+        image = new BufferedImage(size * 5, MEMORY_FRAMES, BufferedImage.TYPE_INT_RGB);
         eraseBrainScan();
     }
 
@@ -65,23 +68,28 @@ void update(double dt) {
 
         // move every row of the image down one.
         updateImage();
+        // write new data to the top row.
         writeInputsToTopLineOfImage();
+
+        // run the neural network
+        for(Neuron n : neurons) {
+            n.update(dt);
+        }
     }
 
     private void writeInputsToTopLineOfImage() {
         // every matrix value is copied to a pixel in the first row of the image.
         // every isTouching value is copied to a pixel in the first row of the image.
         k = 0;
-        int i=0;
-        for (Matrix4d m : matrices) {
+        for(Matrix4d m : matrices) {
             addPixelToTopRow(m.m00, m.m10, m.m20, 1, -1);
             addPixelToTopRow(m.m01, m.m11, m.m21, 1, -1);
             addPixelToTopRow(m.m02, m.m12, m.m22, 1, -1);
-            addPixelToTopRow(m.m03, m.m13, m.m23, 20, -20);
+            addPixelToTopRow(m.m03, m.m13, m.m23, 1, -1);
         }
 
-        for (Matrix4d m : matrices) {
-            image.setRGB(k++, 0, isTouching.get(i++) ? 0xffff0000:0 );
+        for(int i=0; i<matrices.size();++i) {
+            image.setRGB(k++, 0, isTouching.get(i++) ? 0xff0000:0 );
         }
     }
 
@@ -90,7 +98,7 @@ private void addPixelToTopRow(double x,double y,double z,int max,int min) {
         int r = (int)( (Math.max(Math.min(x,max),min) - min) * f );
         int g = (int)( (Math.max(Math.min(y,max),min) - min) * f );
         int b = (int)( (Math.max(Math.min(z,max),min) - min) * f );
-        image.setRGB(k++, 0, 0xff000000 | (g << 16) | (r << 8) | b);
+        image.setRGB(k++, 0, (r << 16) | (g << 8) | b);
     }
 
     public BufferedImage getImage() {
@@ -100,7 +108,7 @@ public BufferedImage getImage() {
     // move every row of the image down one.
     void updateImage() {
         // Copy each row from the original image to itself, one row lower.  start from the bottom.
-        for (int y=memoryLength-1;y>0;--y) {
+        for (int y = MEMORY_FRAMES -1; y>0; --y) {
             for (int x=0;x<image.getWidth();++x) {
                 image.setRGB(x,y,image.getRGB(x,y-1));
             }

src/main/java/com/marginallyclever/ro3/node/nodes/odenode/CreatureController.java

@@ -6,8 +6,11 @@
 import com.marginallyclever.ro3.physics.CollisionListener;
 import org.ode4j.ode.DContact;
 import org.ode4j.ode.DGeom;
+import org.ode4j.ode.DJoint;
 
 import javax.swing.*;
+import javax.vecmath.Matrix4d;
+import javax.vecmath.Vector3d;
 import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.List;
@@ -22,6 +25,10 @@ public class CreatureController extends ODENode implements CollisionListener {
     private final List<ODEHinge> hinges = new ArrayList<>();
     private final List<ODEBody> bodies = new ArrayList<>();
     private final Brain brain = new Brain();
+    // max experienced during simulation
+    private double maxForce = 0;
+    // max experienced during simulation
+    private double maxTorque = 0;
 
     public CreatureController() {
         super("CreatureController");
@@ -99,49 +106,111 @@ protected void onFirstUpdate() {
         hinges.addAll(findHinges());
         bodies.clear();
         bodies.addAll(findBodies());
-        brain.setNumInputs(bodies.size()+1);
+        brain.setNumInputs(bodies.size()+hinges.size()+1);
+
+        // add feedback to hinges
+        for(ODEHinge h : hinges) {
+            h.getHinge().setFeedback(new DJoint.DJointFeedback());
+        }
     }
 
     @Override
     public void update(double dt) {
         super.update(dt);
 
-        // any bodies that are marked isTouching must be because onCollision says so.
+        sendSensoryInputToBrain(dt);
+
+        // perform magic
+        brain.update(dt);
+
+        sendBrainOutputToHinges();
+
+        // reset the isTouching flag on all bodies
+        for (ODEBody b : bodies) {
+            b.setTouchingSomething(false);
+        }
+    }
 
+    private void sendSensoryInputToBrain(double dt) {
         // get the matrix for the torso, probably always bodies[0]?  Not guaranteed.
         var torsoMatrix = bodies.get(0).getWorld();
-        torsoMatrix.invert();
+        var iTorso = new Matrix4d();
+        iTorso.invert(torsoMatrix);
         // multiply all brain matrices by the inverse of torsoMatrix
+        var t = new Vector3d();
         int i=0;
         for (ODEBody b : bodies) {
-            // I'm concerned this is not the correct matrix.
-            // the translation values are not normalized.
             var m = b.getWorld();
-            m.mul(torsoMatrix);
-            brain.setMatrix(i,m);
-            brain.setTouching(i,b.isTouchingSomething());
-            System.out.print(b.isTouchingSomething()?"1":"0");
-            ++i;
+            // normalize rotation.
+            m.mul(iTorso);
+            // normalize translation
+            m.get(t);
+            t.scale(0.1);
+            m.setTranslation(t);
+            // set to brain
+            brain.setMatrix(i++, m);
         }
-        System.out.println();
 
+        // any bodies that are marked isTouching must be because onCollision says so.
+        // onCollision happens before update, so this is the right place to check the flag.
+        // add the isTouching flag to the brain sensory input
+        i=0;
+        for (ODEBody b : bodies) {
+            brain.setTouching(i++,b.isTouchingSomething());
+        }
+
+        // add the hinge feedback to the brain sensory input
+        Matrix4d hm = new Matrix4d();
+        for( ODEHinge hinge : hinges) {
+            var internalHinge = hinge.getHinge();
+            if(internalHinge==null) {
+                hm.setIdentity();
+            } else {
+                convertHingeFeedbackToMatrix(internalHinge.getFeedback(),hm);
+            }
+            brain.setMatrix(i++,hm);
+        }
+
+        // add the torso matrix.  Good for world up, world north, height above flat ground.
         brain.setMatrix(i,torsoMatrix);
 
-        // I have all the input for the robot dog, normalized.
-        // some magic happens here
-        brain.update(dt);
+        //System.out.println("f"+fmax+" t"+tmax);
+    }
 
+    private void convertHingeFeedbackToMatrix(DJoint.DJointFeedback feedback, Matrix4d hm) {
+        hm.m00 = calcMaxF(feedback.f1.get0());
+        hm.m10 = calcMaxF(feedback.f1.get1());
+        hm.m20 = calcMaxF(feedback.f1.get2());
+        hm.m01 = calcMaxT(feedback.t1.get0());
+        hm.m11 = calcMaxT(feedback.t1.get1());
+        hm.m21 = calcMaxT(feedback.t1.get2());
+        hm.m02 = calcMaxF(feedback.f2.get0());
+        hm.m12 = calcMaxF(feedback.f2.get1());
+        hm.m22 = calcMaxF(feedback.f2.get2());
+        hm.m03 = calcMaxT(feedback.t2.get0());
+        hm.m13 = calcMaxT(feedback.t2.get1());
+        hm.m23 = calcMaxT(feedback.t2.get2());
+    }
+
+    private double calcMaxF(double f22) {
+        maxForce = Math.max(maxForce,Math.abs(f22));
+        return maxForce >0 ? (f22/ maxForce) : f22;
+    }
+
+    private double calcMaxT(double t22) {
+        maxTorque = Math.max(maxTorque,Math.abs(t22));
+        return maxTorque >0 ? (t22/ maxTorque) : t22;
+    }
+
+    private void sendBrainOutputToHinges() {
         // I want the system to output torque values for each hinge.
-        i=0;
+        int i=0;
         for (ODEHinge h : hinges) {
-            h.addTorque(brain.getOutput(i));
-            ++i;
-        }
-
-        // reset the isTouching flag on all bodies
-        for (ODEBody b : bodies) {
-            b.setTouchingSomething(false);
+            var force = brain.getOutput(i++);
+            //System.out.print(force+"\t");
+            h.addTorque(force * maxTorque);  // 2.5e5 = 250k
         }
+        //System.out.println();
     }
 
     public List<ODEHinge> getHinges() {