Python plotSampleSubPath 예제들

예제 #1

파일: benchmark4_blender.py 프로젝트: pFernbach/animals_description

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

pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path1", [1,0,0,1])
plotCone (q1, cl, r, "cone_first", "friction_cone"); plotCone (q2, cl, r, "cone_second", "friction_cone_SG")

예제 #2

파일: benchmark2mesh.py 프로젝트: pFernbach/animals_description

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,2.8], 0.5)

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

예제 #3

파일: benchmark5.py 프로젝트: mylene-campana/animals_description

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
pahtId = ps.numberPaths()-offsetOrientedPath
samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [1,0,0,1])

예제 #4

파일: scene_jump_test.py 프로젝트: mylene-campana/animals_description

#q22 = [2.2, -2.7, 0.8, 0, 0, 0, 1, 0, 0, 1, 0] # first goal
q22 = [1.2, 1.4, 0.5, 0, 0, 0, 1, 0, 0, 1, 0] # second goal
r(q22)
#q22 = [-4.4, -1.6, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # ultimate goal!


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

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


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

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

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

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


qwp1=[6.588526674447825, -1.8345785084341553, 0.36443200826644895, 0.1654550896480227, 0.07312786003135581, 0.25630565685312806, -0.9495179512057083, 5.547415126403182e-16, 0.0, 1.0, -1.5231360759319517]

qwp2=[6.376949224610323, -1.8304419228017523, 0.36443200826644895, -0.10812814084986927, 0.8453266741499781, -0.1992256556515323, 0.4837770739267257, 0.0, -1.486905914403694e-16, 1.0, -2.0916445213810797]

ps.setInitialConfig (qwp1); ps.addGoalConfig (qwp2); ps.solve ()


# Plot a cone and rotate it!
qCone = cl.robot.setOrientation (wp[6])

예제 #5

#r.loadObstacleModel ("animals_description","plane_3d","plane_3d")
#r.loadObstacleModel ("animals_description","multiple_planes_3d","multiple_planes_3d")
#r.loadObstacleModel ("animals_description","inclined_plane_3d","inclined_plane_3d")
r.loadObstacleModel ("animals_description","environment_3d","environment_3d")
#r.loadObstacleModel ("animals_description","environment_3d_with_window","environment_3d_with_window")
addLight (r, [-3,3,7,1,0,0,0], "li"); addLight (r, [3,-3,7,1,0,0,0], "li1")
r(q11)

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)
ps.solve () 
# PROBLEM !! not finding solution for environment_3d_window with mu=0.5 V0max=6.5 Projectionshooter ....  V0 or Vimp too much limiting ?? 'cause V0max=7 gives a too "easy" solution ...

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

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

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

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

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

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

예제 #6

파일: scene_jump_harder.py 프로젝트: mylene-campana/monopod_description

#q22 = [-1.7, -1.5, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # third goal
#q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # bottom of column
#q22 = [-3.3, 1.5, 3.4, 0, 0, 0, 1, 0, 0, 1, 0] # in column
#q22 = [-4.2, 0.9, 1.7, 0, 0, 0, 1, 0, 0, 1, 0] # bottom 1 of column
#q22 = [-4.4, 0.9, 4.1, 0, 0, 0, 1, 0, 0, 1, 0] # bottom 3 of column
q22 = [-4.4, -1.8, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # ultimate goal!
r(q22)



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

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

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

samples2 = plotSampleSubPath (cl, r, 2, 20, "curvy2", [0,0.4,0.7,1])

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

예제 #7

파일: scene_jump_harder.py 프로젝트: pFernbach/animals_description

#q22 = [-1.7, -1.5, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # third goal
#q22 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # bottom of column
#q22 = [-3.3, 1.5, 3.4, 0, 0, 0, 1, 0, 0, 1, 0] # in column
#q22 = [-4.2, 0.9, 1.7, 0, 0, 0, 1, 0, 0, 1, 0] # bottom 1 of column
#q22 = [-4.4, 0.9, 4.1, 0, 0, 0, 1, 0, 0, 1, 0] # bottom 3 of column
q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # ultimate goal!
r(q22)

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

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

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

samples2 = plotSampleSubPath (cl, r, 2, 20, "curvy2", [0,0.4,0.7,1])

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

예제 #8

파일: benchmark1.py 프로젝트: mylene-campana/animals_description

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])
plotConeWaypoints (cl, pahtId, r, "wp1", "friction_cone")
plotSpheresWaypoints (cl, pahtId, r, "sphere_wp1", [0.1,0.8,0.8,1], 0.02)

예제 #9

파일: simple_plane_test.py 프로젝트: pFernbach/animals_description

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","plane_3d","plane_3d")
addLight (r, [-3,3,7,1,0,0,0], "li"); addLight (r, [3,-3,7,1,0,0,0], "li1")
r(q11)

plotFrame (r, "_", [0,0,3.1], 0.5)
plotThetaPlane (q11, q22, r, "ThetaPlane")
plotCone (q11, cl, r, "cone1", "friction_cone"); plotCone (q22, cl, r, "cone12", "friction_cone")

ps.setInitialConfig (q11); ps.addGoalConfig (q22)
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(15)
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])

예제 #10

q22 = [1.2, 1.4, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]  # second goal
r(q22)
#q22 = [-4.4, -1.6, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # ultimate goal!

q1 = cl.robot.projectOnObstacle(q11, 0.02)
q2 = cl.robot.projectOnObstacle(q22, 0.02)

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

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

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

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

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

qwp1 = [
    6.588526674447825, -1.8345785084341553, 0.36443200826644895,
    0.1654550896480227, 0.07312786003135581, 0.25630565685312806,
    -0.9495179512057083, 5.547415126403182e-16, 0.0, 1.0, -1.5231360759319517
]

qwp2 = [
    6.376949224610323, -1.8304419228017523, 0.36443200826644895,
    -0.10812814084986927, 0.8453266741499781, -0.1992256556515323,
    0.4837770739267257, 0.0, -1.486905914403694e-16, 1.0, -2.0916445213810797