Python ProblemSolver.solve方法代码示例

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
qt = [0, 0, 0, math.sqrt (1/3.), math.sqrt (1/3.), 0.0, math.sqrt (1/3.)]
qt2 = [0, 0, 0, math.sqrt (1 - 0.95**2), 0.95, 0.0, 0.0]

Q.append (qt)
Q.append (qt2)
Q.append ( [0, 0, 0, 0.99, 0.14003571, 0.013, 0.011]) # to remove

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)

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


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

plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot
nPointsPlot = 50
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

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
solver = ProblemSolver (robot)
print "diffusing planner (general RRT implementation)"
solver.selectPathPlanner("DiffusingPlanner")
solver.loadObstacleFromUrdf("hpp_ros","wall")

solver.setInitialConfig (q1)
solver.addGoalConfig (q2)

#pc.invokeIrreduciblePlanner()
#mpc = MPClient()
#pc = mpc.precomputation
#cnames = pc.getNumericalConstraints (robot.getInitialConfig())
#solver.setNumericalConstraints ("stability-constraints", cnames)
## add obstacles
print "solve"
start_time = float(time.time())
solver.solve ()
end_time = float(time.time())
seconds = end_time - start_time
hours = seconds/3600
print("--- %s seconds ---" % seconds)
print("--- %s hours   ---" % hours)
print "replay path"
fname = "../data-traj/rrt-wall.tau"

pathplayer = PathPlayer (client, publisher)
pathplayer(1)
pathplayer.toFile(1,fname)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
r(q11)

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 parabola(s): vmax = 6.8m/s,  mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(6.8)
ps.clearRoadmap();
solveTime = ps.solve () # 0.085 s
pathId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff

pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path0", [0,0,1,1])
#pp.displayPath(pathId)
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 parabola(s): vmax = 5.3m/s,  mu = 0.5
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(5.3)
ps.clearRoadmap();

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)

from hpp.gepetto import Viewer
gui = Viewer (ps)

gui.loadObstacleModel ('tp-rrt', "scene", "scene")

q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = q_init[0:2]=[-3.7, -4]; gui(q_init)
gui (q_init)

q_goal [0:2] = [15,2]
gui (q_goal)

#~ ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathPlanner ("PlannerTP")
ps.addPathOptimizer ("RandomShortcut")

t = ps.solve ()
print ("solving time", t)


from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, gui)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
# Difficult init config
q1 = [1.45, 1.05, -0.8, 1.0, 0.0, 0.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]

q2 = [6.55, -2.91, 1.605, 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]

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)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
ps.setInitialConfig (q1); ps.addGoalConfig (q2)

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

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
robot.setJointBounds('base_joint_xyz', [xStone-2, xStone+2, yEmu-1, yEmu+2, zEmu-0.5, zEmu+0.5])
# List of configs
q1 = [xStone+2, yEmu+0, zEmu, 1.0, 0.0, 0.0, 0.0]
q2 = [xStone+1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, 0.707106781]
q3 = [xStone+1, yEmu+1.4, zEmu, 0, 0, 0, 1]
q4 = [xStone+0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, 0.707106781]
#q5 = [xStone+0, yEmu+2, zEmu, -1, 0, 0, 0]
q5 = [xStone+0, yEmu+2, zEmu, -0.99, 0.14003571, 0.013, 0.011]
q6 = [xStone-0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, -0.707106781]
q7 = [xStone-1, yEmu+1.4, zEmu, 0, 0, 0, -1]
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)

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
# Load obstacles in HPP
cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','') # do not move (position in urdf)
cl.obstacle.loadObstacleModel('gravity_description','emu','') # loaded as an obstacle for now
position_emu= (0,-1,0.2,1,0,0,0)
cl.obstacle.moveObstacle ('emu_base', position_emu)

q1 = [-2, 0.01, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 0.8, -1.0, -1.0, 0.0, 0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.7, -0.8, 0.1, -0.523599, 0.1, -0.3, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.6, 0.1, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]

q2 = [2, 0.01, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 0.8, -1.0, -1.0, 0.0, 0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.7, -0.8, 0.1, -0.523599, 0.1, -0.3, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.6, 0.1, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]

qconf = [0, 0.01, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 0.8, -1.0, -1.0, 0.0, 0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.7, -0.8, 0.1, -0.523599, 0.1, -0.3, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.6, 0.1, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]

# Problem
ps.setInitialConfig (q3)
ps.addGoalConfig (q2)
ps.solve () # 0-1
ps.setInitialConfig (q2)
ps.resetGoalConfigs ()
ps.addGoalConfig (q2to4)
ps.solve () # 2-3
ps.setInitialConfig (q2to4)
cl.problem.resetGoalConfigs ()
ps.addGoalConfig (q4)
ps.solve () # 4-5
p(1) #to play the solution

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

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

plotSphere (q2, cl, r, "sphere1", [0,1,0,1], 0.02)
nPointsPlot = 60
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2

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

# First parabola: vmax = 7m/s,  mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(7)
plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2")
solveTime = ps.solve ()
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)))

# Second parabola: vmax = 7m/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(7)
ps.clearRoadmap();
solveTime = ps.solve ()
pahtId = ps.numberPaths()-offsetOrientedPath
samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path1", [0.1,0.8,0.8,1])

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
# pp (1)

# test
jp = [-4.016784835909555, -4.671737012514295, 0.9493205942691932, -0.19838422293030356, 0.17672477884262752, -0.35476407311266434, 0.8964120174695772]
ps.createPositionConstraint ("l_gripper", "l_gripper_tool_joint", "", [0,0,0], jp[:3], [1,1,1])
ps.client.problem.setGoalNumericalConstraints ("test",["l_gripper",], [0,])

# qq = [-3.2736323380281704, -4.919012557067956, -0.5645503720485425, 0.8253986172873397, 0.31, 2.4756472106318212, -0.10231121294734401, 0.5832544388135313, 0.9877368674476137, 0.351352194158052, -1.1240948972856866, 0.9832501749355624, -0.1822610586197337, -0.8650504112999698, 0.6602954608097523, -0.7510059283614456, 0.28134369878719734, 0.5218218893957427, 0.5020748972660279, 0.34837002692202573, 0.39307871732911037, 0.012744229982022303, 0.33261894418886817, -0.7484151440244099, -1.5567378515100605, -0.2601290957285226, -0.1203692551331449, -1.9576008301144934, 0.8124905337042794, -0.5829743842064892, -1.8222188044396315, -0.7752624281039475, -0.63163927013001, 0.5474106358659504, 0.11960478411037699, 0.2810869521336104, 0.45983350461154876, 0.3357266282624224, 0.026642845592760873, 0.34937864270125457]
# ps.addGoalConfig (qq)

time = list()
nbNode = list()
nbIter = 1
for i in range(nbIter):
    ps.clearRoadmap ()
    time.append (ps.solve ())
    nbNode.append(len(ps.nodes()))
    print i, time[-1], nbNode[-1]

i = -1
indexes=[]
while True:
    try:
        i = nbNode.index(2, i+1)
    except ValueError:
        break
    indexes.append (i)

import numpy as np
m = np.mean(np.array(time)[indexes],0)
print (m[2]*1000 + m[3]) / 1000., "sec."

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
r(q11)

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 = 8m/s,  mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(4.5)
ps.clearRoadmap();
solveTime = ps.solve () # 0.312 s  388 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
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(4.5)
ps.clearRoadmap();
solveTime = ps.solve () # 0.9689 s

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

robot = Robot ('puzzle_robot') # object5
robot.setJointBounds('base_joint_xyz', [-0.1, 0.1, -0.1, 0.1, -0.1, 0.1])

ps = ProblemSolver (robot)
r = Viewer (ps)
cl = robot.client
pp = PathPlayer (cl, r)

# Patchwork of path
q1 = [0, 0, 0, 1, 0, 0, 0]
q2 = [0, 0, 0, 0.707106781, 0, 0, 0.707106781]
# equivalent to : [0, 0, 0, -0.707106781, 0, 0, -0.707106781] q7
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
ps.solve (); ps.resetGoalConfigs ()

q3 = [0, 0, 0, 0, 0, 0, 1]
# equivalent to : [0, 0, 0, 0, 0, 0, -1] q8
ps.setInitialConfig (q2); ps.addGoalConfig (q3)
ps.solve (); ps.resetGoalConfigs ()

q4 = [0, 0, 0, -0.707106781, 0, 0, 0.707106781]
# equivalent to : [0, 0, 0, 0.707106781, 0, 0, -0.707106781] q9
ps.setInitialConfig (q3); ps.addGoalConfig (q4)
ps.solve (); ps.resetGoalConfigs ()

q5 = [0, 0, 0, -1, 0, 0, 0]
# equivalent to : [0, 0, 0, 1, 0, 0, 0] q1, q10
ps.setInitialConfig (q4); ps.addGoalConfig (q5)
ps.solve (); ps.resetGoalConfigs ()

# 需要导入模块: from hpp.corbaserver import ProblemSolver [as 别名]
# 或者: from hpp.corbaserver.ProblemSolver import solve [as 别名]
r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder")

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