Python ProblemSolver.optimizePath方法代码示例

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

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

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

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

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import optimizePath [as 别名]

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

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

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import optimizePath [as 别名]
"""

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)

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

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