collision checking basic interface built

catkin_ws/src/two_arm_utils/src/two_arm_utils/__init__.py

 from .controller import TwoArmController
+from .planner import CollisionChecker

catkin_ws/src/two_arm_utils/src/two_arm_utils/planner.py

 #!/usr/bin/env python
+
+import pybullet as p
+import time
+from pathlib import Path
+from math import pi
+
+urdf_folder = Path(__file__).parent / "../../urdf"
+
+class CollisionChecker:
+    """
+    wrapper for pyBullet simulation
+    """
+
+    def __init__(self,
+            numRobots = 2,
+            robotPos = [
+                ([0.5, 0, 0.001], [0,0,pi]),
+                ([-0.5, 0, 0.001], [0,0,0])
+                ],
+            gui = True
+            ):
+        """
+        RobotPos is a list of tuples, each tuple is:
+        (RobotPosition, RobotOrientation)
+        e.g. ([0,0,0], [0,0,pi])
+        """
+
+        assert (len(robotPos)==numRobots), "Incorrect number of robot poses"
+
+        # start pybullet
+        if gui:
+            self.client = p.connect(p.GUI)
+        else:
+            self.client = p.connect(p.DIRECT)
+
+        # Load Table
+        self.table = p.loadURDF(str(urdf_folder/"table.urdf"),
+                [0,0,0],
+                p.getQuaternionFromEuler([0,0,0])
+                )
+
+        # Load Arms
+        self.numRobots = numRobots
+        self.robots = []
+        for i in range(numRobots):
+            self.robots.append(p.loadURDF(str(urdf_folder/"ur5e.urdf"),
+                    robotPos[i][0],
+                    p.getQuaternionFromEuler(robotPos[i][1])
+                    ))
+
+
+    def setJoints(self, r, joints):
+        """
+        set the rth robot's joint angles
+        """
+        for j in range(len(joints)):
+            p.resetJointState(self.robots[r], j+1, joints[j])
+        return
+
+
+    def check(self, robots_joints): 
+        """
+        given joint angles of all robots, is there a collision?
+        arguement is a list (robots) of lists (joint angles)
+        """
+        for r in range(self.numRobots):
+            self.setJoints(r, robots_joints[r])
+
+        p.stepSimulation()
+        return len(p.getContactPoints()) - 2 # function returns 2 if no collisions
+

usage/test_pybullet.py

-import pybullet as p
-import time
-from math import pi
-#import pybullet_data
-physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
-#p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
-#p.setGravity(0,0,-10)
-#planeId = p.loadURDF("plane.urdf")
-startPos = [0,0,0]
-defaultOr = p.getQuaternionFromEuler([0,0,0])
-rotatedOr = p.getQuaternionFromEuler([0,0,pi])
-table = p.loadURDF("../catkin_ws/src/two_arm_utils/urdf/table.urdf",[0,0,0], defaultOr)
-robot1 = p.loadURDF("../catkin_ws/src/two_arm_utils/urdf/ur5e.urdf",[0.5, 0, 0.001], rotatedOr)
-robot2 = p.loadURDF("../catkin_ws/src/two_arm_utils/urdf/ur5e.urdf",[-0.5, 0, 0.001], defaultOr)
-#for i in range (10000):
-#    p.stepSimulation()
-#    time.sleep(1./240.)
-p.resetJointState(robot1, 2, -1)
-p.resetJointState(robot2, 2, -1)
+import two_arm_utils
 
-p.stepSimulation() # need to do this to update before collision check
-len(p.getContactPoints()) # always two collisions (guessing with table)
+cc = two_arm_utils.CollisionChecker()
 
-#p.disconnect()
+cc.check([[0,-1,0,0,0,0],[0,-1,0,0,0,0]])