greatly improved removeGripperAndJawsFromHits()

src/main/java/com/marginallyclever/robotoverlord/components/GripperComponentAbstract.java

@@ -10,7 +10,8 @@
 import java.util.List;
 
 /**
- * A gripper is a component that can grab and hold things.
+ * {@link GripperComponentAbstract} is a gripper that can grab and hold things.  It should have child entities of
+ * type {@link GripperComponentJaw}.
  * @author Dan Royer
  * @since 2.11.0
  */

src/main/java/com/marginallyclever/robotoverlord/components/GripperComponentJaw.java

@@ -1,25 +1,23 @@
 package com.marginallyclever.robotoverlord.components;
 
-import com.jogamp.opengl.GL3;
-import com.marginallyclever.convenience.helpers.MatrixHelper;
 import com.marginallyclever.robotoverlord.SerializationContext;
-import com.marginallyclever.robotoverlord.entity.Entity;
+import com.marginallyclever.robotoverlord.parameters.BooleanParameter;
 import com.marginallyclever.robotoverlord.parameters.DoubleParameter;
 import org.json.JSONObject;
 
-import javax.vecmath.Matrix4d;
-import javax.vecmath.Vector3d;
-import java.util.List;
-
 /**
- * Identifier so that systems can find each child jaw of a gripper.
+ * {@link GripperComponentJaw} should be a child of {@link GripperComponentLinear}. or {@link GripperComponentRotary}.
+ * Each instance represents a single jaw.
+ * @since 2.11.0
+ * @author Dan Royer
  */
 public class GripperComponentJaw extends Component {
-    public DoubleParameter openDistance = new DoubleParameter("Open Distance",5.0);
-    public DoubleParameter closeDistance = new DoubleParameter("Close Distance",1.0);
+    public final DoubleParameter openDistance = new DoubleParameter("Open Distance",5.0);
+    public final DoubleParameter closeDistance = new DoubleParameter("Close Distance",1.0);
 
     @Override
     public JSONObject toJSON(SerializationContext context) {
+        super.toJSON(context);
         JSONObject jo = super.toJSON(context);
         jo.put("openDistance",openDistance.toJSON(context));
         jo.put("closeDistance",closeDistance.toJSON(context));
@@ -28,8 +26,13 @@ public JSONObject toJSON(SerializationContext context) {
 
     @Override
     public void parseJSON(JSONObject json, SerializationContext context) {
+        super.parseJSON(json,context);
         openDistance.parseJSON(json.getJSONObject("openDistance"),context);
         if(json.has("closeDistance")) closeDistance.parseJSON(json.getJSONObject("closeDistance"),context);
         super.parseJSON(json,context);
     }
+
+    public double getDistance() {
+        return openDistance.get() - closeDistance.get();
+    }
 }

src/main/java/com/marginallyclever/robotoverlord/components/GripperComponentLinear.java

@@ -14,7 +14,9 @@
 import java.util.List;
 
 /**
- * A gripper is a component that can grab and hold things.  The jaws move in a straight line.
+ * {@link GripperComponentLinear} is a gripper.  The jaws move in a straight line along their local Z axis.
+ * Each moving Jaw should be positioned at the jaw's contact point, with the contact Z axis pointing in the
+ * direction of the jaw's closing motion.  This axis is then used to detect collisions.
  * @author Dan Royer
  * @since 2.6.1
  */
@@ -23,19 +25,6 @@ public GripperComponentLinear() {
         super();
     }
 
-    /**
-     * @return the center of the {@link GripperComponentJaw} child entities in world space.
-     */
-    public List<Point3d> getPoints() {
-        List<Entity> children = getEntity().getChildren();
-        List<Point3d> results = new ArrayList<>();
-        for(Entity child : children) {
-            if(child.getComponent(GripperComponentJaw.class)==null) continue;
-            Matrix4d pose = child.getComponent(PoseComponent.class).getWorld();
-            results.add(new Point3d(MatrixHelper.getPosition(pose)));
-        }
-        return results;
-    }
 
     @Override
     public void render(GL3 gl) {

src/main/java/com/marginallyclever/robotoverlord/components/GripperComponentRotary.java

@@ -15,7 +15,10 @@
 import java.util.List;
 
 /**
- * A gripper is a component that can grab and hold things.  The jaws rotate around a point.
+ * {@link GripperComponentRotary} is a {@link GripperComponentAbstract}.  The jaws rotate around their local Z axis.
+ * The jaws are defined by a {@link DoubleParameter} for the open distance and a {@link DoubleParameter} for the close
+ * distance.  Each moving Jaw should also have an {@link Entity} positioned at the jaw's contact point, with the contact
+ * point Z axis pointing in the direction of the jaw's closing motion.  This axis is then used to detect collisions.
  * @author Dan Royer
  * @since 2.11.0
  */
@@ -24,20 +27,6 @@ public GripperComponentRotary() {
         super();
     }
 
-    /**
-     * @return the center of the {@link GripperComponentJaw} child entities in world space.
-     */
-    public List<Point3d> getPoints() {
-        List<Entity> children = getEntity().getChildren();
-        List<Point3d> results = new ArrayList<>();
-        for(Entity child : children) {
-            if(child.getComponent(GripperComponentJaw.class)==null) continue;
-            Matrix4d pose = child.getComponent(PoseComponent.class).getWorld();
-            results.add(new Point3d(MatrixHelper.getPosition(pose)));
-        }
-        return results;
-    }
-
     @Override
     public void render(GL3 gl) {
         List<Entity> children = getEntity().getChildren();

src/main/java/com/marginallyclever/robotoverlord/components/PoseComponent.java

@@ -50,6 +50,8 @@ public void parseJSON(JSONObject jo,SerializationContext context) throws JSONExc
         position.parseJSON(jo.getJSONObject("position"),context);
         rotation.parseJSON(jo.getJSONObject("rotation"),context);
         scale.parseJSON(jo.getJSONObject("scale"),context);
+        // the property change event will refresh the local matrix once for each parse.
+        // TODO without refreshLocalMatrix() here, the local matrix is not updated.  Why?
         refreshLocalMatrix();
     }
 
@@ -76,7 +78,7 @@ public void setPosition(Vector3d position) {
      * @param arg0 a {@link Vector3d} with three angles (in degrees)
      */
     public void setRotation(Vector3d arg0) {
-        this.rotation.set(arg0);
+        rotation.set(arg0);
         refreshLocalMatrix();
     }
 
@@ -100,7 +102,7 @@ public void setLocalMatrix4(Matrix4d m) {
      * @return a {@link Vector3d} with degree rotations.
      */
     public Vector3d getRotation() {
-        return new Vector3d(this.rotation.get());
+        return new Vector3d(rotation.get());
     }
 
     public Matrix4d getLocal() {

src/main/java/com/marginallyclever/robotoverlord/swing/componentmanagerpanel/ComponentManagerPanel.java

@@ -108,7 +108,6 @@ private void addSingleComponent(Component component) {
 			sys.decorate(factory,component);
 		}
 
-
 		JComponent outerPanel = wrapViewWithCommonComponentControls(factory.getResult(),component);
 		componentList.add(outerPanel);
 	}

src/main/java/com/marginallyclever/robotoverlord/systems/robot/RobotGripperSystem.java

@@ -48,8 +48,8 @@ public void update(double dt) {}
     @Override
     public void decorate(ComponentSwingViewFactory view, Component component) {
         if( component instanceof GripperComponentLinear) decorateGripperLinear(view,(GripperComponentLinear)component);
-        else if( component instanceof GripperComponentRotary) decorateGripperRotary(view,(GripperComponentRotary)component);
-        else if( component instanceof GripperComponentJaw) decorateGripperJaw(view,(GripperComponentJaw)component);
+        if( component instanceof GripperComponentRotary) decorateGripperRotary(view,(GripperComponentRotary)component);
+        if( component instanceof GripperComponentJaw) decorateGripperJaw(view,(GripperComponentJaw)component);
     }
 
     private void decorateGripperJaw(ComponentSwingViewFactory view, GripperComponentJaw jaw) {
@@ -58,8 +58,8 @@ private void decorateGripperJaw(ComponentSwingViewFactory view, GripperComponent
     }
 
     private void decorateGripperRotary(ComponentSwingViewFactory view, GripperComponentRotary gripper) {
-        ViewElementComboBox box = (ViewElementComboBox)view.addComboBox(gripper.mode, GripperComponentAbstract.names);
-        box.setReadOnly(true);
+        view.addComboBox(gripper.mode, GripperComponentAbstract.names);
+
         ViewElementButton bToggleGripper = view.addButton(Translator.get("RobotGripperSystem.grab"));
         bToggleGripper.addActionEventListener((evt)-> {
             switch(gripper.mode.get()) {
@@ -73,8 +73,7 @@ private void decorateGripperRotary(ComponentSwingViewFactory view, GripperCompon
     }
 
     private void decorateGripperLinear(ComponentSwingViewFactory view, GripperComponentLinear gripper) {
-        ViewElementComboBox box = (ViewElementComboBox)view.addComboBox(gripper.mode, GripperComponentAbstract.names);
-        box.setReadOnly(true);
+        view.addComboBox(gripper.mode, GripperComponentAbstract.names);
 
         ViewElementButton bToggleGripper = view.addButton(Translator.get("RobotGripperSystem.grab"));
         bToggleGripper.addActionEventListener((evt)-> {
@@ -114,47 +113,57 @@ private void doGrabRotary(GripperComponentRotary gripper) {
         if (jaws.isEmpty()) return;
 
         // Cast a ray along axis of jaw travel and collect all hits
+        // The jaw travel is an arc.  Break into several small rays.
         List<RayHit> hits = new ArrayList<>();
-        for(GripperComponentJaw jaw : jaws) {
-            double distance = (jaw.openDistance.get() - jaw.closeDistance.get());
-            Matrix4d jawMatrix = jaw.getEntity().getComponent(PoseComponent.class).getWorld();
-            Point3d jawP = new Point3d(MatrixHelper.getPosition(jawMatrix));
-            Vector3d jawZ = MatrixHelper.getZAxis(jawMatrix);
-            Ray ray = new Ray(jawP,jawZ,distance);
-            RayPickSystem picker = new RayPickSystem(entityManager);
-            try {
-                List<RayHit> jawHit = picker.findRayIntersections(ray);
-                hits.addAll(jawHit);
-            } catch (Exception e) {
-                logger.error("Error while ray casting.",e);
+        final int STEPS = 10;
+        for(int step=0;step<STEPS;++step) {
+            for (GripperComponentJaw jaw : jaws) {
+                double distance = jaw.getDistance()/STEPS;
+                if (distance == 0) continue;
+
+                Entity contactPoint = jaw.getEntity().getChildren().get(0);
+                Matrix4d contactPointMatrix = contactPoint.getComponent(PoseComponent.class).getWorld();
+
+                Point3d rayStart = new Point3d(MatrixHelper.getPosition(contactPointMatrix));
+                Vector3d rayDirection = MatrixHelper.getZAxis(contactPointMatrix);
+                Ray ray = new Ray(rayStart, rayDirection, distance);
+                RayPickSystem picker = new RayPickSystem(entityManager);
+                try {
+                    List<RayHit> jawHit = picker.findRayIntersections(ray);
+                    hits.addAll(jawHit);
+                } catch (Exception e) {
+                    logger.error("Error while ray casting.", e);
+                }
+
+                // Close the gripper
+                // FIXME until it touches the object.
+                moveJawsRotary(gripper, 1.0/STEPS);
             }
         }
 
         doGrabShared(gripper, hits);
-
-        // Close the gripper
-        // FIXME until it touches the object.
-        moveJawsRotary(gripper, 1);
     }
 
     void doGrabLinear(GripperComponentLinear gripper) {
         List<GripperComponentJaw> jaws = gripper.getJaws();
         if (jaws.isEmpty()) return;
 
-        // cast a ray along axis of jaw travel and collect all hits
+        // cast a ray along axis of jaw travel and collect all hits.
         List<RayHit> hits = new ArrayList<>();
-        for(GripperComponentJaw jaw : jaws) {
-            double distance = (jaw.openDistance.get() - jaw.closeDistance.get());
+        for (GripperComponentJaw jaw : jaws) {
+            double distance = jaw.getDistance();
+            if (distance == 0) continue;
+
             Matrix4d jawMatrix = jaw.getEntity().getComponent(PoseComponent.class).getWorld();
-            Point3d jawP = new Point3d(MatrixHelper.getPosition(jawMatrix));
-            Vector3d jawZ = MatrixHelper.getZAxis(jawMatrix);
-            Ray ray = new Ray(jawP,jawZ,distance);
+            Ray ray = new Ray(new Point3d(MatrixHelper.getPosition(jawMatrix)),
+                    MatrixHelper.getZAxis(jawMatrix),
+                    distance);
             RayPickSystem picker = new RayPickSystem(entityManager);
             try {
                 List<RayHit> jawHit = picker.findRayIntersections(ray);
                 hits.addAll(jawHit);
             } catch (Exception e) {
-                logger.error("Error while ray casting.",e);
+                logger.error("Error while ray casting.", e);
             }
         }
 
@@ -195,7 +204,7 @@ private void doGrabShared(GripperComponentAbstract gripper, List<RayHit> hits) {
 
     /**
      * Open the gripper
-     * @param gripper
+     * @param gripper the gripper to move
      */
     private void doReleaseRotary(GripperComponentRotary gripper) {
         doReleaseShared(gripper);
@@ -204,7 +213,7 @@ private void doReleaseRotary(GripperComponentRotary gripper) {
 
     /**
      * Open the gripper
-     * @param gripper
+     * @param gripper the gripper to move
      */
     void doReleaseLinear(GripperComponentLinear gripper) {
         doReleaseShared(gripper);
@@ -238,16 +247,22 @@ private void doReleaseShared(GripperComponentAbstract gripper) {
     private void moveJawsRotary(GripperComponentRotary gripper, double direction) {
         List<GripperComponentJaw> jaws = gripper.getJaws();
         for(GripperComponentJaw jaw : jaws) {
-            double distance = (jaw.openDistance.get() - jaw.closeDistance.get());
-            moveOneJawRotary(jaw, distance);
+            moveOneJawRotary(jaw, direction * jaw.getDistance());
         }
     }
 
     private void moveOneJawRotary(GripperComponentJaw jaw, double distance) {
         PoseComponent pose = jaw.getEntity().getComponent(PoseComponent.class);
-        Vector3d r = pose.getRotation();
-        r.z += distance;
-        pose.setRotation(r);
+        Matrix4d world = pose.getWorld();
+        Matrix4d rot = new Matrix4d();
+        rot.rotZ(Math.toRadians(distance));
+        world.mul(rot);
+        pose.setWorld(world);
+
+        // rotates relative to the gripper.
+//        Vector3d r = pose.getRotation();
+//        r.z += distance;
+//        pose.setRotation(r);
     }
 
     /**
@@ -257,8 +272,7 @@ private void moveOneJawRotary(GripperComponentJaw jaw, double distance) {
     private void moveJawsLinear(GripperComponentLinear gripper,double direction) {
         List<GripperComponentJaw> jaws = gripper.getJaws();
         for(GripperComponentJaw jaw : jaws) {
-            double distance = (jaw.openDistance.get() - jaw.closeDistance.get());
-            moveOneJawLinear(jaw, distance*direction);
+            moveOneJawLinear(jaw, direction * jaw.getDistance());
         }
     }
 
@@ -273,12 +287,16 @@ private void moveOneJawLinear(GripperComponentJaw jaw, double distance) {
     }
 
     private void removeGripperAndJawsFromHits(GripperComponentAbstract gripper, List<RayHit> hits) {
-        List<GripperComponentJaw> jaws = gripper.getJaws();
-        List<ShapeComponent> meshes = new ArrayList<>();
-        for(GripperComponentJaw jaw : jaws) {
-            meshes.add(jaw.getEntity().getComponent(ShapeComponent.class));
+        List<RayHit> keep = new ArrayList<>();
+        Entity gripperEntity = gripper.getEntity();
+        for(RayHit hit : hits) {
+            Entity e = hit.target.getEntity();
+            // do not consider the gripper itself, the jaws, or the contact points.
+            if(e.isChildOf(gripperEntity)==-1 && e != gripperEntity) {
+                keep.add(hit);
+            }
         }
-        ShapeComponent gripperBody = gripper.getEntity().getComponent(ShapeComponent.class);
-        hits.removeIf(hit -> meshes.contains(hit.target) || hit.target == gripperBody);
+        hits.clear();
+        hits.addAll(keep);
     }
 }