## 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
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock +
gaze_constraint + waist_constraint))
graph.addConstraints(edge="Loop | f",
constraints=Constraints(numConstraints=com_constraint +
foot_placement + arm_locked))
graph.initialize()
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25)
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("SimpleTimeParameterization")
res, q, err = graph.generateTargetConfig("starting_motion", initConf, initConf)
if not res:
raise RuntimeError('Failed to project initial configuration')
ps.setRandomSeed(54)
ps.setInitialConfig(initConf)
ps.addGoalConfig(q)
ps.solve()
# Delete intermediate non optimized paths
for i in range(ps.numberPaths() - 1):
ps.erasePath(0)
# Generate N random configurations
N = args.N
if N != 0:
configs = [q[::]]
graph.createEdge('home', free , 'start_arm', isInNode="home")
graph.createEdge( free , 'home', 'end_arm', isInNode=free)
graph.addConstraints(node="home", constraints=Constraints(numConstraints=lock_arm+lock_head))
graph.addConstraints(edge="end_arm", constraints=Constraints(numConstraints=["tiago_base", "lock_part"]))
graph.addConstraints(edge="move_base", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"]))
graph.addConstraints(edge="start_arm", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"]))
cproblem = ps.hppcorba.problem.getProblem()
cgraph = cproblem.getConstraintGraph()
graph.initialize()
# 3}}}
res, q0, err = graph.generateTargetConfig('start_arm', q0, q0)
assert (res)
# {{{3 Constraint graph validation
graphValidation = ps.client.manipulation.problem.createGraphValidation()
graphValidation.validate(cgraph)
if graphValidation.hasErrors():
print(graphValidation.str())
print("Graph has infeasibilities")
sys.exit(1)
elif graphValidation.hasWarnings():
print(graphValidation.str())
print("Graph has only warnings")
else:
print("Graph *seems* valid.")
# 3}}}
## 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()
res1, q3, err = graph.generateTargetConfig('approach-ball', q_init, q)
if res1 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
def solve():
print("Solver started")
ps.solve()
print("Solution found, let's display it")
# Constraint in this state are explicit so ps.setMaxIterProjection(1) should not
# make it fail.
res, q1, err = graph.applyNodeConstraints(
'tiago/gripper grasps driller/handle', q0)
q1valid, msg = robot.isConfigValid(q1)
if not q1valid:
print(msg)
assert res
ps.setInitialConfig(q1)
if not isSimulation:
qrand = q1
for i in range(100):
q2valid, q2, err = graph.generateTargetConfig(
'driller/drill_tip > skin/hole | 0-0', q1, qrand)
if q2valid:
q2valid, msg = robot.isConfigValid(q2)
if q2valid:
break
qrand = robot.shootRandomConfig()
assert q2valid
if not isSimulation:
ps.addGoalConfig(q2)
ps.solve()
try:
v = vf.createViewer()
v(q1)
except:
graph.addConstraints(
node="starting_state",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock))
graph.addConstraints(
node="free",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock +
gaze_constraint + waist_constraint))
graph.addConstraints(edge="Loop | f",
constraints=Constraints(numConstraints=com_constraint +
foot_placement + arm_locked))
graph.initialize()
res, q, err = graph.generateTargetConfig("apply_constraints", initConf,
initConf)
if not res:
raise RuntimeError('Failed to project initial configuration')
ps.setInitialConfig(initConf)
ps.addGoalConfig(q)
ps.solve()
pathId = ps.numberPaths() - 1
configs = [q[::]]
# Generate N random configurations
N = args.N
i = 1
while i < N:
q = robot.shootRandomConfig()
res, q1, err = graph.generateTargetConfig("Loop | f", configs[0], q)
