Python gepetto.Viewer类代码示例

rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])

#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver

ps = ProblemSolver( rbprmBuilder )

r = Viewer (ps)


q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [5, 0, 0.63]; r (q_goal)

ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
r.loadObstacleModel (packageName, "groundcrouch", "planning")
#~ ps.solve ()

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

# Easy way to test planning algo (no internal DoF) on SO3 joint.
# b hpp::core::pathOptimization::GradientBased::initialize (const PathVectorPtr_t& path)


from hpp.corbaserver.puzzle import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
import math

robot = Robot ('puzzle')
ps = ProblemSolver (robot)
cl = robot.client

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")

robot.setJointBounds('base_joint_xyz', [-2.2, +2.2, -0.2, 2.2, -0.5, 0.5])
Q = []
Q.append ( [0, 0, 0, 1.0, 0.0, 0.0, 0.0])
"""
Q.append ( [0.0, 0.0, 0.0, 0.8573289792052967, 0.5110043077178016, 0.0474382998474562, 0.04014009987092448])
Q.append ( [0.0, 0.0, 0.0, 0.47002595717039686, 0.8761976030104256, 0.08134045836690892, 0.06882654169507678])
Q.append ( [0.0, 0.0, 0.0, -0.05139523108351973, 0.9913748854243123, 0.09203276443212992, 0.07787387759641762]) # !
Q.append ( [0.0, 0.0, 0.0, -0.5581511991721069, 0.8236712340507641, 0.07646425360117025, 0.06470052227791329])
Q.append ( [0.0, 0.0, 0.0, -0.9056431645733478, 0.420939551154707, 0.03907727653904272, 0.033065387840728454]) # ! one turn
Q.append ( [0.0, 0.0, 0.0, -0.7409238777634497, -0.6666775704281703, 0.06188998802924064, 0.05236845140935746])
Q.append ( [0.0, 0.0, 0.0, 0.2445263320841038, -0.9625514882482619, 0.08935698863687985, 0.07560975961582142])
Q.append ( [0.0, 0.0, 0.0, 0.708781393086984, -0.7002692759274627, 0.0650084224020931, 0.05500712664792493])

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)

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#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 ...

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

from hpp.corbaserver.puzzle import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver

robot = Robot ('puzzle') # object5
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1., 1.])
#robot.setJointBounds('base_joint_xyz', [-0.6, 0.6, -0.6, 0.6, -0.3, 1.0])
ps = ProblemSolver (robot)
cl = robot.client

q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] # simpler
#q1 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0] # simpler2

from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
r.loadObstacleModel ("puzzle_description","decor_easy","decor_easy")
r(q2) # r(q1)

#q1bis = q2; q2bis = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#ps.resetGoalConfigs (); ps.setInitialConfig (q1bis); ps.addGoalConfig (q2bis); ps.solve ()
# ps.resetGoalConfigs (); ps.setInitialConfig (q1); ps.addGoalConfig (q2bis); ps.solve ()
#i = ps.numberPaths()-1

ps.setInitialConfig (q1); ps.addGoalConfig (q2)

ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/puzzle_veryEasy_PRM.rdm')

#/usr/bin/env python

import time
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.pr2_with_set import Robot
robot = Robot ('pr2_with_set') # will containt the set fixed on the left hand... (moving with l_wrist_flex_joint)
robot.setJointBounds ("base_joint_xy", [-4, -0.5, -7, -1.5])

from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
Viewer.withFloor = True
r = Viewer (ps)
r.loadObstacleModel ("iai_maps","kitchen_area","kitchen_area") # visual kitchen
r.loadObstacleModel ("iai_maps","floor","floor")

q1 = robot.getCurrentConfig ()
q1 [0:2] = [-3.4, -6]
q1 [robot.rankInConfiguration ['torso_lift_joint']] = 0.13
q1 [robot.rankInConfiguration ['l_shoulder_pan_joint']] = 0.5 # ecarte les bras
q1 [robot.rankInConfiguration ['r_shoulder_pan_joint']] = -0.5
q1 [robot.rankInConfiguration ['l_upper_arm_roll_joint']] = 1.57 # tourne bras sur lui-mm
q1 [robot.rankInConfiguration ['r_upper_arm_roll_joint']] = -1.57
q1 [robot.rankInConfiguration ['l_elbow_flex_joint']] = -0.49 # plier coude
q1 [robot.rankInConfiguration ['r_elbow_flex_joint']] = -0.50
q1 [robot.rankInConfiguration ['l_wrist_flex_joint']] = -0.2 # plie poignet
q1 [robot.rankInConfiguration ['r_wrist_flex_joint']] = -0.2
q1 [robot.rankInConfiguration ['l_gripper_r_finger_joint']] = 0.09 # ouvrir pince
q1 [robot.rankInConfiguration ['l_gripper_l_finger_joint']] = 0.12
q1 [robot.rankInConfiguration ['r_gripper_r_finger_joint']] = 0.13
q1 [robot.rankInConfiguration ['r_gripper_l_finger_joint']] = 0.095
r(q1)

from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer
from hpp.gepetto import PathPlayer

# define colors for the roadmap
white=[1.0,1.0,1.0,1.0]
green=[0.23,0.75,0.2,0.5]
brown=[0.85,0.75,0.15,0.5]
blue = [0.0, 0.0, 0.8, 1.0]
grey = [0.7,0.7,0.7,1.0]
red = [0.8,0.0,0.0,1.0]

robot = Robot ('pr2')
robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3])
ps = ProblemSolver (robot)
v = Viewer (ps)

q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
v (q_init)

q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5

from hpp.corbaserver.pr2 import Robot

robot = Robot("pr2")
robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3])

from hpp.corbaserver import ProblemSolver

ps = ProblemSolver(robot)

from hpp.gepetto import Viewer

r = Viewer(ps)

q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration["torso_lift_joint"]
q_init[rank] = 0.2
r(q_init)

q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration["l_shoulder_lift_joint"]
q_goal[rank] = 0.5
rank = robot.rankInConfiguration["l_elbow_flex_joint"]
q_goal[rank] = -0.5
rank = robot.rankInConfiguration["r_shoulder_lift_joint"]
q_goal[rank] = 0.5
rank = robot.rankInConfiguration["r_elbow_flex_joint"]
q_goal[rank] = -0.5
r(q_goal)

rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2, -1.5, 1, 0.5, 0.9])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom','hrp2_larm_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.boundSO3([-1.5,1.5,0,3,-0.0,0.0])

#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver

ps = ProblemSolver( rbprmBuilder )

r = Viewer (ps)

q0 = rbprmBuilder.getCurrentConfig ();
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = q_init [::]


q_init = rbprmBuilder.getCurrentConfig ();
q_init[0:3] = [0.15, -0.45, 0.8];  r(q_init)
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal)
q_goal[0:3] = [0.2,-1.1,0.58]; r(q_goal)
 
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

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)

q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
r(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])

lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "EFORT", 0.05)

scale = sys.argv[len(sys.argv)-2]
scene = sys.argv[len(sys.argv)-1]
#~ configFile = sys.argv[len(sys.argv)-1]

import pickle
sFile = "false_negative_configs_"+scene+'_'+scale+'.pkl'
pkl_file = open(sFile, 'rb')
falseNeg = pickle.load(pkl_file)
pkl_file.close()

ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', scene, "planning")

q_init =  [
        0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,                         	 # Free flyer 0-6
        0.0, 0.0, 0.0, 0.0,                                                  # CHEST HEAD 7-10
        0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM       11-17
        0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM       18-24
        0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,               # LLEG       25-30
        0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,               # RLEG       31-36
        ]; r (q_init)

fullBody.setCurrentConfig (q_init)

confsize = len(falseNeg[0])

# Script to solve ompl problem in hpp:
# name = Abstract
# robot = Absrtact_robot
# world = Abstract_env
from math import pi
from hpp.corbaserver.hpp_ompl_benchmark.Abstract import Robot
robot = Robot('ompl')
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer

v = Viewer (ps)
v.loadObstacleModel("hpp-ompl-benchmark", "Abstract_env", "env")



q_init = [2.1, -5.81216, -1, 1 , 0, 0, 0]
q_goal = [-3, -4.3, 1, 0.7071, 0.7071, 0, 0]
v(q_init)

robot.setJointBounds ('base_joint_xyz', [-5.5, 6.5, -12, 0, -5, 6])
# robot.setJointBounds ('base_joint_SO3', [-pi, pi, -pi, pi, -pi, pi, -pi, pi])
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

from hpp.corbaserver import Benchmark
benchmark = Benchmark (robot.client, robot, ps)
ps.client.problem.setRandomSeed(1927402002)
benchmark.seedRange = range (50)
benchmark.iterPerCase = 1
results = benchmark.do()

#------------------------------------------------------------------
# name = pipedream
# robot = ring
# world = pipedream_env
from math import pi
from hpp.corbaserver.hpp_ompl_benchmark.pipedream import Robot
robot = Robot('ompl')
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer

v = Viewer (ps)
v.loadObstacleModel("hpp-ompl-benchmark", "pipedream_env", "env")

q_init = [2.02,-0.0912,0.786,0.707,0.707,0,0]
q_goal = [1.587,-0.345,0.634,1,0,0,0]

robot.setJointBounds ('base_joint_xyz', [1.08, 2.43, -0.824, 0.28, 0.3, 1.7])
# robot.setJointBounds ('base_joint_SO3', [-pi, pi, -pi, pi, -pi, pi, -pi, pi])

ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectPathValidation("Progressive",0.03)
ps.client.problem.clearPathOptimizers()

v(q_init)
from hpp.corbaserver import Benchmark
benchmark = Benchmark (robot.client, robot, ps)
#ps.client.problem.setRandomSeed(3530408688)
benchmark.seedRange = range (50)

from hpp.gepetto import Viewer

import time  

white=[1.0,1.0,1.0,1.0]
green=[0.23,0.75,0.2,0.5]
yellow=[0.85,0.75,0.15,0.5]

print("chargement robot")
robot = Robot ('robot_boule', True)
robot.setJointBounds ("base_joint_xyz", [0,5,0,2,0,2])	
# room : -5,4,-7,5,0.5,0.5
robot.tf_root = 'base_link'

ps = ProblemSolver (robot)
v = Viewer (ps)
q_init = robot.getCurrentConfig ()

q_init[0:3] = [0.5, 1, 1]	#z=0.4 pour sphere
v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [5,1, 1]
#v (q_goal)

print("chargement map")
v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")



ps.selectPathPlanner("rrtPerso")
print("Debut motion planning")