r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","envir3d_window_mesh","envir3d_window_mesh")
addLight (r, [-3,3,4,1,0,0,0], "li"); addLight (r, [3,-3,4,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)
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 ()
r.loadObstacleModel ("animals_description","cave","cave")
#addLight (r, [-0.3, 3.8, 0,1,0,0,0], "li"); addLight (r, [-0.18, -3, 0.1,1,0,0,0], "li1"); addLight (r, [-0.3, 4, 0,1,0,0,0], "li3")
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,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()
coneNamePrefix="cony2"; coneName = coneNamePrefix; height = 0.18
r.client.gui.addCone (coneName,height*0.5,height,[0,0.5,0.5,0.2]) # grey
r.client.gui.addToGroup (coneName, r.sceneName)
r.client.gui.applyConfiguration (coneName, qRobot[0:7])
r.client.gui.refresh ()
#r.client.gui.removeFromGroup (coneName, r.sceneName)
# cone orientation seems not to be good....
r.client.gui.applyConfiguration (coneName, [6, -1.6, 0.5, 0, 1, 0, 0])
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)
plotPath (cl, 0, r, "pathy", 0.1)
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 = qa[::]
plotStraightLine ([q [index],q [index+1],q [index+2]], q, r, "normale2")
plotConeWaypoints (cl, 0, r, 0.5, 0.4, "wpcones")
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)
## 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")
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)
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")
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
cl.obstacle.getObstaclePosition('decor_base')
robot.getJointOuterObjects('base_joint_xyz')
robot.isConfigValid(q1)
robot.distancesToCollision()
r( ps.configAtDistance(0,5) )
ps.optimizePath (0)
ps.clearRoadmap ()
ps.resetGoalConfigs ()
from numpy import *
argmin(robot.distancesToCollision()[0])
r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder")
addLight (r, [-5,0,2,1,0,0,0], "li"); addLight (r, [2,-2,5,1,0,0,0], "li1")
#addLight (r, [-4.4,-2,6.5,1,0,0,0], "li2")
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, 'frame_group', [0,0,0], 0.6)
# First parabolas: vmax = 7m/s, mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(7)
plotCone (q1, cl, r, "cone_11", "friction_cone2"); plotCone (q2, cl, r, "cone_21", "friction_cone2")
ps.clearRoadmap();
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, "cone_wp_group", "friction_cone2")
plotSpheresWaypoints (cl, pahtId, r, "sphere_wp_group", [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 ()
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, "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)))
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
# Load box obstacle in HPP for collision avoidance
#cl.obstacle.loadObstacleModel('puzzle_description','decor_very_easy','')
r( ps.configAtParam(0,2) )
ps.pathLength(0)
ps.getWaypoints (0)
cl.problem.generateValidConfig(2)
plotPath (cl, 0, r, "pathy", 0.1) # time-step should depend on sub-path length ?
plotFrame (r, "framy", [0,0,1], 0.5)
plotThetaPlane (qt1, qt2, r, "ThetaPlane2")
q = q1[::]
plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale")
plotCone (q1, cl, r, 0.5, 0.2, "cone1")
r.startCapture ("capture","png")
pp(1)
r.stopCapture ()
#ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4
## DEBUG commands
cl.obstacle.getObstaclePosition('decor_base')
robot.getJointOuterObjects('base_joint_xyz')
samples = plotSampleSubPath (cl, r, 1, 20, "curvy2", [0,0.85,0.25,1])
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_ant_in_cave1.rdm')
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(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, 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))
