rank = robot.rankInConfiguration ['r_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['r_elbow_flex_joint'] q_goal [rank] = -0.5 vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.selectPathValidation ("Dichotomy", 0) import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (N): ps.clearRoadmap () ps.resetGoalConfigs () ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) t1 = dt.datetime.now () ps.solve () t2 = dt.datetime.now () totalTime += t2 - t1 print (t2-t1) n = len (ps.client.problem.nodes ()) totalNumberNodes += n print ("Number nodes: " + str(n)) print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N))) r = vf.createViewer(); r (q_init)
-0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0 ] res = ps.applyConstraints(q2) if res[0]: q2proj = res[1] else: raise RuntimeError("Failed to apply constraint.") ps.selectPathOptimizer("None") import datetime as dt totalTime = dt.timedelta(0) totalNumberNodes = 0 for i in range(20): ps.client.problem.clearRoadmap() ps.resetGoalConfigs() ps.setInitialConfig(q1proj) ps.addGoalConfig(q2proj) t1 = dt.datetime.now() ps.solve() t2 = dt.datetime.now() totalTime += t2 - t1 print(t2 - t1) n = len(ps.client.problem.nodes()) totalNumberNodes += n print("Number nodes: " + str(n)) print("Average time: " + str((totalTime.seconds + 1e-6 * totalTime.microseconds) / 20.)) print("Average number nodes: " + str(totalNumberNodes / 20.))
qt = [0, 0, 0, math.sqrt (1/3.), math.sqrt (1/3.), 0.0, math.sqrt (1/3.)] qt2 = [0, 0, 0, math.sqrt (1 - 0.95**2), 0.95, 0.0, 0.0] Q.append (qt) Q.append (qt2) Q.append ( [0, 0, 0, 0.99, 0.14003571, 0.013, 0.011]) # to remove r(Q[0]) for i in range(0, len(Q)): r(Q[i]) time.sleep (0.5) #robot.isConfigValid(Q[0]) for i in range(0, len(Q)-1): ps.setInitialConfig (Q[i]); ps.addGoalConfig (Q[i+1]); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (Q[0]); ps.addGoalConfig (Q[len(Q)-1]); ps.solve (); nInitialPath = ps.numberPaths()-1 #8 ps.pathLength(nInitialPath) #ps.addPathOptimizer('RandomShortcut') #9 #ps.optimizePath (nInitialPath) #ps.pathLength(ps.numberPaths()-1) #ps.clearPathOptimizers() ps.addPathOptimizer("GradientBased") ps.optimizePath (nInitialPath)
robot.setJointBounds('base_joint_xyz', [xStone-2, xStone+2, yEmu-1, yEmu+2, zEmu-0.5, zEmu+0.5]) # List of configs q1 = [xStone+2, yEmu+0, zEmu, 1.0, 0.0, 0.0, 0.0] q2 = [xStone+1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, 0.707106781] q3 = [xStone+1, yEmu+1.4, zEmu, 0, 0, 0, 1] q4 = [xStone+0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, 0.707106781] #q5 = [xStone+0, yEmu+2, zEmu, -1, 0, 0, 0] q5 = [xStone+0, yEmu+2, zEmu, -0.99, 0.14003571, 0.013, 0.011] q6 = [xStone-0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, -0.707106781] q7 = [xStone-1, yEmu+1.4, zEmu, 0, 0, 0, -1] q8 = [xStone-1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, -0.707106781] q9 = [xStone-2, yEmu+0, zEmu, 1, 0, 0, 0] r(q1) robot.isConfigValid(q1) ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q5); ps.addGoalConfig (q6); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q6); ps.addGoalConfig (q7); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q7); ps.addGoalConfig (q8); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q8); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q1); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs () nInitialPath = ps.numberPaths()-1 #8 ps.pathLength(nInitialPath) #ps.addPathOptimizer('RandomShortcut') #9 #ps.optimizePath (nInitialPath) #ps.pathLength(ps.numberPaths()-1)
ps = ProblemSolver (robot) cl = robot.client cl.obstacle.loadObstacleModel('robot_2d_description','cylinder_obstacle','') # q = [x, y] # limits in URDF file """ q1 = [-2, 0]; q2 = [-0.2, 2]; q3 = [0.2, 2]; q4 = [2, 0] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q1); ps.addGoalConfig (q4); ps.solve (); #3 """ q1 = [-2, 0]; q2 = [-1, 1]; q3 = [-1.2, 1.8]; q4 = [-0.2, 1.2]; q5 = [0.5, 1.9] q6 = [2, 1.5]; q7 = [1, 0.5]; q8 = [2, 0] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q5); ps.addGoalConfig (q6); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q6); ps.addGoalConfig (q7); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q7); ps.addGoalConfig (q8); ps.solve (); ps.resetGoalConfigs () ps.setInitialConfig (q1); ps.addGoalConfig (q8); ps.solve (); # 7 nInitialPath = ps.numberPaths()-1 #8 ps.pathLength(nInitialPath) ps.addPathOptimizer("GradientBased") #ps.addPathOptimizer("Prune") #ps.addPathOptimizer("PartialRandomShortcut")
import time robot = Robot ('puzzle_robot') # object5 robot.setJointBounds('base_joint_xyz', [-0.1, 0.1, -0.1, 0.1, -0.1, 0.1]) ps = ProblemSolver (robot) r = Viewer (ps) cl = robot.client pp = PathPlayer (cl, r) # Patchwork of path q1 = [0, 0, 0, 1, 0, 0, 0] q2 = [0, 0, 0, 0.707106781, 0, 0, 0.707106781] # equivalent to : [0, 0, 0, -0.707106781, 0, 0, -0.707106781] q7 ps.setInitialConfig (q1); ps.addGoalConfig (q2) ps.solve (); ps.resetGoalConfigs () q3 = [0, 0, 0, 0, 0, 0, 1] # equivalent to : [0, 0, 0, 0, 0, 0, -1] q8 ps.setInitialConfig (q2); ps.addGoalConfig (q3) ps.solve (); ps.resetGoalConfigs () q4 = [0, 0, 0, -0.707106781, 0, 0, 0.707106781] # equivalent to : [0, 0, 0, 0.707106781, 0, 0, -0.707106781] q9 ps.setInitialConfig (q3); ps.addGoalConfig (q4) ps.solve (); ps.resetGoalConfigs () q5 = [0, 0, 0, -1, 0, 0, 0] # equivalent to : [0, 0, 0, 1, 0, 0, 0] q1, q10 ps.setInitialConfig (q4); ps.addGoalConfig (q5) ps.solve (); ps.resetGoalConfigs ()
import time from hpp.corbaserver.motion_prior.client import Client as MPClient import rospy import sys #from hrp2_irreducible import Robot from hrp2 import Robot robot = Robot () robot.setJointBounds ("base_joint_xyz", [-1, 1, -3, 3, 0, 2]) from hpp_ros import ScenePublisher, PathPlayer publisher = ScenePublisher(robot) from hpp.corbaserver import ProblemSolver solver = ProblemSolver (robot) solver.resetGoalConfigs() #robot.setTranslationBounds (-0.5, 0.5, -3, 3, 0, 1) #robot.setJointBounds ("base_link", [-0.5, 0.5]) #names = robot.getJointNames() #q_init = robot.getInitialConfig () #print q_init #q_goal = q_init [::] #zfloor = 0.648 #q_init[0:3] = [-0.2, 0.5, zfloor] #q_goal[0:3] = [0.3, 0.8, zfloor] #original from antonio zfloor=0.64870180180254433111 #zfloor=0.74970180180254433111 #q1=[0,1.5,zfloor,0.9249114088877176,0,0,-0.38018270043406405,0,0,0,0,0.26179900000000000393,0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.26179900000000000393,-0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0,0,-0.45378600000000002268,0.87266500000000002402,-0.41887900000000000134,0,0,0,-0.45378600000000002268,0.87266500000000002402,-0.41887900000000000134,0]