Python ProblemSolver.optimizePath Examples

Example #1

r.client.gui.writeNodeFile(0, 'scene.osg')
# osgconvd -O NoExtras scene.osg scene.dae
from hpp.gepetto.blender.exportmotion import exportStates, exportPath
exportPath(r, cl.robot, cl.problem, 0, 1, 'path2.txt')
exportStates(r, cl.robot, q11, 'configs.txt')
from gepettoimport import loadmotion
loadmotion('/local/mcampana/devel/hpp/videos/path2.txt') # and rename first node manually ...

# --------------------------------------------------------------------#
## DEBUG commands
cl.obstacle.getObstaclePosition('decor_base')
robot.isConfigValid(q1)
robot.setCurrentConfig(q1)
res=robot.distancesToCollision()
r( ps.configAtParam(0,5) )
ps.optimizePath (0)
ps.clearRoadmap ()
ps.resetGoalConfigs ()
from numpy import *
argmin(robot.distancesToCollision()[0])
robot.getJointNames ()
robot.getConfigSize ()
r.client.gui.getNodeList()


['0_scene_hpp_', 'inclined_plane_3d', 'inclined_plane_3d/l_one', 'inclined_plane_3d/l_one_0', 'inclined_plane_3d/obstacle_base', 'inclined_plane_3d/obstacle_base_0', 'robot', 'robot/base_link', 'robot/base_link_0']

# get all theta:
for i in range(0,len(wp)):
    print wp[i][10]

Example #2

File: gravity.py Project: mylene-campana/gravity_description

q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0]

robot.isConfigValid(q1)
robot.isConfigValid(q2)

# qf should be invalid
qf = [1, -3, 3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
robot.isConfigValid(qf)

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

ps.solve ()
ps.pathLength(0)

ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(1)

ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)

pp(ps.numberPaths()-1)


r(ps.configAtParam(0,2))
ps.getWaypoints (0)

Example #3

import datetime as dt
totalSolveTime = dt.timedelta (0)
totalOptimTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
    ps.clearPathOptimizers()
    ps.clearRoadmap ()
    ps.resetGoalConfigs ()
    ps.setInitialConfig (q_init)
    ps.addGoalConfig (q_goal)

    t1 = dt.datetime.now ()
    ps.solve ()
    t2 = dt.datetime.now ()
    ps.addPathOptimizer ("SplineGradientBased_bezier3")
    ps.optimizePath (ps.numberPaths() - 1)
    t3 = dt.datetime.now ()

    totalSolveTime += t2 - t1
    totalOptimTime += t3 - t2
    print "Solve:", t2-t1
    print "Optim:", t3-t2
    n = len (ps.client.problem.nodes ())
    totalNumberNodes += n
    print ("Number nodes: " + str(n))

print ("Average solve time: " + str ((totalSolveTime.seconds+1e-6*totalSolveTime.microseconds)/float (N)))
print ("Average optim time: " + str ((totalOptimTime.seconds+1e-6*totalOptimTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))

Example #4

File: puzzle_test.py Project: mylene-campana/puzzle_description

r(q1)

import numpy as np
"""
ps.addPathOptimizer("Prune")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
len(ps.getWaypoints (ps.numberPaths()-1))
"""
ps.clearPathOptimizers()
cl.problem.setAlphaInit (0.05)
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)

tGB = cl.problem.getTimeGB ()
timeValuesGB = cl.problem.getTimeValues ()
gainValuesGB = cl.problem.getGainValues ()
newGainValuesGB = ((1-np.array(gainValuesGB))*100).tolist() #percentage of initial length-value

ps.clearPathOptimizers()
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(ps.numberPaths()-1)

ps.clearPathOptimizers()
ps.addPathOptimizer('PartialShortcut')

Example #5

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)

# r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "kitchen/")

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

print(ps.solve())

ps.addPathOptimizer("RandomShortcut")

print(ps.optimizePath(0))

Example #6

File: squel.py Project: mylene-campana/room_description

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

#cl.obstacle.loadObstacleModel('room_description','room','')
#cl.obstacle.loadObstacleModel('room_description','walls','')

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

ps.addPathOptimizer("GradientBased")
begin = time.time()
ps.optimizePath(0)
end = time.time()
print "Optim time: " + str(end - begin)

cl.problem.getIterationNumber()
ps.pathLength(0)
ps.pathLength(1)

ps.optimizePath(1)  # first use of active comp 1.601s  19iter
ps.pathLength(2)

len(ps.getWaypoints(0))

pp(1)

## Video recording

Example #7

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")

ps.optimizePath(nInitialPath)
ps.pathLength(ps.numberPaths()-1)

ps.getWaypoints (nInitialPath)
ps.getWaypoints (ps.numberPaths()-1)



"""
from hpp.gepetto import Viewer, PathPlayer
Viewer.withFloor = True
r = Viewer (ps)
pp = PathPlayer (cl, r)
r.loadObstacleModel ("robot_2d_description","cylinder_obstacle","cylinder_obstacle")
"""

Example #8

File: script.py Project: airobert/hpp_tutorial

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)

# r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "kitchen/")

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

print ps.solve ()

ps.addPathOptimizer ("RandomShortcut")

print ps.optimizePath (0)

Example #9

File: squel.py Project: mylene-campana/room_description

"""

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

#cl.obstacle.loadObstacleModel('room_description','room','')
#cl.obstacle.loadObstacleModel('room_description','walls','')

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

ps.addPathOptimizer("GradientBased")
begin=time.time()
ps.optimizePath (0)
end=time.time()
print "Optim time: "+str(end-begin)

cl.problem.getIterationNumber ()
ps.pathLength(0)
ps.pathLength(1)

ps.optimizePath (1)  # first use of active comp 1.601s  19iter
ps.pathLength(2)

len(ps.getWaypoints (0))

pp (1)

Example #10

#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-RRT.rdm')

ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)

ps.solve ()
ps.pathLength(0)
len(ps.getWaypoints (0))

#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm')


ps.addPathOptimizer("Prune") # NO CHANGE WITH PRM+DISCR
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
len(ps.getWaypoints (ps.numberPaths()-1))

ps.clearPathOptimizers()
cl.problem.setAlphaInit (0.3)
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
tGB = cl.problem.getTimeGB ()
timeValuesGB = cl.problem.getTimeValues ()
gainValuesGB = cl.problem.getGainValues ()
newGainValuesGB = ((1-np.array(gainValuesGB))*100).tolist() #percentage of initial length-value

Example #11

File: puzzle_test2.py Project: mylene-campana/puzzle_description

q8 = [0, 0, 0, 0.707106781, 0, 0, -0.707106781]
ps.setInitialConfig (q7); ps.addGoalConfig (q8)
ps.solve (); ps.resetGoalConfigs ()

q9 = [0, 0, 0, 1, 0, 0, 0]
ps.setInitialConfig (q8); ps.addGoalConfig (q9)
ps.solve (); ps.resetGoalConfigs ()
"""
# Only path to do a useless turn ! (Not the case if solved separately)
# r( ps.configAtParam(7,2.17) ) = [0.0, 0.0, 0.0, -0.9999559023922103, 0.0, 0.0, -0.00939112724262876]

ps.setInitialConfig (q1); ps.addGoalConfig (q5) # do a turn as expected
ps.solve ()

ps.optimizePath (4) # reduced to a point as expected

len(ps.getWaypoints (0))
ps.pathLength(5) # = 0


##############################"
# TEST quaternions x0 HRP2 + optimisation = meme resultat (-1) (un ou 2 tours)?
# Oui mais il faut imposer alpha = alpha_init et ne pas MàJ H1_
# les valeurs propres de la Hessienne sont bien >0
q1 = [0, 0, 0, 0.9999028455952614, -0.00643910090823595, -0.012362740316661774, 1.3620998722148461e-06]
q2 = [0, 0, 0, 0.8258496711518952, -0.2042733013060623, -0.19724860126354768, -0.4871732015735299]
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
ps.solve (); ps.resetGoalConfigs ()

q3 = [0, 0, 0, 0.6659085913630002, -0.04107471639409278, 0.06948325706470262, -0.7416540248726288]

Example #12

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 (); ps.resetGoalConfigs ()
# pp(3) = p0 final

#ps.addPathOptimizer("GradientBased")
#ps.addPathOptimizer("Prune")
ps.addPathOptimizer("PartialRandomShortcut")
ps.optimizePath(3) # pp(4) = p1 final

ps.pathLength(3)
ps.pathLength(4)
ps.getWaypoints (3)
ps.getWaypoints (4)
# should be [-0.07 0] [0.07 0] if alpha_init=1


"""
q1 = [-2, 0]; q2 = [-1, 1]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.resetGoalConfigs ()
q1 = [-1, 1]; q2 = [-1.2, 1.8]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.resetGoalConfigs ()

Example #13

File: tutorial_2.py Project: mylene-campana/hpp_tutorial

#ps.createPositionConstraint ("posConstraint", "torso_lift_joint", "", [0,0,0], [-3.25, -4.0, 0.790675], [1,1,0])
#ps.setNumericalConstraints ("constraints", ["posConstraint"])


ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") # environment
ps.setInitialConfig (q_init); ps.addGoalConfig (q_goal)

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

ps.addPathOptimizer("GradientBased")
begin=time.time()
ps.optimizePath(0)
end=time.time()
print "Optim time: "+str(end-begin)
cl = robot.client
cl.problem.getIterationNumber ()
cl.problem.getComputationTime ()

ps.pathLength(0)
ps.pathLength(1)

begin=time.time()
ps.optimizePath(1)
end=time.time()
print "Optim2 time: "+str(end-begin)
cl.problem.getIterationNumber ()

Example #14

File: hyq_slalom_debris_path.py Project: teguhSL/hpp-rbprm-corba

ps.addGoalConfig(q_goal)

# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation", 0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")

# Solve the planning problem :
t = ps.solve()
print "Guide planning time : ", t

ps.optimizePath(1)
pid = ps.numberPaths() - 1
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.1
#pp.displayVelocityPath(pid)
#v.client.gui.setVisibility("path_2_root","ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(0)

# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)

Example #15

File: script.py Project: nassimeblinlaas/altran_hpp_tutorial

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)

# r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "kitchen/")

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

print ps.solve()

ps.addPathOptimizer("RandomShortcut")

print ps.optimizePath(0)

Example #16

q_goal[2] = Z_VALUE_WOOD
ps.resetGoalConfigs()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)

# Solve the planning problem :
t = ps.solve()
print("Guide planning done in " + str(t))

pidLast = ps.numberPaths() - 1

# concatenate paths :
pid = pidBegin
ps.concatenatePath(pid, pidMiddle)
ps.concatenatePath(pid, pidLast)
"""
ps.optimizePath(pid)
pid = ps.numberPaths()-1
"""
try:
    # display solution :
    from hpp.gepetto import PathPlayer
    pp = PathPlayer(v)
    pp.dt = 0.1
    pp.displayPath(pid)  #pp.displayVelocityPath(0) #
    v.client.gui.setVisibility("path_" + str(pid) + "_root", "ALWAYS_ON_TOP")
    pp.dt = 0.01
    pp(pid)
except Exception:
    pass

Example #17

File: anymal_slalom2_path.py Project: nim65s/hpp-rbprm-corba

# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
ps.addPathOptimizer ("RandomShortcutDynamic")
#ps.addPathOptimizer ("RandomShortcut")
# Solve the planning problem :
t = ps.solve ()
print("Guide planning time : ",t)
#v.solveAndDisplay('rm',2,0.001)
#v.client.gui.removeFromGroup("rm",v.sceneName)
pid = ps.numberPaths()-1

for i in range(5):
    print("Optimization pass ",i)
    ps.optimizePath(pid)
    pid = ps.numberPaths()-1


# display solution : 
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayVelocityPath(pid)
#v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
pp.dt = 0.01
pp(pid)

# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2

Example #18

File: rotation_optimization.py Project: mylene-campana/puzzle_description

    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)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)

pp(ps.numberPaths()-1)


ps.configAtParam(2,0.5)
r(ps.configAtParam(0,2))
ps.getWaypoints (0)
ps.getWaypoints (ps.numberPaths()-1)

# plot paths
import numpy as np
dt = 0.1
nPath = ps.numberPaths()-1

Example #19

ps.resetGoalConfigs()
ps.setInitialConfig(q8)
ps.addGoalConfig(q9)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q9)
ps.addGoalConfig(q10)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q1)
ps.addGoalConfig(q10)
ps.solve()
print ps.pathLength(9)

ps.addPathOptimizer("GradientBased")
ps.optimizePath(9)
ps.numberPaths()
print ps.pathLength(ps.numberPaths() - 1)
"""
# pp(9) = p0 final
ps.optimizePath(9) # pp(10) = p1 final

ps.pathLength(9)
ps.pathLength(10)


ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (9)
ps.pathLength(10)

ps.clearPathOptimizers()