From 5b2f85b52396d19770855b2f134a3839ef53fe7f Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Fri, 18 Jan 2019 11:22:24 +0100
Subject: [PATCH] [script] delete old scripts

---
 script/tests/darpa_hyq_interp.py         |  99 -------------------
 script/tests/darpa_hyq_path.py           |  84 ----------------
 script/tests/robot_bigStep_STEVE_path.py |  89 -----------------
 script/tests/robot_bigStep_path.py       |  91 -----------------
 script/tests/robot_jumpEasy_path.py      | 101 -------------------
 script/tests/robot_test_path.py          |  91 -----------------
 script/tests/robot_withBridge_path.py    | 119 -----------------------
 script/tests/truck_hrp2_interp.py        | 107 --------------------
 script/tests/truck_hrp2_path.py          |  72 --------------
 9 files changed, 853 deletions(-)
 delete mode 100755 script/tests/darpa_hyq_interp.py
 delete mode 100644 script/tests/darpa_hyq_path.py
 delete mode 100644 script/tests/robot_bigStep_STEVE_path.py
 delete mode 100644 script/tests/robot_bigStep_path.py
 delete mode 100644 script/tests/robot_jumpEasy_path.py
 delete mode 100644 script/tests/robot_test_path.py
 delete mode 100644 script/tests/robot_withBridge_path.py
 delete mode 100755 script/tests/truck_hrp2_interp.py
 delete mode 100755 script/tests/truck_hrp2_path.py

diff --git a/script/tests/darpa_hyq_interp.py b/script/tests/darpa_hyq_interp.py
deleted file mode 100755
index 8f413329..00000000
--- a/script/tests/darpa_hyq_interp.py
+++ /dev/null
@@ -1,99 +0,0 @@
-#Importing helper class for RBPRM
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-from hpp.gepetto import Viewer
-
-#calling script darpa_hyq_path to compute root path
-import darpa_hyq_path as tp
-
-packageName = "hyq_description"
-meshPackageName = "hyq_description"
-rootJointType = "freeflyer"
-
-#  Information to retrieve urdf and srdf files.
-urdfName = "hyq"
-urdfSuffix = ""
-srdfSuffix = ""
-
-#  This time we load the full body model of HyQ
-fullBody = FullBody () 
-fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-
-#  Setting a number of sample configurations used
-nbSamples = 20000
-
-ps = tp.ProblemSolver(fullBody)
-r = tp.Viewer (ps)
-
-rootName = 'base_joint_xyz'
-
-#  Creating limbs
-# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
-cType = "_3_DOF"
-# string identifying the limb
-rLegId = 'rfleg'
-# First joint of the limb, as in urdf file
-rLeg = 'rf_haa_joint'
-# Last joint of the limb, as in urdf file
-rfoot = 'rf_foot_joint'
-# Specifying the distance between last joint and contact surface
-offset = [0.,-0.021,0.]
-# Specifying the contact surface direction when the limb is in rest pose
-normal = [0,1,0]
-# Specifying the rectangular contact surface length
-legx = 0.02; legy = 0.02
-# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
-fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
-
-lLegId = 'lhleg'
-lLeg = 'lh_haa_joint'
-lfoot = 'lh_foot_joint'
-fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
-
-rarmId = 'rhleg'
-rarm = 'rh_haa_joint'
-rHand = 'rh_foot_joint'
-fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
-
-larmId = 'lfleg'
-larm = 'lf_haa_joint'
-lHand = 'lf_foot_joint'
-fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
-
-q_0 = fullBody.getCurrentConfig(); 
-q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
-q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
-
-# Randomly generating a contact configuration at q_init
-fullBody.setCurrentConfig (q_init)
-q_init = fullBody.generateContacts(q_init, [0,0,1])
-
-# Randomly generating a contact configuration at q_end
-fullBody.setCurrentConfig (q_goal)
-q_goal = fullBody.generateContacts(q_goal, [0,0,1])
-
-# specifying the full body configurations as start and goal state of the problem
-fullBody.setStartState(q_init,[])
-fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
-
-
-r(q_init)
-# computing the contact sequence
-configs = fullBody.interpolate(0.1, 0, 0)
-
-
-r.client.gui.removeFromGroup("planning",r.sceneName)
-r.client.gui.removeFromGroup("hyq_trunk_large",r.sceneName)
-r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
-
-import time
-i = 0;
-while(i < (len(configs)-1)):
-  fullBody.draw(configs[i],r); i=i+1; i-1
-  time.sleep(0.3)
-
-
-#~ fullBody.exportAll(r, configs, 'darpa_hyq_robust_1');
-
-
diff --git a/script/tests/darpa_hyq_path.py b/script/tests/darpa_hyq_path.py
deleted file mode 100644
index edcba868..00000000
--- a/script/tests/darpa_hyq_path.py
+++ /dev/null
@@ -1,84 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-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,1]
-pink=[1,0.6,1,1]
-orange=[1,0.42,0,1]
-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]
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'hyq_trunk_large_realist'
-urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
-rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
-rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.5)
-rbprmBuilder.boundSO3([-0.4,0.4,-0.5,0.5,-0.5,0.5])
-rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(3)
-rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0])
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
-ps = ProblemSolver( rbprmBuilder )
-
-r = Viewer (ps)
-
-
-
-q_init = [-2, 0, 0.63,1,0,0,0,0,0,1]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-#~ q_init [0:3] = [2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-q_goal = q_init [::]
-q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
-
-#~ ps.addPathOptimizer("GradientBased")
-# ps.addPathOptimizer("RandomShortcut")
-ps.selectPathPlanner("RRTdynamic")
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-r.loadObstacleModel (packageName, "darpa", "planning")
-r(q_init)
-
-ps.solve ()
-#r.solveAndDisplay("rm",1,0.02)
-
-#r.displayRoadmap("rm",0.005)
-r.displayPathMap("rmPath",0,0.02)
-
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-
-#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.1, "darpa_hyq_robust_20_path.txt")
-#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-#~ 
-#~ pp (2)
-#~ pp (0)
-pp.displayPath(0)
-r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
-pp (0)
-
-#r.client.gui.removeFromGroup("rm",r.sceneName)
-r.client.gui.removeFromGroup("rmPath",r.sceneName)
-#r.client.gui.removeFromGroup("path_1_root",r.sceneName)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-
diff --git a/script/tests/robot_bigStep_STEVE_path.py b/script/tests/robot_bigStep_STEVE_path.py
deleted file mode 100644
index 4d310959..00000000
--- a/script/tests/robot_bigStep_STEVE_path.py
+++ /dev/null
@@ -1,89 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-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,1]
-pink=[1,0.6,1,1]
-orange=[1,0.42,0,1]
-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]
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'robot_test_trunk'
-urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 1.5])
-rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.0])
-rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
-rbprmBuilder.setNormalFilter('robot_test_lleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
-
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
-ps = ProblemSolver( rbprmBuilder )
-
-r = Viewer (ps)
-
-r.loadObstacleModel (packageName, "ground_jump_easy", "planning")
-
-
-q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-4, 1, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-
-q_goal = q_init [::]
-#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
-q_goal [0:3] = [4, -1, 0.9]; r (q_goal) # pont
-
-
-#~ ps.addPathOptimizer("GradientBased")
-ps.addPathOptimizer("RandomShortcut")
-#ps.client.problem.selectSteeringMethod("SteeringParabola")
-#ps.selectPathPlanner("RRTdynamic")
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-
-r(q_init)
-
-# (nameRoadmap,numberIt,colorNode,radiusSphere,sizeAxis,colorEdge
-r.solveAndDisplay("rm",10,white,0.02,1,brown)
-
-
-#t = ps.solve ()
-
-#r.displayRoadmap("rm",0.005)
-
-#r.displayPathMap("rmPath",0,0.02)
-
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-
-pp(0)
-
-
-#pp.displayPath(1,blue)
-#r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
-
-
-pp (1)
-
-#r.client.gui.removeFromGroup("rm",r.sceneName)
-#r.client.gui.removeFromGroup("rmPath",r.sceneName)
-#r.client.gui.removeFromGroup("path_1_root",r.sceneName)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-
diff --git a/script/tests/robot_bigStep_path.py b/script/tests/robot_bigStep_path.py
deleted file mode 100644
index 0f33117c..00000000
--- a/script/tests/robot_bigStep_path.py
+++ /dev/null
@@ -1,91 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-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,1]
-pink=[1,0.6,1,1]
-orange=[1,0.42,0,1]
-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]
-black=[0,0,0,1]
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'robot_test_trunk'
-urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 1.5])
-rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.0])
-rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
-rbprmBuilder.setNormalFilter('robot_test_lleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
-rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(3)
-rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0])
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
-ps = ProblemSolver( rbprmBuilder )
-
-r = Viewer (ps)
-
-r.loadObstacleModel (packageName, "ground_bigStep", "planning")
-
-
-
-q_init = rbprmBuilder.getCurrentConfig ();
-#q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
-q_init [0:3] = [-4, 1, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-
-q_goal = q_init [::]
-#q_goal [0:3] = [-2, 1, 0.6]; r (q_goal) # tryDirectPath
-q_goal [0:3] = [4, -1, 0.9]; r (q_goal) 
-
-
-#~ ps.addPathOptimizer("GradientBased")
-#ps.addPathOptimizer("RandomShortcut")
-ps.selectPathPlanner("RRTdynamic")
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-
-r(q_init)
-
-#r.solveAndDisplay("rm",1,0.02)
-
-
-t = ps.solve ()
-
-r.displayRoadmap("rm",0.005)
-
-#r.displayPathMap("rmPath",0,0.02,colorEdge=blue)
-
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-
-pp.displayPath(0,red)
-
-pp(0)
-
-r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
-
-pp.displayPath(1,black)
-pp (1)
-
-#r.client.gui.removeFromGroup("rm",r.sceneName)
-r.client.gui.removeFromGroup("rmPath",r.sceneName)
-r.client.gui.removeFromGroup("path_1_root",r.sceneName)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-
diff --git a/script/tests/robot_jumpEasy_path.py b/script/tests/robot_jumpEasy_path.py
deleted file mode 100644
index 0ed2d4c4..00000000
--- a/script/tests/robot_jumpEasy_path.py
+++ /dev/null
@@ -1,101 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-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,1]
-pink=[1,0.6,1,1]
-orange=[1,0.42,0,1]
-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]
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'robot_test_trunk'
-urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 2.5])
-rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
-rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
-rbprmBuilder.setNormalFilter('robot_test_lleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
-rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(3)
-rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0])
-
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
-ps = ProblemSolver( rbprmBuilder )
-
-r = Viewer (ps)
-
-r.loadObstacleModel (packageName, "ground_jump_easy", "planning")
-
-
-q_init = rbprmBuilder.getCurrentConfig ();
-#q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
-q_init [0:3] = [-4, 1, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-
-q_goal = q_init [::]
-#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
-q_goal [0:3] = [3, 1, 0.9]; r (q_goal) # pont
-
-
-#~ ps.addPathOptimizer("GradientBased")
-ps.addPathOptimizer("RandomShortcut")
-#ps.client.problem.selectSteeringMethod("SteeringDynamic")
-ps.selectPathPlanner("RRTdynamic")
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-
-r(q_init)
-
-#ps.client.problem.prepareSolveStepByStep()
-#i = 0
-#r.displayRoadmap("rm"+str(i),0.02)
-#ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
-#t = ps.solve ()
-
-r.solveAndDisplay("rm",1,0.02)
-
-
-#t = ps.solve ()
-
-#r.displayRoadmap("rm",0.02)
-
-
-r.displayPathMap("rmPath",0,0.02)
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-
-pp.displayPath(0,r.color.lightGreen)
-pp(0)
-
-
-pp.displayPath(1,blue)
-r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
-
-pp.displayPath(1,black)
-pp (1)
-
-#r.client.gui.removeFromGroup("rm",r.sceneName)
-r.client.gui.removeFromGroup("rmPath",r.sceneName)
-r.client.gui.removeFromGroup("path_1_root",r.sceneName)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-
-
-
-
diff --git a/script/tests/robot_test_path.py b/script/tests/robot_test_path.py
deleted file mode 100644
index 4a7e12b0..00000000
--- a/script/tests/robot_test_path.py
+++ /dev/null
@@ -1,91 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-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,1]
-pink=[1,0.6,1,1]
-orange=[1,0.42,0,1]
-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]
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'robot_test_trunk'
-urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-8,6, -2, 2, 0.4, 1.3])
-rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.0])
-rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
-rbprmBuilder.setNormalFilter('robot_test_lleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
-rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(3)
-rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0])
-
-
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
-ps = ProblemSolver( rbprmBuilder )
-
-r = Viewer (ps)
-
-r.loadObstacleModel (packageName, "groundcrouch", "planning")
-
-
-q_init = rbprmBuilder.getCurrentConfig ();
-q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
-q_init [0:3] = [-6, 0, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-
-q_goal = q_init [::]
-#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
-q_goal [0:3] = [4, 0, 0.9]; r (q_goal) # pont
-
-
-#~ ps.addPathOptimizer("GradientBased")
-ps.addPathOptimizer("RandomShortcut")
-ps.selectPathPlanner("RRTdynamic")
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-
-r(q_init)
-
-r.solveAndDisplay("rm",1,0.02)
-
-
-#t = ps.solve ()
-
-#r.displayRoadmap("rm",0.005)
-
-r.displayPathMap("rmPath",0,0.02)
-
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-
-pp(0)
-
-
-pp.displayPath(1,blue)
-r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
-
-
-pp (1)
-
-#r.client.gui.removeFromGroup("rm",r.sceneName)
-r.client.gui.removeFromGroup("rmPath",r.sceneName)
-r.client.gui.removeFromGroup("path_1_root",r.sceneName)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-
diff --git a/script/tests/robot_withBridge_path.py b/script/tests/robot_withBridge_path.py
deleted file mode 100644
index fdd3c7a0..00000000
--- a/script/tests/robot_withBridge_path.py
+++ /dev/null
@@ -1,119 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-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,1]
-pink=[1,0.6,1,1]
-orange=[1,0.42,0,1]
-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]
-black=[0,0,0,1]
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'robot_test_trunk'
-urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 2.5])
-# limits zyx
-rbprmBuilder.boundSO3([-1,1,-0.2,0.2,-0.2,0.2])
-rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
-rbprmBuilder.setNormalFilter('robot_test_lleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
-rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(3)
-rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0])
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
-ps = ProblemSolver( rbprmBuilder )
-
-r = Viewer (ps)
-
-r.loadObstacleModel (packageName, "ground_jump_easy", "planning")
-
-
-
-q_init = rbprmBuilder.getCurrentConfig ();
-q_init = [-4,1,0.9,1,0,0,0,0,0,1]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-
-q_goal = q_init [::]
-#q_goal [0:3] = [-2, 1, 0.6]; r (q_goal) # tryDirectPath
-q_goal= [4,-1,0.9,1,0,0,0,0,0,1]; r (q_goal) 
-
-
-#~ ps.addPathOptimizer("GradientBased")
-#ps.addPathOptimizer("RandomShortcut")
-#ps.client.problem.selectSteeringMethod("SteeringParabola")
-ps.selectPathPlanner("RRTdynamic")
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-
-r(q_init)
-#ps.solve()
-
-
-r.solveAndDisplay("rm",1,0.02)
-
-#rbprmBuilder.isConfigValid([-4,1,1.9,1,0,0,0,0,0,1])
-
-#t = ps.solve ()
-#r.displayRoadmap("rm",0.005)
-
-#####
-i = 0
-ps.client.problem.prepareSolveStepByStep()
-r.displayRoadmap("rm"+str(i),0.02)
-ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName); r(ps.node(ps.numberNodes()-1));
-
-
-r.displayPathMap("rmPath",0,0.025)
-
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-
-pp.displayPath(0,r.color.lightGreen)
-
-pp(0)
-
-r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
-
-pp.displayPath(1,black)
-pp (1)
-
-#r.client.gui.removeFromGroup("rm",r.sceneName)
-r.client.gui.removeFromGroup("rmPath",r.sceneName)
-r.client.gui.removeFromGroup("path_0_root",r.sceneName)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-
-i=0
-
-ps.clearRoadmap(); ps.solve(); r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName); pp.displayPath(i+1,r.color.lightGreen); i=i+1;
-
-
-
-
-
-
-###############################
-
-
-cu=[[-0.379915,0.75376,-0.0935008],[-0.455828,0.116297,-0.0891179],[-0.324647,-0.161917,0.195276],[-0.41015,-3.40644,-0.0576228],[-0.469389,0.0748034,0.572076],[-0.455828,0.116297,-0.0891179],[-0.469389,0.0748034,0.572076],[-0.455828,0.116297,-0.0891179],[-0.469389,0.0748034,0.572076],[-0.469389,0.0748034,0.572076]]
-r.client.gui.addCurve("c2",cu,blue)
-r.client.gui.addToGroup("c2",r.sceneName)
-r.client.gui.setVisibility("c2","ALWAYS_ON_TOP")
-
-
diff --git a/script/tests/truck_hrp2_interp.py b/script/tests/truck_hrp2_interp.py
deleted file mode 100755
index d514bf30..00000000
--- a/script/tests/truck_hrp2_interp.py
+++ /dev/null
@@ -1,107 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
-from hpp.gepetto import Viewer
-import time 
-
-import truck_hrp2_path as tp
-
-
-
-packageName = "hrp2_14_description"
-meshPackageName = "hrp2_14_description"
-rootJointType = "freeflyer"
-##
-#  Information to retrieve urdf and srdf files.
-urdfName = "hrp2_14"
-urdfSuffix = "_reduced"
-srdfSuffix = ""
-
-fullBody = FullBody ()
-
-fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-fullBody.setJointBounds ("base_joint_xyz", [0,2.2, -1, 1, 0.7, 2.5])
-
-
-ps = tp.ProblemSolver( fullBody )
-r = tp.Viewer (ps)
-r.client.gui.removeFromGroup("planning",r.sceneName)
-r.client.gui.removeFromGroup("hrp2_trunk_flexible",r.sceneName)
-
-#~ AFTER loading obstacles
-rLegId = '0rLeg'
-rLeg = 'RLEG_JOINT0'
-rLegOffset = [0,-0.105,0,]
-rLegNormal = [0,1,0]
-rLegx = 0.09; rLegy = 0.05
-fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.03)
-
-lLegId = '1lLeg'
-lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,-0.105,0]
-lLegNormal = [0,1,0]
-lLegx = 0.09; lLegy = 0.05
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03)
-
-rarmId = '3Rarm'
-rarm = 'RARM_JOINT0'
-rHand = 'RARM_JOINT5'
-rArmOffset = [-0.05,-0.050,-0.050]
-rArmNormal = [1,0,0]
-rArmx = 0.024; rArmy = 0.024
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "forward", 0.05)
-
-larmId = '4Larm'
-larm = 'LARM_JOINT0'
-lHand = 'LARM_JOINT5'
-lArmOffset = [-0.05,-0.050,-0.050]
-lArmNormal = [1,0,0]
-lArmx = 0.024; lArmy = 0.024
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "forward", 0.05)
-
-q_0 = fullBody.getCurrentConfig(); 
-
-confsize = len(tp.q_init)
-q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize]
-q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize]
-
-
-fullBody.setCurrentConfig (q_init)
-#~ q_0 = fullBody.getCurrentConfig(); 
-q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
-
-fullBody.setCurrentConfig (q_goal)
-#~ r(q_goal)
-q_goal = fullBody.generateContacts(q_goal, [0,0,1])
-#~ r(q_goal)
-
-#~ gui = r.client.gui
-
-
-
-
-fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
-#~ fullBody.setStartState(q_init,[rLegId,lLegId])
-#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId])
-#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId])
-#~ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
-#~ fullBody.setEndState(q_goal,[rLegId,lLegIdlarmId])
-fullBody.setEndState(q_goal,[rLegId,lLegId])
-#~ 
-#~ r(q_init)
-configs = fullBody.interpolate(0.1)
-r.loadObstacleModel ('hpp-rbprm-corba', "truck", "contact")
-#~ fullBody.exportAll(r, configs, 'truck_hrp2_not_robust');
-
-
-i = 0;
-while(i < (len(configs)-1)):
-  fullBody.draw(configs[i],r); i=i+1; i-1
-  time.sleep(0.3)
-
-
-
-
-#~ 
-#~ f1 = open("hrp2_standing_29_10_15","w+")
-#~ f1.write(str(configs))
-#~ f1.close()
diff --git a/script/tests/truck_hrp2_path.py b/script/tests/truck_hrp2_path.py
deleted file mode 100755
index 3e43c0fd..00000000
--- a/script/tests/truck_hrp2_path.py
+++ /dev/null
@@ -1,72 +0,0 @@
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.gepetto import Viewer
-
-rootJointType = 'freeflyer'
-packageName = 'hpp-rbprm-corba'
-meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'hrp2_trunk_flexible'
-urdfNameRoms =  ['hrp2_lleg_rom']
-urdfSuffix = ""
-srdfSuffix = ""
-
-rbprmBuilder = Builder ()
-
-rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2.2, -1, 1, 0.7, 2.5])
-#~ rbprmBuilder.setJointBounds ("base_joint_xyz", [-20,20.2, -10, 10, 0.7, 2.5])
-#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
-rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_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([-0.1,0.1,-3,3,-1.0,1.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_goal [0:3] = [0.19, 0.05, 0.9]; r (q_goal)
-
-
-rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
-q_init = rbprmBuilder.getCurrentConfig ();
-q_init[0:9] = [0.27, -0.05, 1.2, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0472]
-
-rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-
-q_goal[0:3] = [2,0,1.7]; 
-q_goal[0:3]=[1.6,-0.1,1.5];
-#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; 
-#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
-
-#~ ps.addPathOptimizer("GradientBased")
-ps.addPathOptimizer("RandomShortcut")
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.client.problem.selectConFigurationShooter("RbprmShooter")
-ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-r.loadObstacleModel (packageName, "truck", "planning")
-t = ps.solve ()
-t
-
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (rbprmBuilder.client.basic, r)
-#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-#~ 
-#~ pp (2)
-r.displayRoadmap("rm",0.02)
-r.displayPathMap("rmPath",0,0.025)
-pp.displayPath(0)
-pp (0)
-
-- 
GitLab