add gripper to test

MarginallyClever · Dec 7, 2023 · 8aed116 · 8aed116
1 parent 64dee90
commit 8aed116
Showing 1 changed file with 30 additions and 2 deletions.
diff --git a/...nallyclever/robotoverlord/systems/robot/robotarm/controlarmpanel/ControlArmPanelTest.java b/...nallyclever/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));
+    }
 }