#/usr/bin/env python # RUNS version! launch command: "python -i environment3d_runs.py" # Script which goes with animals_description package, runs version of Benchmark3. # The script launches a point-robot and the cave environment. # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). # Do not forget to launch it in Release mode ! from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 1.6]) # low end #robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 7]) # top end ps = ProblemSolver (robot) cl = robot.client cl.obstacle.loadObstacleModel('animals_description','cave','') # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-0.18, 3.5, -0.11, 1, 0, 0, 0, 0, 0, 1, 0]; q22 = [0.4, -5.2, -0.7, 1, 0, 0, 0, 0, 0, 1, 0] # cave low end #q22 = [0.27, -5.75, 5.90, 1, 0, 0, 0, 0, 0, 1, 0] # cave top end q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001) ps.setInitialConfig (q1); ps.addGoalConfig (q2) vmax = 8; mu = 1.2 #vmax = 6.5; mu = 0.5 #cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)
#/usr/bin/env python # Benchmark3. Script which goes with animals_description package. # The script launches a point-robot and the cave environment. # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot('robot') robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 7]) # top end ps = ProblemSolver(robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-0.18, 3.5, -0.11, 1, 0, 0, 0, 0, 0, 1, 0] q22 = [0.27, -5.75, 5.90, 1, 0, 0, 0, 0, 0, 1, 0] # cave top end from hpp.gepetto import Viewer, PathPlayer r = Viewer(ps) pp = PathPlayer(robot.client, r) r.loadObstacleModel("animals_description", "cave", "cave") #addLight (r, [0.2,-13.5,-2.3,1,0,0,0], "li"); addLight (r, [0,-5,-2.5,1,0,0,0], "li1") r(q11)
#/usr/bin/env python # Script which goes with animals_description package. # Easy way to test parabola-planning algo (no internal DoF) on SO3 joint. from hpp.corbaserver.sphere import Robot #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseConfig, parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 8]) # ultimate goal! #robot.setJointBounds('base_joint_xyz', [1.6, 6.8, -2.2, 1.5, 0, 3]) # first goal #robot.setJointBounds('base_joint_xyz', [-0.3, 6.8, -2.2, 2.4, 0, 3]) # second goal #robot.setJointBounds('base_joint_xyz', [-2.6, 6.8, -2.2, 2.4, 0, 3]) # third goal #robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.8, 3.2, 0, 3]) # start to bottom #robot.setJointBounds('base_joint_xyz', [-6, -2.2, -2.4, 3, 0, 8]) # bottom to ultimate #robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 6]) # bottom to middle column #robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 3]) # bottom to bottom 1 #robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 3]) # first to bottom ps = ProblemSolver (robot) cl = robot.client #cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d') from hpp.gepetto import Viewer, PathPlayer
#/usr/bin/env python # Benchmark3. Script which goes with animals_description package. # The script launches a point-robot and the cave environment. # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 7]) # top end ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-0.18, 3.5, -0.11, 1, 0, 0, 0, 0, 0, 1, 0]; q22 = [0.27, -5.75, 5.90, 1, 0, 0, 0, 0, 0, 1, 0] # cave top end from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","cave","cave") #addLight (r, [0.2,-13.5,-2.3,1,0,0,0], "li"); addLight (r, [0,-5,-2.5,1,0,0,0], "li1") r(q11)
#/usr/bin/env python # Benchmark1. Script which goes with animals_description package. # The script launches a point-robot and the environment containing a simple plane. # It defines init and final configs, and solve them for three couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-3.9, 3.9, -3.9, 3.9, 2, 9]) ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] #q1 = [-1.5, -1.5, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]; q2 = [2.6, 3.7, 3.41, 0, 0, 0, 1, 0, 0, 1, 0] q11 = [-1.8, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [1.8, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # theta ~= 0 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) q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
# Script which goes with animals_description package. # Easy way to test parabola-planning algo (no internal DoF) on SO3 joint. from hpp.corbaserver.sphere import Robot #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot #from hpp.corbaserver.monopod_mesh import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') #robot.setJointBounds('base_joint_xyz', [-3.9, 3.9, -3.9, 3.9, 1, 12]) # plane robot.setJointBounds('base_joint_xyz', [-6, 6, -6, 6, 2, 9]) # environment_3d ps = ProblemSolver (robot) cl = robot.client #cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d') # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] #q1 = [-1.5, -1.5, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]; q2 = [2.6, 3.7, 3.41, 0, 0, 0, 1, 0, 0, 1, 0] #q11 = [2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta = Pi #q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta ~= 0 #q11 = [-2.5, 3, 4, 0, 0, 0, 1,-0.1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, -0.1, 0, 0, 1, 0] # plane with theta ~= 0 MONOPOD #q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 9, 0, 0, 0, 1, 0, 0, 1, 0] # multiple-planes (3 planes) q11 = [-5, 3.1, 4.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [5.2, -5.2, 4, 0, 0, 0, 1, 0, 0, 1, 0] # environment_3d #r(q22)
#/usr/bin/env python # Benchmark3. Script which goes with animals_description package. # The script launches a point-robot and the cave_mono_short environment. # It defines init and final configs, and solve them for 2 couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -3, 4.2, -2.5, 4]) ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-0.3, 3.65, -0.25, 1, 0, 0, 0, 0, 0, 1, 0]; q22 = [-0.18, -2.2, -0.4, 1, 0, 0, 0, 0, 0, 1, 0] #cl.obstacle.loadObstacleModel('animals_description','cave','') from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) 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)
#/usr/bin/env python # RUNS version! launch command: "python -i environment3d_runs.py" # Script which goes with animals_description package, runs version of Benchmark2. # The script launches a point-robot and the environment containing an environment 3d (with windows ?). # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). # Do not forget to launch it in Release mode ! from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-6, 6, -6, 6, 2, 9]) ps = ProblemSolver (robot) cl = robot.client cl.obstacle.loadObstacleModel('animals_description','environment_3d','') #cl.obstacle.loadObstacleModel('animals_description','environment_3d_with_window','') # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-5, 3.1, 4.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [5.2, -5.2, 4, 0, 0, 0, 1, 0, 0, 1, 0] q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001) ps.setInitialConfig (q1); ps.addGoalConfig (q2) vmax = 8; mu = 1.2 #vmax = 6.5; mu = 0.5 #cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax) toSeconds = np.array ([60*60,60,1,1e-3]) offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
#/usr/bin/env python # Benchmark3. Script which goes with animals_description package. # The script launches a point-robot and the cave_mono_short environment. # It defines init and final configs, and solve them for 2 couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints, plotConesRoadmap import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -3, 4.2, -2.5, 4]) ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-0.3, 3.65, -0.25, 1, 0, 0, 0, 0, 0, 1, 0]; q22 = [-0.18, -2.2, -0.4, 1, 0, 0, 0, 0, 0, 1, 0] #cl.obstacle.loadObstacleModel('animals_description','cave','') from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps); gui = r.client.gui pp = PathPlayer (robot.client, r) 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)
#/usr/bin/env python # Script which goes with animals_description or room_description package. # Easy way to test planning algo (no internal DoF) on SO3 joint. #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import numpy as np from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles robot = Robot ('robot') #robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 1.6]) # low end robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 7]) # top end ps = ProblemSolver (robot) cl = robot.client #cl.obstacle.loadObstacleModel('animals_description','cave','cave') from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","cave","cave") addLight (r, [0.2,-13.5,-2.3,1,0,0,0], "li"); addLight (r, [0,-5,-2.5,1,0,0,0], "li1") # Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta] q11 = [-0.18, 3.5, -0.11, 1, 0, 0, 0, 0, 0, 1, 0] # cave entry r(q11) #q22 = [0.4, -5.2, -0.7, 1, 0, 0, 0, 0, 0, 1, 0] # cave low end
#/usr/bin/env python # RUNS version! launch command: "python -i scene_jump_harder_runs.py" # Script which goes with animals_description package, runs version of Benchmark4. # The script launches a point-robot and the scene_jump environment. # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). # Do not forget to launch it in Release mode ! from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import math import numpy as np robot = Robot('robot') robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 8]) # ultimate goal! ps = ProblemSolver(robot) cl = robot.client cl.obstacle.loadObstacleModel('animals_description', 'scene_jump_harder', '') # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [6.2, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0] q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # start to ultimate goal! q1 = cl.robot.projectOnObstacle(q11, 0.001) q2 = cl.robot.projectOnObstacle(q22, 0.001) ps.setInitialConfig(q1) ps.addGoalConfig(q2) #vmax = 7.0; mu = 1.2 # results1.txt #vmax = 7.0; mu = 0.5 # results2.txt
#/usr/bin/env python # Benchmark2. Script which goes with animals_description package. # The script launches a point-robot and the environment containing an environment 3d (with windows ?). # It defines init and final configs, and solve them for 2 couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-6, 6, -6, 6, 2, 9]) ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-5, 3.1, 4.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [5.2, -5.2, 4, 0, 0, 0, 1, 0, 0, 1, 0] from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) #r.loadObstacleModel ("animals_description","environment_3d","environment_3d") r.loadObstacleModel ("animals_description","environment_3d_with_window","environment_3d_with_window") addLight (r, [-5,5,7,1,0,0,0], "li"); addLight (r, [5,-5,7,1,0,0,0], "li1") r(q11)
#/usr/bin/env python # Script which goes with animals_description or room_description package. # Easy way to test planning algo (no internal DoF) on SO3 joint. #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import numpy as np from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles robot = Robot ('robot') #robot.setJointBounds('base_joint_xyz', [-6, 6, -14, 20, -8, 10]) # cave robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -14, -1, -2.5, 1.6]) # cave middle #robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -14, 4.5, -2.5, 1.6]) # cave end ps = ProblemSolver (robot) cl = robot.client from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","cave","cave") #cl.obstacle.loadObstacleModel('animals_description','cave','cave') # Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta] q11 = [0, -13, -2, 1, 0, 0, 0, 0, 0, 1, 0] # cave entry q22 = [-0.4, -3, -0.7, 1, 0, 0, 0, 0, 0, 1, 0] # cave middle #q22 = [-0.18, 3.5, -0.11, 1, 0, 0, 0, 0, 0, 1, 0] # cave end r(q22)
#/usr/bin/env python # Script which goes with animals_description package. # Easy way to test parabola-planning algo (no internal DoF) on SO3 joint. from hpp.corbaserver.sphere import Robot #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotVerticalCone, plotCone, plotPath, plotVerticalConeWaypoints, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot('robot') #robot.setJointBounds('base_joint_xyz', [-6, 7, -4, 4, 0, 8]) # ultimate goal! #robot.setJointBounds('base_joint_xyz', [1.2, 7, -3, -0.5, 0, 2.2]) # first goal robot.setJointBounds('base_joint_xyz', [0, 7, -3, 2.4, 0, 2.2]) # second goal ps = ProblemSolver(robot) cl = robot.client #cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d') from hpp.gepetto import Viewer, PathPlayer r = Viewer(ps) pp = PathPlayer(robot.client, r) r.loadObstacleModel("animals_description", "scene_jump", "scene_jump") # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [6, -1.6, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]
#/usr/bin/env python # Script which goes with animals_description package. # Easy way to test parabola-planning algo (no internal DoF) on SO3 joint. from hpp.corbaserver.sphere import Robot #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath from parseLog import parseConfig, parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') #robot.setJointBounds('base_joint_xyz', [-6, 6.7, -2.5, 3.2, 0, 8]) # ultimate goal! #robot.setJointBounds('base_joint_xyz', [1.6, 6.7, -2.2, 1.5, 0, 3]) # first goal #robot.setJointBounds('base_joint_xyz', [-0.3, 6.7, -2.2, 2.4, 0, 3]) # second goal #robot.setJointBounds('base_joint_xyz', [-2.6, 6.7, -2.2, 2.4, 0, 3]) # third goal #robot.setJointBounds('base_joint_xyz', [-6, 6.9, -2.8, 3.2, 0, 3]) # start to bottom robot.setJointBounds('base_joint_xyz', [-6, -2.2, -2.4, 3, 0, 8]) # bottom to ultimate #robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 6]) # bottom to middle column #robot.setJointBounds('base_joint_xyz', [-5, -2.2, -0.1, 2.8, 0, 3]) # bottom to bottom 1 #robot.setJointBounds('base_joint_xyz', [-6, 6.7, -2.5, 3.2, 0, 3]) # first to bottom ps = ProblemSolver (robot) cl = robot.client #cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d') from hpp.gepetto import Viewer, PathPlayer
#/usr/bin/env python # Benchmark1. Script which goes with animals_description package. # The script launches a point-robot and the environment containing a simple plane. # It defines init and final configs, and solve them for three couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot('robot') robot.setJointBounds('base_joint_xyz', [-3.9, 3.9, -3.9, 3.9, 2, 9]) ps = ProblemSolver(robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] #q1 = [-1.5, -1.5, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]; q2 = [2.6, 3.7, 3.41, 0, 0, 0, 1, 0, 0, 1, 0] q11 = [-1.8, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0] q22 = [1.8, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # theta ~= 0 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")
#/usr/bin/env python # Script which goes with animals_description package. # Easy way to test parabola-planning algo (no internal DoF) on SO3 joint. from hpp.corbaserver.sphere import Robot #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotVerticalCone, plotCone, plotPath, plotVerticalConeWaypoints, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') #robot.setJointBounds('base_joint_xyz', [-6, 7, -4, 4, 0, 8]) # ultimate goal! #robot.setJointBounds('base_joint_xyz', [1.2, 7, -3, -0.5, 0, 2.2]) # first goal robot.setJointBounds('base_joint_xyz', [0, 7, -3, 2.4, 0, 2.2]) # second goal ps = ProblemSolver (robot) cl = robot.client #cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d') from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","scene_jump","scene_jump") # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [6, -1.6, 0.5, 0, 0, 0, 1, 0, 0, 1, 0] #q22 = [2.2, -2.7, 0.8, 0, 0, 0, 1, 0, 0, 1, 0] # first goal
# Script which goes with animals_description package. # Easy way to test parabola-planning algo (no internal DoF) on SO3 joint. from hpp.corbaserver.sphere import Robot #from hpp.corbaserver.ant import Robot #from hpp.corbaserver.ant_sphere import Robot #from hpp.corbaserver.monopod_mesh import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-5, 5, -5, 5, -1, 7]) ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-3.8, 2.4, 1.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [3.5, -3.3, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # environment_3d mesh r(q22) from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","envir3d_window_mesh","envir3d_window_mesh") #addLight (r, [-5,5,7,1,0,0,0], "li"); addLight (r, [5,-5,7,1,0,0,0], "li1") r(q11)
#/usr/bin/env python # Benchmark2mesh. Script which goes with animals_description package. # The script launches a point-robot and the environment containing an environment 3d mesh (with windows). # It defines init and final configs, and solve them for 2 couples of mu / vmax (parabola constraints). #blender/urdf_to_blender.py -p /local/mcampana/devel/hpp/videos/ -i robot.urdf -o robot_blend.py from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints, plotConesRoadmap, plotEdgesRoadmap import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-5, 5, -5, 5, -1, 7]) ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-3.8, 2.4, 1.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [3.5, -3.3, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps); gui = r.client.gui 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)
#/usr/bin/env python # RUNS version! launch command: "python -i environment3d_runs.py" # Script which goes with animals_description package, runs version of Benchmark2. # The script launches a point-robot and the environment containing an environment 3d (with windows ?). # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). # Do not forget to launch it in Release mode ! from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import math import numpy as np robot = Robot('robot') robot.setJointBounds('base_joint_xyz', [-6, 6, -6, 6, 2, 9]) ps = ProblemSolver(robot) cl = robot.client cl.obstacle.loadObstacleModel('animals_description', 'environment_3d', '') #cl.obstacle.loadObstacleModel('animals_description','environment_3d_with_window','') # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [-5, 3.1, 4.2, 0, 0, 0, 1, 0, 0, 1, 0] q22 = [5.2, -5.2, 4, 0, 0, 0, 1, 0, 0, 1, 0] q1 = cl.robot.projectOnObstacle(q11, 0.001) q2 = cl.robot.projectOnObstacle(q22, 0.001) ps.setInitialConfig(q1) ps.addGoalConfig(q2) vmax = 8 mu = 1.2 #vmax = 6.5; mu = 0.5
#/usr/bin/env python # Benchmark4. Script which goes with animals_description package. # The script launches a point-robot and the scene_jump_harder (mesh version 10) environment. # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). from hpp.corbaserver.sphere import Robot #from hpp.corbaserver.sphere_mesh import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints, plotConesRoadmap import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 8]) ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [6.2, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps); gui = r.client.gui pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder") q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001) robot.isConfigValid(q1); robot.isConfigValid(q2) r(q1)
#/usr/bin/env python # RUNS version! launch command: "python -i scene_jump_harder_runs.py" # Script which goes with animals_description package, runs version of Benchmark4. # The script launches a point-robot and the scene_jump environment. # It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints). # Do not forget to launch it in Release mode ! from hpp.corbaserver.sphere import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import math import numpy as np robot = Robot ('robot') robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 8]) # ultimate goal! ps = ProblemSolver (robot) cl = robot.client cl.obstacle.loadObstacleModel('animals_description','scene_jump_harder','') # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [6.2, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] # start to ultimate goal! q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001) ps.setInitialConfig (q1); ps.addGoalConfig (q2) #vmax = 7.0; mu = 1.2 # results1.txt #vmax = 7.0; mu = 0.5 # results2.txt #vmax = 6.5; mu = 1.2 # results3.txt vmax = 6.5; mu = 0.5 cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)