Commit 30d09192 authored by Jayant Khatkar's avatar Jayant Khatkar

collision checking basic interface built

parent 0606be2b
from .controller import TwoArmController
from .planner import CollisionChecker
#!/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
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]])
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment