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]
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)
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)))
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')
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))
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
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")
"""
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)
"""
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)
#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
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]
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 ()
#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 ()
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)
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)
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
# 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
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
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()