Commit 80d6b665 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Update scripts

parent 6386077a
......@@ -176,3 +176,6 @@ graph.setConstraints (graph=True, lockDof = lockDofs)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
r = vf.createRealClient()
pp = PathPlayer (robot.client.basic, r)
......@@ -82,6 +82,7 @@ for n in jointNames['all']:
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
ps.addPassiveDofs ('pr2', jointNames ['pr2'])
cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', 'pr2')
......@@ -202,10 +203,15 @@ cg.setConstraints (graph = True, lockDof = lockAll)
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q_init_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal_proj = res [1]
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
r = vf.createRealClient()
pp = PathPlayer (robot.client.basic, r)
......@@ -63,4 +63,4 @@ ps.addGoalConfig (q2proj)
ps.solve ()
pp = PathPlayer (cl, r)
pp (1)
#pp (1)
......@@ -109,3 +109,6 @@ graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balance
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
r = vf.createRealClient()
pp = PathPlayer (robot.client.basic, r)
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