# --------------------------------------------------------------------# ## Export to Blender ## r.client.gui.writeNodeFile(0, 'scene.osg') # osgconvd -O NoExtras scene.osg scene.dae from hpp.gepetto.blender.exportmotion import exportStates, exportPath exportPath(r, cl.robot, cl.problem, 0, 1, 'path2.txt') exportStates(r, cl.robot, q11, 'configs.txt') from gepettoimport import loadmotion loadmotion('/local/mcampana/devel/hpp/videos/path2.txt') # and rename first node manually ... # --------------------------------------------------------------------# ## DEBUG commands cl.obstacle.getObstaclePosition('decor_base') robot.isConfigValid(q1) robot.setCurrentConfig(q1) res=robot.distancesToCollision() r( ps.configAtParam(0,5) ) ps.optimizePath (0) ps.clearRoadmap () ps.resetGoalConfigs () from numpy import * argmin(robot.distancesToCollision()[0]) robot.getJointNames () robot.getConfigSize () r.client.gui.getNodeList() ['0_scene_hpp_', 'inclined_plane_3d', 'inclined_plane_3d/l_one', 'inclined_plane_3d/l_one_0', 'inclined_plane_3d/obstacle_base', 'inclined_plane_3d/obstacle_base_0', 'robot', 'robot/base_link', 'robot/base_link_0']
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) 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])
# --------------------------------------------------------------------# ## Export to Blender ## r.client.gui.writeNodeFile(0, 'scene.osg') # osgconvd -O NoExtras scene.osg scene.dae from hpp.gepetto.blender.exportmotion import exportStates, exportPath exportPath(r, cl.robot, cl.problem, 0, 1, 'path2.txt') exportStates(r, cl.robot, q11, 'configs.txt') from gepettoimport import loadmotion loadmotion('/local/mcampana/devel/hpp/videos/path2.txt') # and rename first node manually ... # --------------------------------------------------------------------# ## DEBUG commands cl.obstacle.getObstaclePosition('decor_base') robot.isConfigValid(q1) robot.setCurrentConfig(q1) res=robot.distancesToCollision() r( ps.configAtParam(0,5) ) ps.optimizePath (0) ps.clearRoadmap () ps.resetGoalConfigs () from numpy import * argmin(robot.distancesToCollision()[0]) robot.getJointNames () robot.getConfigSize () r.client.gui.getNodeList() # get all theta: for i in range(0,len(wp)):