## Project initial configuration on state 'placement'
res, q_init, error = graph.applyNodeConstraints ('placement', q1)
q2 = q1 [::]
q2 [7] = .2
## Project goal configuration on state 'placement'
res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# v = vf.createViewer ()
# pp = PathPlayer (v)
# v (q1)
## Build relative position of the ball with respect to the gripper
for i in range (100):
q = robot.shootRandomConfig ()
res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
if res and robot.isConfigValid (q3): break;
if res:
robot.setCurrentConfig (q3)
gripperPose = Transform (robot.getJointPosition (gripperName))
ballPose = Transform (robot.getJointPosition (ballName))
gripperGraspsBall = gripperPose.inverse () * ballPose
gripperAboveBall = Transform (gripperGraspsBall)
gripperAboveBall.translation [2] += .1
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q1)
q2 = q1 [::]
q2 [7] = .2
## Project goal configuration on state 'placement'
res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q2)
## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
pp = PathPlayer (ps.client.basic, r)
## Build relative position of the ball with respect to the gripper
for i in range (100):
q = robot.shootRandomConfig ()
res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
if res and robot.isConfigValid (q3): break;
if res:
robot.setCurrentConfig (q3)
gripperPose = Transform (robot.getLinkPosition (gripperName))
ballPose = Transform (robot.getLinkPosition (ballName))
gripperAboveBall = gripperPose.inverse () * ballPose
gripperAboveBall.translation [2] += .1