Python ProblemSolver.getWaypoints方法代码示例

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2

plotFrame (r, "_", [0,0,1.5], 0.5)

# First parabolas: vmax = 8m/s,  mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(4.5)
plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2")
ps.clearRoadmap();
solveTime = ps.solve () # 0.312 s
pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff
samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1])
plotConeWaypoints (cl, pahtId, r, "wp0", "friction_cone2")
plotSpheresWaypoints (cl, pahtId, r, "sphere_wp0", [0,0,1,1], 0.02)
print "solve duration: " + str(solveTime)
print "path length: " + str(ps.pathLength(pahtId))
print "number of waypoints: " + str(len(ps.getWaypoints (pahtId)))
print "number of nodes: " + str(ps.numberNodes ())
cl.problem.getResultValues()

# Second parabolas: vmax = 6.5m/s,  mu = 0.5
plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone")
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(4.5)
ps.clearRoadmap();
solveTime = ps.solve () # 0.9689 s
pahtId = ps.numberPaths()-offsetOrientedPath
samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [1,0,0,1])
plotConeWaypoints (cl, pahtId, r, "wp2", "friction_cone")
plotSpheresWaypoints (cl, pahtId, r, "sphere_wp2", [1,0,0,1], 0.02)
print "solve duration: " + str(solveTime)
print "path length: " + str(ps.pathLength(pahtId))
print "number of waypoints: " + str(len(ps.getWaypoints (pahtId)))

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
#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
lineNamePrefix = "optimized_path"
for t in np.arange(0., cl.problem.pathLength(nPath), dt):
    lineName = lineNamePrefix+str(t)
    r.client.gui.addLine(lineName,[cl.problem.configAtParam(nPath, t)[0],cl.problem.configAtParam(nPath, t)[1],0],[cl.problem.configAtParam(nPath, t+dt)[0],cl.problem.configAtParam(nPath, t+dt)[1],0],[0,1,0.3,1])
    r.client.gui.addToGroup (lineName, r.sceneName)

nPath = nInitialPath
lineNamePrefix = "initial_path"
for t in np.arange(0., cl.problem.pathLength(nPath), dt):

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
ps.setInitialConfig (q1proj); ps.addGoalConfig (q2proj)
ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") # environment

#RRT-connect: path of 36s with 68 waypoints (2600 var to optimize)
#ps.selectPathPlanner ("VisibilityPrmPlanner")

ps.solve ()
ps.optimizePath(0)


pp = PathPlayer (robot.client, r)

pp (0)
pp (1)

len(ps.getWaypoints (0))
robot.client.problem.getIterationNumber ()
ps.pathLength(0)
ps.pathLength(1)


## Video recording
r.startCapture ("capture","png")
pp(0)
r.stopCapture ()

## DEBUG commands
cl.obstacle.getObstaclePosition('obstacle_base')
cl.robot.getJointOuterObjects('CHEST_JOINT1')
cl.robot.getCurrentConfig()
robot.isConfigValid(q1)

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


## Video recording
pp.dt = 0.02
r.startCapture ("capture","png")
#pp(1)
pp(ps.numberPaths()-1)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_envir3d_with_window.rdm')

# start to bottom
ps.resetGoalConfigs ()
robot.setJointBounds('base_joint_xyz', [-6, 6.7, -2.5, 3.2, 0, 3]) # start to bottom
q11 = [5.7, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]
q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0]

cl.problem.generateValidConfig(2)

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


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

# Get projected random configs CONES and display them
num_log = 7308
qrands = parseConfig(num_log,'INFO:/local/mcampana/devel/hpp/src/hpp-core/src/parabola/parabola-planner.cc:157: q_proj: ')
sphereNamePrefix="qrand_sphere_"
for i in range(0,len(qrands)):
    qrand = qrands[i]
    #sphereName = sphereNamePrefix+str(i)
    #r.client.gui.addSphere (sphereName,0.1,[0.8,0.1,0.1,1]) # red
    #r.client.gui.applyConfiguration (sphereName, [qrand[0],qrand[1],qrand[2],1,0,0,0])
    #r.client.gui.addToGroup (sphereName, r.sceneName)
    coneName = sphereNamePrefix+str(i)
    qCone = cl.robot.setOrientation (qrand.tolist())
    r.loadObstacleModel ("animals_description","friction_cone2",coneName)
    r.client.gui.applyConfiguration (coneName, qCone[0:7])

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
# q = [x, y, theta] # (z not considered since planar)
q1 = [-4, 4, 1, 0]; q2 = [4, -4, 1, 0] # obstS 1
#q1 = [-4, 4, 0]; q2 = [4, -4, 0] # obstS 1

ps.setInitialConfig (q1); ps.addGoalConfig (q2)
cl.obstacle.loadObstacleModel('potential_description','obstacles_concaves','obstacles_concaves')

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

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
# Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta]
q1 = [0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0]
q2 = [1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0]


from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("animals_description","cave","cave")
#cl.obstacle.loadObstacleModel('animals_description','cave','cave')
r(q2)

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

# verif orientation:
r(ps.getWaypoints (0)[0]) # ref
r(ps.getWaypoints (1)[0]) # should be good...

index = cl.robot.getConfigSize () - 4

q = q1[::]
plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale")
plotCone (q1, cl, r, 0.5, 0.2, "cone1")
plotCone (q2, cl, r, 0.5, 0.2, "cone2")

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

plotPath (cl, 0, r, "pathy", 0.1) # time-step should depend on sub-path length ?

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
#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')
    f.write('number of waypoints: '+str(pathNumberWaypoints)+'\n')
    f.write('number of nodes: '+str(roadmapNumberNodes)+'\n')

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import getWaypoints [as 别名]
# q = [x, y, theta] # (z not considered since planar)
q1 = [-2, 0, 1, 0]; q2 = [2, 0, 1, 0]

ps.setInitialConfig (q1); ps.addGoalConfig (q2)
cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','')

ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1]) # OK
ps.setNumericalConstraints ("constraints", ["orConstraint"])

ps.solve ()

ps.configAtParam(0,2)
ps.configAtParam(0,6)
ps.configAtParam(0,13)
ps.getWaypoints (0)[1]

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

ps.configAtParam(1,2)
ps.configAtParam(1,6)
ps.configAtParam(1,13)
ps.getWaypoints (1)[1]

len(ps.getWaypoints (0))
cl.problem.getIterationNumber()


"""

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

len(ps.getWaypoints (0))

ps.optimizePath(3)
ps.pathLength(4)
cl.problem.getIterationNumber ()

pp (0)
pp (1)

## Video recording
r.startCapture ("capture","png")
pp(1)
r.stopCapture ()
r.startCapture ("capture","png")
pp(2)
r.stopCapture ()

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

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

samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1])

#ps.resetGoalConfigs ()
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_ant_in_room1.rdm')

wp = ps.getWaypoints (1)

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

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


## 3D Plot tools ##
q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0];
r(q0)

plotFrame (r, "_", [0,0,4], 0.5)

plotPath (cl, 0, r, "pathy", 0.1)