Skip to content

Commit

Permalink
added test for rotary gripper
Browse files Browse the repository at this point in the history
  • Loading branch information
@i-make-robots
i-make-robots committed Dec 7, 2023
1 parent 8aed116 commit f31fceb
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 95 deletions.
4 changes: 2 additions & 2 deletions src/main/java/com/marginallyclever/robotoverlord/systems/robot/RobotGripperSystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ private void setGripperButton(ViewElementButton bToggleGripper, GripperComponent
}
}

private void doGrabRotary(GripperComponentRotary gripper) {
void doGrabRotary(GripperComponentRotary gripper) {
List<GripperComponentJaw> jaws = gripper.getJaws();
if (jaws.isEmpty()) return;

Expand Down Expand Up @@ -206,7 +206,7 @@ private void doGrabShared(GripperComponentAbstract gripper, List<RayHit> hits) {
* Open the gripper
* @param gripper the gripper to move
*/
private void doReleaseRotary(GripperComponentRotary gripper) {
void doReleaseRotary(GripperComponentRotary gripper) {
doReleaseShared(gripper);
moveJawsRotary(gripper, -1);
}
Expand Down
165 changes: 101 additions & 64 deletions src/test/java/com/marginallyclever/robotoverlord/systems/robot/RobotGripperSystemTest.java
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,7 @@
package com.marginallyclever.robotoverlord.systems.robot;

import com.marginallyclever.convenience.helpers.MatrixHelper;
import com.marginallyclever.robotoverlord.components.PoseComponent;
import com.marginallyclever.robotoverlord.components.GripperComponentLinear;
import com.marginallyclever.robotoverlord.components.GripperComponentJaw;
import com.marginallyclever.robotoverlord.components.*;
import com.marginallyclever.robotoverlord.components.shapes.Box;
import com.marginallyclever.robotoverlord.components.shapes.Cylinder;
import com.marginallyclever.robotoverlord.entity.Entity;
Expand All @@ -15,49 +13,109 @@
import javax.vecmath.Matrix4d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Vector3d;
import java.util.List;

public class RobotGripperSystemTest {
private EntityManager entityManager;
private RobotGripperSystem gripperSystem;
private Entity base;
private Entity j0;
private Entity j1;
private Entity subject;
private GripperComponentLinear gripperComponent;

@BeforeEach
public void BeforeEach() {
public void setup() {
entityManager = new EntityManager();
gripperSystem = new RobotGripperSystem(entityManager);
base = new Entity("base");
j0 = new Entity("j0");
j1 = new Entity("j1");
subject = new Entity("subject");
}

entityManager.addEntityToParent(j0,base);
entityManager.addEntityToParent(j1,base);
entityManager.addEntityToParent(base,entityManager.getRoot());
public GripperComponentRotary addGripperRotary(EntityManager entityManager, Entity parent) {
// add gripper
Entity base = new Entity("Gripper");
entityManager.addEntityToParent(base, parent);
GripperComponentRotary gripperComponent = new GripperComponentRotary();
base.addComponent(gripperComponent);
base.addComponent(new Cylinder());
gripperComponent.mode.set(GripperComponentAbstract.MODE_OPEN);

// add a fixed jaw to the gripper
Entity fixedJaw = new Entity("FixedJaw");
entityManager.addEntityToParent(fixedJaw, base);
fixedJaw.addComponent(new Box());

// add a single jaw to the gripper
Entity jaw = new Entity("Jaw");
entityManager.addEntityToParent(jaw, base);
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);
jaw.addComponent(new Box());

// 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));

return gripperComponent;
}

@Test
public void grabAndReleaseRotary() {
GripperComponentRotary gripperComponent = addGripperRotary(entityManager,entityManager.getRoot());
Entity base = gripperComponent.getEntity();

// something to grab
Entity subject = new Entity("subject");
entityManager.addEntityToParent(subject,entityManager.getRoot());
subject.addComponent(new Box());
// put the subject at the tip of the fixed gripper
subject.getComponent(PoseComponent.class).setPosition(new Vector3d(0,-3.874,6.431));

GripperComponentJaw jaw0 = new GripperComponentJaw();
GripperComponentJaw jaw1 = new GripperComponentJaw();
// do the grab
gripperSystem.doGrabRotary(gripperComponent);
Assertions.assertEquals(base, subject.getParent());
// turn the gripper Z+90 degrees
base.getComponent(PoseComponent.class).setRotation(new Vector3d(0,0,90));
// translate (1,2,3)
base.getComponent(PoseComponent.class).setPosition(new Vector3d(1,2,3));
// release the subject
gripperSystem.doReleaseRotary(gripperComponent);
Assertions.assertEquals(entityManager.getRoot(), subject.getParent());

j0.addComponent(new Box());
j0.addComponent(jaw0);
// assert that the subject is rotated same as base
checkSubjectMatchesBase(subject,base);

j1.addComponent(new Box());
j1.addComponent(jaw1);
Matrix4d subjectWorld = subject.getComponent(PoseComponent.class).getWorld();
assertEquals(MatrixHelper.getPosition(subjectWorld),new Vector3d(4.874,2,9.431),0.0001);
}

public GripperComponentLinear addGripperLinear(EntityManager entityManager, Entity parent) {
Entity base = new Entity("base");
entityManager.addEntityToParent(base,parent);
GripperComponentLinear gripperComponent = new GripperComponentLinear();
base.addComponent(gripperComponent);
base.addComponent(new Cylinder());

subject.addComponent(new Box());

// two jaws
Entity j0 = new Entity("j0");
entityManager.addEntityToParent(j0,base);
GripperComponentJaw jaw0 = new GripperComponentJaw();
j0.addComponent(new Box());
j0.addComponent(jaw0);
jaw0.openDistance.set(5.0);
jaw0.closeDistance.set(0.0);

Entity j1 = new Entity("j1");
entityManager.addEntityToParent(j1,base);
GripperComponentJaw jaw1 = new GripperComponentJaw();
j1.addComponent(new Box());
j1.addComponent(jaw1);
jaw1.openDistance.set(5.0);
jaw1.closeDistance.set(0.0);

// set the jaws in the open position
gripperComponent.mode.set(GripperComponentAbstract.MODE_OPEN);
Matrix4d m = MatrixHelper.createIdentityMatrix4();
m.rotY(Math.PI/2);
m.setTranslation(new Vector3d(-2.5,0,2.5));
Expand All @@ -66,45 +124,8 @@ public void BeforeEach() {
m.rotY(-Math.PI/2);
m.setTranslation(new Vector3d(2.5,0,2.5));
j1.getComponent(PoseComponent.class).setWorld(m);
// put the subject in the middle of the gripper
subject.getComponent(PoseComponent.class).setPosition(new Vector3d(0,0,2.5));

// set the gripper base to the origin
gripperComponent = new GripperComponentLinear();
base.addComponent(gripperComponent);
}

@Test
public void grabAndRelease() {
// test grab
gripperSystem.doGrabLinear(gripperComponent);
Assertions.assertEquals(base, subject.getParent());

// test release
gripperSystem.doReleaseLinear(gripperComponent);
Assertions.assertEquals(entityManager.getRoot(), subject.getParent());

// assert that the subjectWorld pose is rotated Z+90 degrees
checkSubjectMatchesBase(subject, base);
}

@Test
public void grabAndReleaseWithRotation() {
gripperSystem.doGrabLinear(gripperComponent);
// turn the gripper Z+90 degrees
base.getComponent(PoseComponent.class).setRotation(new Vector3d(0,0,90));
gripperSystem.doReleaseLinear(gripperComponent);

// assert that the subject is rotated same as base
checkSubjectMatchesBase(subject,base);

Matrix4d subjectWorld = subject.getComponent(PoseComponent.class).getWorld();
assertEquals(MatrixHelper.getPosition(subjectWorld),new Vector3d(0.0,0.0,2.5),0.0001);

Vector3d jp0 = MatrixHelper.getPosition(j0.getComponent(PoseComponent.class).getWorld());
assertEquals(jp0,new Vector3d(0.0,-2.5,2.5),0.0001);
Vector3d jp1 = MatrixHelper.getPosition(j1.getComponent(PoseComponent.class).getWorld());
assertEquals(jp1,new Vector3d(0.0,2.5,2.5),0.0001);
return gripperComponent;
}

private void assertEquals(Tuple3d a,Tuple3d b,double margin) {
Expand All @@ -114,25 +135,41 @@ private void assertEquals(Tuple3d a,Tuple3d b,double margin) {
}

@Test
public void grabAndReleaseWithRotationAndTranslation() {
public void grabAndReleaseLinear() {
GripperComponentLinear gripperComponent = addGripperLinear(entityManager,entityManager.getRoot());
Entity base = gripperComponent.getEntity();

// something to grab
Entity subject = new Entity("subject");
entityManager.addEntityToParent(subject,entityManager.getRoot());
subject.addComponent(new Box());
// put the subject in the middle of the gripper
subject.getComponent(PoseComponent.class).setPosition(new Vector3d(0,0,2.5));

// do the grab
gripperSystem.doGrabLinear(gripperComponent);
Assertions.assertEquals(base, subject.getParent());
// turn the gripper Z+90 degrees
base.getComponent(PoseComponent.class).setRotation(new Vector3d(0,0,90));
// translate (1,2,3)
base.getComponent(PoseComponent.class).setPosition(new Vector3d(1,2,3));
// release the subject
gripperSystem.doReleaseLinear(gripperComponent);
Assertions.assertEquals(entityManager.getRoot(), subject.getParent());

// assert that the subject is rotated same as base
checkSubjectMatchesBase(subject,base);

Matrix4d subjectWorld = subject.getComponent(PoseComponent.class).getWorld();
assertEquals(MatrixHelper.getPosition(subjectWorld),new Vector3d(1.0,2.0,5.5),0.0001);

List<GripperComponentJaw> jaws = gripperComponent.getJaws();
Entity j0 = jaws.get(0).getEntity();
Entity j1 = jaws.get(1).getEntity();
Vector3d jp0 = MatrixHelper.getPosition(j0.getComponent(PoseComponent.class).getWorld());
assertEquals(jp0,new Vector3d(1.0,2.0-2.5,5.5),0.0001);
Vector3d jp1 = MatrixHelper.getPosition(j1.getComponent(PoseComponent.class).getWorld());
assertEquals(jp1,new Vector3d(1.0,2.0+2.5,5.5),0.0001);

}

private void checkSubjectMatchesBase(Entity subject, Entity base) {
Expand Down
34 changes: 5 additions & 29 deletions ...nallyclever/robotoverlord/systems/robot/robotarm/controlarmpanel/ControlArmPanelTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,11 @@

import com.marginallyclever.convenience.log.Log;
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 com.marginallyclever.robotoverlord.systems.robot.RobotGripperSystemTest;

import javax.swing.*;
import javax.vecmath.Vector3d;

public class ControlArmPanelTest {
public static void main(String[] args) {
Expand All @@ -18,8 +17,11 @@ public static void main(String[] args) {
} catch(Exception ignored) {}

EntityManager entityManager = new EntityManager();
// build an arm
RobotComponent robot = RobotComponentTest.build5AxisSixi3(entityManager);
addRotaryGripper(entityManager,robot);
// add a gripper on the end of the arm
RobotGripperSystemTest rgst = new RobotGripperSystemTest();
rgst.addGripperRotary(entityManager,robot.getBone(robot.getNumBones()-1).getEntity());

JFrame frame = new JFrame(ControlArmPanel.class.getSimpleName());
frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
Expand All @@ -28,32 +30,6 @@ 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));
}
}


Expand Down

0 comments on commit f31fceb

Please sign in to comment.