def main(env,options):
"Main example code."
env.Load('data/pr2test1.env.xml')
robot=env.GetRobots()[0]
raw_input('press key to show at least one contact point')
with env:
# move both arms to collision
lindex = robot.GetJoint('l_shoulder_pan_joint').GetDOFIndex()
rindex = robot.GetJoint('r_shoulder_pan_joint').GetDOFIndex()
robot.SetDOFValues([0.226,-1.058],[lindex,rindex])
# setup the collision checker to return contacts
env.GetCollisionChecker().SetCollisionOptions(CollisionOptions.Contacts)
# get first collision
report = CollisionReport()
collision=env.CheckCollision(robot,report=report)
print '%d contacts'%len(report.contacts)
positions = [c.pos for c in report.contacts]
h1=env.plot3(array(positions),20,[1,0,0])
raw_input('press key to show collisions with links')
with env:
# collisions with individual links
positions = []
for link in robot.GetLinks():
collision=env.CheckCollision(link,report=report)
if len(report.contacts) > 0:
print 'link %s %d contacts'%(link.GetName(),len(report.contacts))
positions += [c.pos for c in report.contacts]
h2=env.plot3(array(positions),20,[1,0,0])
raw_input('press any key to exit')