From 64dee90f86e9bedb894513ba1c1d9f454c955b57 Mon Sep 17 00:00:00 2001
From: Dan Royer <dan@marginallyclever.com>
Date: Wed, 6 Dec 2023 16:17:35 -0800
Subject: [PATCH] Successfully run ControlArmPanelTest

---
 .../systems/robot/robotarm/EditArmPanel.java  |  5 ++
 .../components/RobotComponentTest.java        | 76 ++++++++++++++-----
 .../controlarmpanel/ControlArmPanelTest.java  | 13 +++-
 3 files changed, 73 insertions(+), 21 deletions(-)

diff --git a/src/main/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/EditArmPanel.java b/src/main/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/EditArmPanel.java
index 6fe5455d7..77eabdb01 100644
--- a/src/main/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/EditArmPanel.java
+++ b/src/main/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/EditArmPanel.java
@@ -110,6 +110,11 @@ private ShapeComponent findChildShapeComponent(Entity entity) {
         return null;
     }
 
+    /**
+     * Look for a DHComponent in the children of the given Entity.
+     * @param entity the entity to search
+     * @return the DHComponent if found, null otherwise
+     */
     private DHComponent findChildDHComponent(Entity entity) {
         for( Entity child : entity.getChildren()) {
             DHComponent found = child.getComponent(DHComponent.class);
diff --git a/src/test/java/com/marginallyclever/robotoverlord/components/RobotComponentTest.java b/src/test/java/com/marginallyclever/robotoverlord/components/RobotComponentTest.java
index 2ccef60f5..a980a1756 100644
--- a/src/test/java/com/marginallyclever/robotoverlord/components/RobotComponentTest.java
+++ b/src/test/java/com/marginallyclever/robotoverlord/components/RobotComponentTest.java
@@ -10,40 +10,72 @@
 import javax.vecmath.Matrix4d;
 import javax.vecmath.Point3d;
 import javax.vecmath.Vector3d;
-import java.util.ArrayList;
-import java.util.List;
 
 public class RobotComponentTest {
-    private RobotComponent build3AxisArm() {
-        EntityManager entityManager = new EntityManager();
+    private static RobotComponent buildGenericArm(EntityManager entityManager,int numJoints) {
         Entity base = new Entity("Base");
+        entityManager.addEntityToParent(base,entityManager.getRoot());
         RobotComponent robot = new RobotComponent();
         base.addComponent(robot);
         // add target
         Entity target = new Entity(RobotComponent.TARGET_NAME);
         entityManager.addEntityToParent(target, base);
-        // position arm
-        List<Entity> joints = new ArrayList<>();
-        List<DHComponent> dh = new ArrayList<>();
+
+        // add bones
         Entity prev = base;
-        for(int i=0;i<3;++i) {
+        for(int i=0;i<numJoints;++i) {
             Entity e = new Entity("J"+i);
-            joints.add(e);
             entityManager.addEntityToParent(e,prev);
+            e.addComponent(new DHComponent());
             prev = e;
-            DHComponent dhc = new DHComponent();
-            dh.add(dhc);
-            e.addComponent(dhc);
         }
-        dh.get(0).set(0,0,90,0,60,-60,true);
-        dh.get(1).set(0,10,0,45,170,-170,true);
-        dh.get(2).set(0,10,0,-90,170,-170,true);
-        joints.get(2).addComponent(new ArmEndEffectorComponent());
+
+        // add end effector
+        prev.addComponent(new ArmEndEffectorComponent());
+
+        // connect everything
         robot.findBones();
         robot.set(Robot.END_EFFECTOR_TARGET,robot.get(Robot.END_EFFECTOR));
 
         return robot;
     }
+    public static RobotComponent build5AxisSixi3(EntityManager entityManager) {
+        RobotComponent robot = buildGenericArm(entityManager,5);
+
+        // set DH parameters
+        DHComponent bone0 = robot.getBone(0);
+        DHComponent bone1 = robot.getBone(1);
+        DHComponent bone2 = robot.getBone(2);
+        DHComponent bone3 = robot.getBone(3);
+        DHComponent bone4 = robot.getBone(4);
+        bone0.set(8.000, 0.000, 270.000, 0.000, 170.000, -170.00, true);
+        bone1.set(9.130, 17.909, 0.000, 270.000, 370.000, 170.000, true);
+        bone2.set(0.000, 12.455, 0.000, 0.000, 150.000, -150.00, true);
+        bone3.set(0.000, 0.000, 270.000, 270.000, 440.000, 100.000, true);
+        bone4.set(5.100, 0.000, 0.000, 0.000, 360.000, 0.000, true);
+        bone0.setJointHome(0.000);
+        bone1.setJointHome(270.000);
+        bone2.setJointHome(0.000);
+        bone3.setJointHome(270.000);
+        bone4.setJointHome(0.000);
+
+        robot.set(Robot.END_EFFECTOR_TARGET,robot.get(Robot.END_EFFECTOR));
+
+        return robot;
+    }
+
+    private static RobotComponent build3AxisArm(EntityManager entityManager) {
+        RobotComponent robot = buildGenericArm(entityManager,3);
+
+        // set DH parameters
+        robot.getBone(0).set(0,0,90,0,60,-60,true);
+        robot.getBone(1).set(0,10,0,45,170,-170,true);
+        robot.getBone(2).set(0,10,0,-90,170,-170,true);
+
+        robot.set(Robot.END_EFFECTOR_TARGET,robot.get(Robot.END_EFFECTOR));
+
+        return robot;
+    }
 
     private void testEndEffectorAtPosition(RobotComponent robot, Point3d expected) {
         Point3d actual = (Point3d)robot.get(Robot.END_EFFECTOR_TARGET_POSITION);
@@ -55,7 +87,8 @@ private void testEndEffectorAtPosition(RobotComponent robot, Point3d expected) {
      */
     @Test
     public void test3AxisArmAtPosition() {
-        RobotComponent robot = build3AxisArm();
+        EntityManager entityManager = new EntityManager();
+        RobotComponent robot = build3AxisArm(entityManager);
         testEndEffectorAtPosition(robot,new Point3d(14.14214,0,0));
     }
 
@@ -65,7 +98,8 @@ public void test3AxisArmAtPosition() {
      */
     @Test
     public void test3AxisArmAtPosition2() {
-        RobotComponent robot = build3AxisArm();
+        EntityManager entityManager = new EntityManager();
+        RobotComponent robot = build3AxisArm(entityManager);
         // a few iterations are needed to refine the movement.
         for(int i=0;i<12;++i) {
             robot.set(Robot.END_EFFECTOR_TARGET_POSITION, new Point3d(15.14214, 0, 0));
@@ -79,7 +113,8 @@ public void test3AxisArmAtPosition2() {
      */
     @Test
     public void test3AxisArmAtPosition3() {
-        RobotComponent robot = build3AxisArm();
+        EntityManager entityManager = new EntityManager();
+        RobotComponent robot = build3AxisArm(entityManager);
         Matrix4d pose = MatrixHelper.createIdentityMatrix4();
         pose.setTranslation(new Vector3d(10,0,0));
         robot.set(RobotComponent.POSE,pose);
@@ -92,7 +127,8 @@ public void test3AxisArmAtPosition3() {
      */
     @Test
     public void test3AxisArmAtPosition4() {
-        RobotComponent robot = build3AxisArm();
+        EntityManager entityManager = new EntityManager();
+        RobotComponent robot = build3AxisArm(entityManager);
         Matrix4d pose = (Matrix4d)robot.get(RobotComponent.POSE);
         robot.set(RobotComponent.POSE,pose);
         // a few iterations are needed to refine the movement.
diff --git a/src/test/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/controlarmpanel/ControlArmPanelTest.java b/src/test/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/controlarmpanel/ControlArmPanelTest.java
index b06583b2c..72cfb5cba 100644
--- a/src/test/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/controlarmpanel/ControlArmPanelTest.java
+++ b/src/test/java/com/marginallyclever/robotoverlord/systems/robot/robotarm/controlarmpanel/ControlArmPanelTest.java
@@ -2,6 +2,8 @@
 
 import com.marginallyclever.convenience.log.Log;
 import com.marginallyclever.robotoverlord.components.RobotComponent;
+import com.marginallyclever.robotoverlord.components.RobotComponentTest;
+import com.marginallyclever.robotoverlord.entity.EntityManager;
 import com.marginallyclever.robotoverlord.swing.translator.Translator;
 
 import javax.swing.*;
@@ -14,7 +16,8 @@ public static void main(String[] args) {
             UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName());
         } catch(Exception ignored) {}
 
-        RobotComponent robot = new RobotComponent();
+        EntityManager entityManager = new EntityManager();
+        RobotComponent robot = RobotComponentTest.build5AxisSixi3(entityManager);
 
         JFrame frame = new JFrame(ControlArmPanel.class.getSimpleName());
         frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
@@ -24,3 +27,11 @@ public static void main(String[] args) {
         frame.setVisible(true);
     }
 }
+
+
+
+
+
+
+
+