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