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")
plotConeWaypoints (cl, pathId, r, "cone_wp_group", "friction_cone_WP")
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) 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")
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, "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)
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 ?
plotFrame (r, "framy", [0,0,0], 0.5)
plotThetaPlane (q1, q2, r, "ThetaPlane")
## DEBUG commands
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)
plotThetaPlane (q1, q2, r, "ThetaPlane2")
plotCone (q1, cl, r, 0.5, 0.4, "c1")
plotCone (q2, cl, r, 0.5, 0.4, "c2")
index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize ()
q = qa[::]
q = [-4.77862,-1.56995,2.87339,-0.416537,-0.469186,-0.619709,0.471511,-0.197677,-0.0998335,0.97517,0.619095]
qprojCorba=[-4.778619492059025, -1.5699444231861588, 2.873387956706481, 0.9470998051218645, 0.017748399125124163, -0.10999926666084152, 0.3009769340010935, -0.19767685053184691, -0.0998334947491579, 0.9751703113251448, 0.619095]
plotStraightLine ([q [index],q [index+1],q [index+2]], q, r, "normale2")
plotConeWaypoints (cl, 0, r, 0.5, 0.4, "wpcones")
wp = cl.problem.getWaypoints (0)
for i in np.arange(0, len(wp), 1):
plotCone (wp[i], cl, r, 0.5, 0.4, "wpcones"+str(i))
# Plot a cone and rotate it!
qCone = cl.robot.setOrientation (q2) # wp[15]
coneName = "cone5"
r.loadObstacleModel ("animals_description","friction_cone2",coneName)
r.client.gui.applyConfiguration (coneName, qCone[0:7])
r.client.gui.refresh ()
#r.client.gui.removeFromGroup (coneName, r.sceneName)
## 3D Plot tools ##
q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0];
r(q0)
plotFrame (r, "frame", [0,0,0], 0.5)
plotCone (q, cl, r, "yep", "friction_cone2")
plotConeWaypoints (cl, 0, r, "wp", "friction_cone2")
plotThetaPlane (q1, q2, r, "ThetaPlane")
r.client.gui.removeFromGroup ("ThetaPlane", r.sceneName)
r.client.gui.removeFromGroup ("ThetaPlanebis", r.sceneName)
#plotCone (q1, cl, r, 0.5, 0.4, "c1"); plotCone (q2, cl, r, 0.5, 0.4, "c2")
index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize ()
q = q2[::]
plotStraightLine ([q [index],q [index+1],q [index+2]], q, r, "normale2")
## Plot all cone waypoints:
#plotConeWaypoints (cl, 0, r, 0.5, 0.4, "wpcones")
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')
# DRAFT return contact position:
qConeContact = contactPosition (q22, cl, r)
plotCone (qConeContact, cl, r, "verif", "friction_cone2")
q0 = [0, 0, 0, 0, 0, 0, 1, 0, 0, 1, Pi]
r(ps.configAtParam(0,0.001))
ps.pathLength(0)
wp = ps.getWaypoints (0)
cl.problem.generateValidConfig(2)
# 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]
coneName = sphereNamePrefix+str(i)
plotCone (qrand, cl, r, coneNameSufffix, "friction_cone2")
r(ps.configAtParam(0,0.001))
ps.pathLength(0)
wp = ps.getWaypoints (0)
r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF")
## 3D Plot tools ##
q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0];
r(q0)
plotFrame (r, "frame", [0,0,4], 0.5)
plotThetaPlane (q1, q2, r, "ThetaPlane")
plotCone (q2, cl, r, "yep", "friction_cone2")
plotConeWaypoints (cl, 0, r, "wp", "friction_cone")
plotConeWaypoints (cl, 1, r, "wp2", "friction_cone")
index = cl.robot.getConfigSize () - cl.robot.getExtraConfigSize ()
q = q2[::]
plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale")
# --------------------------------------------------------------------#
## Video capture ##
import time
pp.dt = 0.01; pp.speed=0.5
r(q1)
r.startCapture ("capture","png")
r(q1); time.sleep(0.2); r(q1)
pp(0)
#q11 = [2, 1.1, 5.5, 0, 0, 0, 1, -math.sqrt(1-0.8), 0, -math.sqrt(0.8), 0]; q22 = [1.8, 0.8, 3.1, 0, 0, 0, 1, 0, 0, 1, 0] # theta != Pi/2
#q11 = [2, 1.1, 5.5, 0, 0, 0, 1, 0, -math.sqrt(1-0.8), -math.sqrt(0.8), 0]; q22 = [2, 0.9, 3.1, 0, 0, 0, 1, 0, 0, 1, 0] # theta = -Pi/2
#q11 = [2, 1.1, 5.5, 0, 0, 0, 1, 0, math.sqrt(1-0.8), -math.sqrt(0.8), 0]; q22 = [2, 1.3, 3.1, 0, 0, 0, 1, 0, 0, 1, 0] # theta = Pi/2 solved with |W|
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)
q0 = [0, 0, 0, 0, 0, 0, 1, 0, 0, 1, Pi]
r(ps.configAtParam(0,0.001))
ps.pathLength(0)
wp = ps.getWaypoints (0)
cl.problem.generateValidConfig(2)
r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF")
# 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]
coneName = sphereNamePrefix+str(i)
plotCone (qrand, cl, r, coneNameSufffix, "friction_cone2")
r.client.gui.refresh ()
## 3D Plot tools ##
q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0];
r(q0)
plotFrame (r, "frame", [0,0,0], 0.5)
plotCone (q, cl, r, "yep", "friction_cone2")
plotConeWaypoints (cl, 0, r, "wp", "friction_cone2")
plotThetaPlane (q1, q2, r, "ThetaPlane")
r.client.gui.removeFromGroup ("ThetaPlane", r.sceneName)
