q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
r (q_init)
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
r (q_goal)
ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner ("PRM")
ps.solve ()
from hpp_ros import PathPlayer
pp = PathPlayer (robot.client, r)
pp (0)
pp (1)
raise RuntimeError ("Failed to apply constraint.")
if not(robot.isConfigValid(q1proj)):
raise RuntimeError ("Projected config non valid.")
res = ps.applyConstraints (q2)
if res [0]:
q2proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
if not(robot.isConfigValid(q2proj)):
raise RuntimeError ("Projected config non valid.")
ps.setInitialConfig (q1proj); ps.addGoalConfig (q2proj)
ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") # environment
#RRT-connect: path of 36s with 68 waypoints (2600 var to optimize)
#ps.selectPathPlanner ("VisibilityPrmPlanner")
ps.solve ()
ps.optimizePath(0)
pp = PathPlayer (robot.client, r)
pp (0)
pp (1)
len(ps.getWaypoints (0))
robot.client.problem.getIterationNumber ()
#q1=[0,1.5,0.64870180180254433111, 1,0,0,0 ,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]
#original from antonio
q2=[0,-2.5,0.64870180180254433111, 0.7101853756232854, 0, 0, -0.7040147244559684, 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]
#q2=[0,-2.5,0.64870180180254433111, 1,0,0,0, 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]
publisher = ScenePublisher(robot)
publisher(q2)
time.sleep(1)
publisher(q1)
## Add constraints
solver = ProblemSolver (robot)
print "diffusing planner (general RRT implementation)"
solver.selectPathPlanner("DiffusingPlanner")
solver.loadObstacleFromUrdf("hpp_ros","wall")
solver.setInitialConfig (q1)
solver.addGoalConfig (q2)
#pc.invokeIrreduciblePlanner()
#mpc = MPClient()
#pc = mpc.precomputation
#cnames = pc.getNumericalConstraints (robot.getInitialConfig())
#solver.setNumericalConstraints ("stability-constraints", cnames)
## add obstacles
print "solve"
start_time = float(time.time())
solver.solve ()
end_time = float(time.time())
seconds = end_time - start_time
q1r = [1,0,0,0]
q1=[0,0.0,zfloor,q1r[0],q1r[1],q1r[2],q1r[3],0.0,0.0,0.0,0.0,0.0,0.0,-0.0,0.0,0.0,0.0]
q2 = q1[::]
q2[1] += 0.1
#qI = robot.shootRandomConfig()
#qG = robot.shootRandomConfig()
qI = robot.getInitialConfig()
qG = robot.getGoalConfig()
#qI = q1
#qG = q2
#solver.loadObstacleFromUrdf("hpp-motion-prior","floor","")
solver.loadObstacleFromUrdf("hpp-motion-prior","floor_obstacle","")
#solver.loadObstacleFromUrdf("hpp-motion-prior","wall","")
## Add constraints
#qI[1]=1.0
solver.setInitialConfig (qI)
solver.addGoalConfig (qG)
#publisher(q)
#print q
#sys.exit(0)
qqinit = solver.getInitialConfig()[0:3]
qqgoal = solver.getGoalConfigs()[0][0:3]
print "planning from",qqinit,"to",qqgoal
#print "Irreducible Planner"
