def generateSubGoals(q0, edges):
'''
Generate a list of subgoals
Input:
- q0: initial configuration
- edges: list of transitions
Return
- a list of subgoal configurations
The first subgoal is the initial configuration q0. The following ones are
generated by projecting random configurations on the destination node of
each transition of the list. Each subgoal is reachable from the previous one
by a path of the transition.
'''
subgoals = list()
node = 'free'
res, q_init, err = cg.applyNodeConstraints(node, q0)
if not res:
raise RuntimeError('Failed to project configuration on node ' + node)
subgoals.append(q_init[::])
## Starting from initial configuration, iteratively produce random
# configurations on each successive node in such a way that each new
# configuration is reachable from the previous one through the transition
# linking the states of the configurations.
q_init = subgoals[0]
for edge in edges[:-1]:
edgeSuccess = False
for i in range(400):
if i == 0:
q = q_init
else:
q = robot.shootRandomConfig()
res, q1, err = cg.generateTargetConfig(edge, q_init, q)
if not res: continue
res, msg = robot.isConfigValid(q1)
if not res: continue
v(q1)
ps.addConfigToRoadmap(q1)
subgoals.append(q1[::])
q_init = q1[::]
edgeSuccess = True
break
if not edgeSuccess:
raise RuntimeError('Failed to generate a subgoal through edge ' +
edge)
## Generate last sub goal configuration to move back manipulator arms in
# initial configurations
q_goal = subgoals[-1][::]
q_goal[0:6] = q0_r0
q_goal[6:12] = q0_r1
subgoals.append(q_goal)
return subgoals
def generateSubGoals (q0, edges):
'''
Generate a list of subgoals
Input:
- q0: initial configuration
- edges: list of transitions
Return
- a list of subgoal configurations
The first subgoal is the initial configuration q0. The following ones are
generated by projecting random configurations on the destination node of
each transition of the list. Each subgoal is reachable from the previous one
by a path of the transition.
'''
subgoals = list ()
node = 'free'
res, q_init, err = cg.applyNodeConstraints (node, q0)
if not res:
raise RuntimeError ('Failed to project configuration on node ' + node)
subgoals.append (q_init [::])
## Starting from initial configuration, iteratively produce random
# configurations on each successive node in such a way that each new
# configuration is reachable from the previous one through the transition
# linking the states of the configurations.
q_init = subgoals [0]
for edge in edges [:-1]:
edgeSuccess = False
for i in range (400):
if i == 0:
q = q_init
else:
q = robot.shootRandomConfig ()
res, q1, err = cg.generateTargetConfig (edge, q_init, q)
if not res: continue
res, msg = robot.isConfigValid (q1)
if not res: continue
v (q1)
ps.addConfigToRoadmap (q1)
subgoals.append (q1 [::])
q_init = q1 [::]
edgeSuccess = True
break
if not edgeSuccess:
raise RuntimeError ('Failed to generate a subgoal through edge ' + edge)
## Generate last sub goal configuration to move back manipulator arms in
# initial configurations
q_goal = subgoals [-1] [::]
q_goal [0:6] = q0_r0; q_goal [6:12] = q0_r1
subgoals.append (q_goal)
return subgoals
if args.run:
pp(0)
q1 = [
-1.3762019511901535, -1.6026285891202607, 1.7698841773948761,
0.07766259702973155, -0.10292472893634562, 0, -0.8453817919408094,
-1.9613814755717633, 1.7509886934894354, 0.3286982532479891,
-0.21554233746381954, 0.1436178636671443, 0.02673796658911215,
-0.11683111862697124, 0.2805429927243004, 0.19030120185664454,
-0.6901027958010973, 0.017731848182281414, 0.6980180265290187, -0.0, -0.16,
0.025, 0, 0, 0, 1, 0.014796844811246754, -0.10624562226403622,
0.34044841428272643, 0.5911820185346596, 0.40388829235346274,
0.6581270459058463, -0.2329095526343504, 0.45, -0.07999999999999999, 0.025,
0, 0, 0, 1
]
ps.selectSteeringMethod('CrossStateOptimization-Straight')
ps.setParameter('CrossStateOptimization/maxDepth', 4)
n = 'cylinder0/magnet0 grasps sphere0/magnet : r0/gripper grasps sphere0/handle : r1/gripper grasps cylinder0/handle'
for i in range(100):
q = robot.shootRandomConfig()
res, q1, err = cg.applyNodeConstraints(n, q)
if res:
# move cylinder 2 and sphere2 back to initial position
q1[19:19 + 7] = q0[19:19 + 7]
q1[33:33 + 7] = q0[33:33 + 7]
res, pid, msg = ps.directPath(q0, q1, False)
if res: break
#ps.directPath (q0, q1, False)