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