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[::]]
configs += shootRandomConfigs(ps, graph, configs[0], N)
configs = orderConfigurations(ps, configs)
configs.append(initConf)
else:
#read configurations in a file
configs = readConfigsInFile('./data/all-configurations.csv')
buildRoadmap(configs)
visitConfigurations(ps, configs)
rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/torso_lift_joint']
q_init[rank] = 0.2
q_goal = q_init[::]
#check if there are collisions in each path
p = PathChecker(ps, q_init, q_goal)
dtime = 0.001
for i in range(ps.numberPaths()):
print("\n---Path {}---".format(i))
p.check_path(i, dtime)
rank = robot.rankInConfiguration['box/root_joint']
c = sqrt(2) / 2
#q_init [rank:rank+4] = [c, 0, c, 0]
q_init[rank:rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
rank = robot.rankInConfiguration['box/root_joint']
q_goal[rank:rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [c, 0, -c, 0]
del c
pp(p)
if not failed:
if joinPaths:
# join path
i0 = paths[0]
for i in paths[1:]:
ps.concatenatePath(i0, i)
if cleanPaths:
for k, i in enumerate(paths[1:]):
ps.erasePath(i - k)
ps.optimizePath(i0)
print(
"Optimized path:",
ps.numberPaths() - 1,
",",
ps.pathLength(ps.numberPaths() - 1),
)
else:
optpaths = []
for i in paths:
ps.optimizePath(i)
optpaths.append(ps.numberPaths() - 1)
print("Solution paths are\noptpaths=", str(optpaths))
print("displayPaths(v,optpaths) # to visualize paths")
res, pid, msg = ps.directPath(q1, q2, True)
if res:
print("Path from", i, "to", i + 1, ":", pid)
paths.append(pid)
else:
print("Could not joint", i, "to", i + 1, ":", msg)
failed = True
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
cleanPaths = True
if not failed:
# join path
i0 = paths[0]
for i in paths[1:]:
ps.concatenatePath(i0, i)
if cleanPaths:
for k, i in enumerate(paths[1:]):
ps.erasePath(i - k)
ps.optimizePath(i0)
print(
"Optimized path:",
ps.numberPaths() - 1,
",",
ps.pathLength(ps.numberPaths() - 1),
)
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)
if not res: continue
res = validateGazeConstraint(ps, q1, args.arm)
if not res: continue
res, msg = robot.isConfigValid(q1)
if res:
configs.append(q1)
i += 1
