Python ProblemSolver.numberPaths方法代码示例

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import numberPaths [as 别名]
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
r(q1)

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

offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2

#plotFrame (r, 'frame_group', [0,0,0], 0.6)

# First parabolas: vmax = 7m/s,  mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(7)
ps.clearRoadmap();
solveTime = ps.solve () # 299 nodes
pathId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff

pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path0", [0,0,1,1])
plotCone (q1, cl, r, "cone_first", "friction_cone_SG2"); plotCone (q2, cl, r, "cone_second", "friction_cone_SG2")
plotConeWaypoints (cl, pathId, r, "cone_wp_group", "friction_cone_WP2")

gui.writeNodeFile('cone_wp_group','cones_path.dae')
gui.writeNodeFile('cone_first','cone_start.dae')
gui.writeNodeFile('cone_second','cone_goal.dae')

# Second parabolas: vmax = 6.5m/s,  mu = 0.5  # DO NOT SOLVE FIRST PATH BEFORE
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(6.5)
ps.clearRoadmap();
solveTime = ps.solve () # 4216 nodes .... 37848 edges
pathId = ps.numberPaths()-offsetOrientedPath

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


# Add light to scene
lightName = "li4"
r.client.gui.addLight (lightName, r.windowId, 0.01, [0.4,0.4,0.4,0.5])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [1,0,0,1,0,0,0])
r.client.gui.refresh ()

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import numberPaths [as 别名]
r(Q[0])
for i in range(0, len(Q)):
    r(Q[i])
    time.sleep (0.5)

#robot.isConfigValid(Q[0])

for i in range(0, len(Q)-1):
    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)

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

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import numberPaths [as 别名]
q8 = [xStone-1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, -0.707106781]
q9 = [xStone-2, yEmu+0, zEmu, 1, 0, 0, 0]
r(q1)
robot.isConfigValid(q1)

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 (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 (q8); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q1); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs ()

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)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import numberPaths [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 numberPaths [as 别名]
#ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1])
#ps.setNumericalConstraints ("constraints", ["orConstraint"])

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

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

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 (1)
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()

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import numberPaths [as 别名]
vmax = 8; mu = 1.2
#vmax = 6.5; mu = 0.5
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)

toSeconds = np.array ([60*60,60,1,1e-3])
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
imax=3;
f = open('results.txt','a')
    
# Assuming that seed in modified directly in HPP (e.g. in core::PathPlanner::solve or ProblemSolver constr)
for i in range(0, imax):
    print i
    ps.clearRoadmap ()
    solveTimeVector = ps.solve ()
    solveTime = np.array (solveTimeVector).dot (toSeconds)
    pathId = ps.numberPaths()-offsetOrientedPath
    pathLength = ps.pathLength (pathId)
    pathNumberWaypoints = len(ps.getWaypoints (pathId))
    roadmapNumberNodes = ps.numberNodes ()
    #TODO: number collisions (checked ???)
    #TODO: number parabola that has failed (because of which constraint ??)
    
    #ps.addPathOptimizer("Prune")
    #ps.optimizePath (pathId)
    #prunePathId = ps.numberPaths()-1
    
    # Write important results #
    f.write('Try number: '+str(i)+'\n')
    f.write('with parameters: vmax='+str(vmax)+' and mu='+str(mu)+'\n')
    f.write('solve duration: '+str(solveTime)+'\n')
    f.write('path length: '+str(pathLength)+'\n')

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import numberPaths [as 别名]
ps.solve ()
samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.2,1,1])

#r.client.gui.removeFromGroup ("ThetaPlane", r.sceneName)


q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)


ps.setInitialConfig (q1); ps.addGoalConfig (q2)
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(8)
ps.solve ()

samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.2,1,1])
samples = plotSampleSubPath (cl, r, ps.numberPaths()-2, 20, "curvy1", [0.2,0.2,0.8,1])
samples = plotSampleSubPath (cl, r, ps.numberPaths()-2, 20, "curvy2", [0.2,0.8,0.2,1])

plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone")
plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2")

plotConeWaypoints (cl, 0, r, "wp", "friction_cone")

r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF")

r(ps.configAtParam(0,0.001))
ps.pathLength(0)
ps.getWaypoints (0)

ps.clearRoadmap()

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import numberPaths [as 别名]
q8 = [8.5, 0.5, 0, 0.707106781, 0, 0, -0.707106781, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]

q9 = [8, 0, 0, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.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 (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 (q8); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs ()


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

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

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

pp(ps.numberPaths()-1)


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]