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