Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
What's new
10
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
bigprint
twins-planner
Commits
30d09192
Commit
30d09192
authored
Nov 05, 2020
by
Jayant Khatkar
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
collision checking basic interface built
parent
0606be2b
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
75 additions
and
22 deletions
+75
-22
catkin_ws/src/two_arm_utils/src/two_arm_utils/__init__.py
catkin_ws/src/two_arm_utils/src/two_arm_utils/__init__.py
+1
-0
catkin_ws/src/two_arm_utils/src/two_arm_utils/planner.py
catkin_ws/src/two_arm_utils/src/two_arm_utils/planner.py
+71
-0
usage/test_pybullet.py
usage/test_pybullet.py
+3
-22
No files found.
catkin_ws/src/two_arm_utils/src/two_arm_utils/__init__.py
View file @
30d09192
from
.controller
import
TwoArmController
from
.planner
import
CollisionChecker
catkin_ws/src/two_arm_utils/src/two_arm_utils/planner.py
View file @
30d09192
#!/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
View file @
30d09192
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
]]
)
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment