Python ProblemSolver.selectPathPlanner方法代码示例

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
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)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
r.loadObstacleModel ("puzzle_description","decor_easy","decor_easy")
r(q2) # r(q1)

#q1bis = q2; q2bis = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#ps.resetGoalConfigs (); ps.setInitialConfig (q1bis); ps.addGoalConfig (q2bis); ps.solve ()
# ps.resetGoalConfigs (); ps.setInitialConfig (q1); ps.addGoalConfig (q2bis); ps.solve ()
#i = ps.numberPaths()-1

ps.setInitialConfig (q1); ps.addGoalConfig (q2)

ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/puzzle_veryEasy_PRM.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_RRT.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM1.rdm') # srand # problem ?
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM1.rdm') # srand 1453110445(909sec) [COLL!]
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM2.rdm') # srand  # just after solve, GB OK. But after readroadmap+solve, segfault quaternions....
ps.readRoadmap ('/local/mcampana/devel/hpp/data/puzzle_easy_PRM_test1.rdm') #srand 1454520599 working0.05
# srand 1454521537 (no RM saved) works 7 -> 5, best 0.2
ps.solve ()
ps.pathLength(0)
len(ps.getWaypoints (0))



r(q1)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
q1=[0,1.5,0.64870180180254433111, 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]
#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())

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
from hpp.corbaserver.box import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer
from hpp.gepetto import PathPlayer
white=[1.0,1.0,1.0,1.0]
green=[0.23,0.75,0.2,0.5]
yellow=[0.85,0.75,0.15,1]
pink=[1,0.6,1,1]
orange=[1,0.42,0,1]

robot = Robot ('box')
robot.setJointBounds ("base_joint_xyz", [0,5,0,2,0,2])
robot.client.robot.setDimensionExtraConfigSpace(robot.getNumberDof()) # extraDof for velocitiy

ps = ProblemSolver (robot)
ps.selectPathPlanner("dyn")
v = Viewer (ps)

#v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")
v.loadObstacleModel ("iai_maps", "abstract", "abstract")


q_init = [0,1,1,1,0,0,0]
q_goal = [5,1,1,0.9239,0,-0.3827,0]
v (q_init)

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

ps.addPathOptimizer ("RandomShortcut")

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
ps = ProblemSolver (robot)
v = Viewer (ps)
q_init = robot.getCurrentConfig ()

q_init[0:3] = [0.5, 1, 1]	#z=0.4 pour sphere
v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [5,1, 1]
#v (q_goal)

print("chargement map")
v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")



ps.selectPathPlanner("rrtPerso")
print("Debut motion planning")

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

v.solveAndDisplay(white,0.02,1,yellow)


ps.solve ()


v.displayRoadmap(white,0.02,1,yellow)


v.client.gui.addXYZaxis("test0",white,0.05,1)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
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"
#solver.selectPathPlanner("PRM")
#solver.selectPathPlanner("DiffusingPlanner")
print "solve"
start_time = float(time.time())

solver.selectPathPlanner("IrreduciblePlanner")
solver.solve ()

end_time = float(time.time())
seconds = end_time - start_time
hours = seconds/3600
print("--- %s seconds ---" % seconds)
print("--- %s hours   ---" % hours)
fname0 = "../data-traj/rrt-wall0.tau"
fname1 = "../data-traj/rrt-wall1.tau"

pathplayer = PathPlayer (robot.client, publisher)

pathplayer(0)
pathplayer(1)
pathplayer.toFile(0,fname0)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
ps.lockJoint ('base_joint_xy', [0,0])
ps.lockJoint ('base_joint_rz', [1,0])

res = ps.applyConstraints (q_init)
if res [0]:
    q_init = res [1]
else:
    raise RuntimeError ('Failed to apply constraints on init config.')

res = ps.applyConstraints (q_goal)
if res [0]:
    q_goal = res [1]
else:
    raise RuntimeError ('Failed to apply constraints on goal config.')

robot.setCurrentConfig (q_init)
r = vf.createViewer (collisionURDF = True)
r (q_init)

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

ps.selectPathPlanner ("DiffusingPlanner")
ps.selectPathValidation("Progressive", 0.01)
#ps.selectPathValidation("Progressive", 0.05)
#ps.readRoadmap ('/home/florent/devel/fiad/data/roadmap.rdm')

# ps.solve ()

pp = PathPlayer (ps.client, r)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
from hpp.corbaserver.lydia import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer, PathPlayer

robot = Robot ('lydia')
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1.1, 1.1])
ps = ProblemSolver (robot)
r = Viewer (ps)

r.loadObstacleModel ("hpp_benchmark", "obstacle", "obstacle")

q_init = robot.getCurrentConfig ()
q_goal = q_init [::]

q_init [2] = -0.6
q_goal [2] =  0.6

ps.selectPathPlanner ("VisibilityPRM")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.solve ()

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)

from hpp.gepetto import Viewer
gui = Viewer (ps)

gui.loadObstacleModel ('tp-rrt', "scene", "scene")

q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = q_init[0:2]=[-3.7, -4]; gui(q_init)
gui (q_init)

q_goal [0:2] = [15,2]
gui (q_goal)

#~ ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner ("PlannerTP")
ps.addPathOptimizer ("RandomShortcut")

t = ps.solve ()
print ("solving time", t)


from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, gui)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
q1 = [0, 0, 0.9, 0, 0, 0]; q2 = [0, 0, -0.9, 0, 0, 0]

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
r(q1)

ps.setInitialConfig (q1); ps.addGoalConfig (q2)

# Load box obstacle in HPP for collision avoidance
cl.obstacle.loadObstacleModel('puzzle_description','decor_very_easy','')
#cl.obstacle.loadObstacleModel('puzzle_description','decor_easy','')
#cl.obstacle.loadObstacleModel('puzzle_description','decor','')

ps.selectPathPlanner ("VisibilityPrmPlanner") # 26min solve time
begin=time.time()
ps.solve ()
end=time.time()
print "Solving time: "+str(end-begin)
ps.pathLength(0)

ps.addPathOptimizer("GradientBased")
begin=time.time()
ps.optimizePath (0)
end=time.time()
print "Optim time: "+str(end-begin)
cl.problem.getIterationNumber ()
ps.pathLength(1)
#vPRM: 24 iter, 25.3s->21s, weighted Cost + comp linear error
#RRTc: 30 iter, 33.4s->14.9s, weighted Cost + comp linear error

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
#q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
#set goal velocity (along x,y,z axis) : 
q_goal[-6:-3]=[0,0,0]

vf.loadObstacleModel ("iai_maps", "room", "room")
# with displayArrow parameter the viewer will display velocity and acceleration of the center of the robot with 3D arrow. The length scale with the amplitude with a length of 1 for the maximal amplitude
v = vf.createViewer(displayArrows = True)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

ps.addPathOptimizer ("RandomShortcut")
#select kinodynamic methods : 
ps.selectSteeringMethod("Kinodynamic")
ps.selectDistance("Kinodynamic")
# the Kinodynamic steering method require a planner that build directionnal roadmap (with oriented edges) as the trajectories cannot always be reversed. 
ps.selectPathPlanner("BiRRTPlanner")


print (ps.solve ())

# display the computed roadmap. Note that the edges are all represented as straight line and may not show the real motion of the robot between the nodes : 
v.displayRoadmap("rm")

#Alternatively, use the following line instead of ps.solve() to display the roadmap as it's computed (note that it slow down a lot the computation)
#v.solveAndDisplay('rm',1)

# Highlight the solution path used in the roadmap
v.displayPathMap('pm',0)

# remove the roadmap for the scene : 
#v.client.gui.removeFromGroup("rm",v.sceneName)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import selectPathPlanner [as 别名]
q_init = robot.getCurrentConfig ()

q_init[0:3] = [6, 7, 0.5]	#z=0.4 pour sphere
v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [-2.5, -3.5, 0.5]
#v (q_goal)

print("chargement map")
v.loadObstacleModel ("iai_maps", "room", "room")

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

ps.selectPathPlanner("test2")

print("debut motion plannning")

tps1 = time.clock()
ps.solve ()
tps2 = time.clock()
print(tps2-tps1)


from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, v)
#print("affichage solution")
#pp (0)
while 1 :
	print("Appuyer sur entree pour afficher la solution")