From 8aed116bd5363be5a3ebe2bab1a0f58b333d0626 Mon Sep 17 00:00:00 2001
From: Dan Royer <dan@marginallyclever.com>
Date: Wed, 6 Dec 2023 16:27:27 -0800
Subject: [PATCH] add gripper to test

---
 .../controlarmpanel/ControlArmPanelTest.java  | 32 +++++++++++++++++--
 1 file changed, 30 insertions(+), 2 deletions(-)

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 72cfb5cba..b270a103d 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
@@ -1,12 +1,13 @@
 package com.marginallyclever.robotoverlord.systems.robot.robotarm.controlarmpanel;
 
 import com.marginallyclever.convenience.log.Log;
-import com.marginallyclever.robotoverlord.components.RobotComponent;
-import com.marginallyclever.robotoverlord.components.RobotComponentTest;
+import com.marginallyclever.robotoverlord.components.*;
+import com.marginallyclever.robotoverlord.entity.Entity;
 import com.marginallyclever.robotoverlord.entity.EntityManager;
 import com.marginallyclever.robotoverlord.swing.translator.Translator;
 
 import javax.swing.*;
+import javax.vecmath.Vector3d;
 
 public class ControlArmPanelTest {
     public static void main(String[] args) {
@@ -18,6 +19,7 @@ public static void main(String[] args) {
 
         EntityManager entityManager = new EntityManager();
         RobotComponent robot = RobotComponentTest.build5AxisSixi3(entityManager);
+        addRotaryGripper(entityManager,robot);
 
         JFrame frame = new JFrame(ControlArmPanel.class.getSimpleName());
         frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
@@ -26,6 +28,32 @@ public static void main(String[] args) {
         frame.setLocationRelativeTo(null);
         frame.setVisible(true);
     }
+
+    private static void addRotaryGripper(EntityManager entityManager, RobotComponent robot) {
+        // add gripper
+        Entity gripper = new Entity("Gripper");
+        entityManager.addEntityToParent(gripper, robot.getBone(robot.getNumBones()-1).getEntity());
+        GripperComponentRotary gcr = new GripperComponentRotary();
+        gripper.addComponent(gcr);
+        gcr.mode.set(GripperComponentAbstract.MODE_OPEN);
+
+        // add a jaw to the gripper
+        Entity jaw = new Entity("Jaw");
+        entityManager.addEntityToParent(jaw, gripper);
+        jaw.addComponent(new GripperComponentJaw());
+        PoseComponent jawPose = jaw.getComponent(PoseComponent.class);
+        jawPose.setPosition(new Vector3d(-0.014,-1.398,3.332));
+        jawPose.setRotation(new Vector3d(0,-45,-90));
+        jaw.getComponent(GripperComponentJaw.class).openDistance.set(90.0);
+
+        // add a contact point to the jaw
+        Entity contact = new Entity("Contact");
+        entityManager.addEntityToParent(contact, jaw);
+
+        PoseComponent contactPose = contact.getComponent(PoseComponent.class);
+        contactPose.setPosition(new Vector3d(0.65,-4,0.4));
+        contactPose.setRotation(new Vector3d(0,90,0));
+    }
 }