def main(env,options):
"Main example code."
env.Load(options.scene)
robots = env.GetRobots()
manips = [env.GetRobot('pr2').GetManipulator('head_torso'), env.GetRobot('BarrettWAM').GetManipulator('arm')]
ikmodels = []
freeindices=[None,manips[1].GetArmIndices()[:-2]]
for imanip,manip in enumerate(manips):
# generate the ik solver
manip.GetRobot().SetActiveManipulator(manip)
ikmodel = databases.inversekinematics.InverseKinematicsModel(manip.GetRobot(), iktype=IkParameterization.Type.Lookat3D,freeindices=freeindices[imanip])
if not ikmodel.load():
ikmodel.autogenerate()
ikmodels.append(ikmodel)
while True:
maxsolutions = 40
goodsolutions = []
with env:
# move the robot in a random collision-free position and call the IK
while True:
target=ikmodel.manip.GetTransform()[0:3,3]+(random.rand(3)-0.5)
robotsolutions = []
for ikmodel in ikmodels:
for ikmodel2 in ikmodels:
ikmodel2.robot.Enable(ikmodel==ikmodel2)
solutions = ikmodel.manip.FindIKSolutions(IkParameterization(target,IkParameterization.Type.Lookat3D),IkFilterOptions.CheckEnvCollisions)
if len(solutions) == 0:
break
robotsolutions.append(solutions)
for ikmodel2 in ikmodels:
ikmodel2.robot.Enable(True)
if len(robotsolutions) == len(robots):
print 'found solutions for all manipulators, search for a joint collision-free one'
goodsolutions = []
# permute randomly to get more interesting solutions
allsols = [sols for sols in sequence_cross_product(*robotsolutions)]
stdrandom.shuffle(allsols)
for sols in allsols:
for ikmodel,sol in izip(ikmodels,sols):
ikmodel.robot.SetDOFValues(sol,ikmodel.manip.GetArmIndices())
if not any([ikmodel.robot.CheckSelfCollision() or env.CheckCollision(ikmodel.robot) for ikmodel in ikmodels]):
goodsolutions.append(sols)
if len(goodsolutions) >= maxsolutions:
break
if len(goodsolutions) > 0: # found solutions, so break!
break
handles = [env.plot3(array([target]),20.0)]
for sols in goodsolutions:
handlerays = []
with env:
for ikmodel,sol in izip(ikmodels,sols):
ikmodel.robot.SetDOFValues(sol,ikmodel.manip.GetArmIndices())
T = ikmodel.manip.GetTransform()
globaldir = numpy.dot(T[0:3,0:3],ikmodel.manip.GetDirection())
dist = linalg.norm(T[0:3,3]-target)+0.4
handlerays.append(env.drawlinelist(array([T[0:3,3], T[0:3,3]+dist*globaldir]),5,colors=[0.1,0.1,1]))
env.UpdatePublishedBodies()
time.sleep(0.1)