import numpy as np robot = Robot ('sphere') 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] #cl.obstacle.loadObstacleModel('animals_description','envir3d_window_mesh','') 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, [-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)
#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 r = Viewer (ps) pp = PathPlayer (robot.client, r) 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") # 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] # start #q11 = [-3.5, 1.7, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # bottom of column r(q11) #q22 = [2.6, -1.4, 0.35, 0, 0, 0, 1, 0, 0, 1, 0] # first goal #q22 = [0.7, 1.55, 0.4, 0, 0, 0, 1, 0, 0, 1, 0] # second goal #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)
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) 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
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) 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
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 r(q22) q22 = [0.27, -5.75, 5.90, 1, 0, 0, 0, 0, 0, 1, 0] # cave top end r(q22) selectConFigurationShooter("ContactConfigurationShooter") selectConFigurationShooter("ConfigurationProjectionShooter") q1 = cl.robot.projectOnObstacle (q11, 0.002) robot.isConfigValid(q1)
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) 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 # First parabola(s): vmax = 8m/s, mu = 1.2 plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") solveTime = ps.solve ()