Merge pull request #62 from pFernbach/devel

Add no-viewer mode and tests

CMakeLists.txt

@@ -72,5 +72,6 @@ endif()
 
 PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
 ADD_SUBDIRECTORY(src)
+ADD_SUBDIRECTORY(tests)
 
 CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh)

src/CMakeLists.txt

@@ -142,7 +142,9 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
     talos_navBauzil_hard_path.py
     talos_navBauzil_hard.py
     talos_navBauzil_path.py
+    talos_navBauzil_obstacles_path.py
     talos_navBauzil.py
+    talos_navBauzil_obstacles.py
     talos_plateformes_path.py
     talos_plateformes.py
     talos_stairs10_path.py 

src/hpp/corbaserver/rbprm/__main__.py

@@ -14,6 +14,8 @@ parser.add_argument('scenario_name',
                     type=str,
                     help="The name of the scenario script to run. "
                     "If a relative path is given, hpp.corbaserver.rbprm.scenarios is prepended")
+parser.add_argument("-n", "--no_viewer", help="Run rbprm without visualization.",action="store_true")
+
 args = parser.parse_args()
 # retrieve argument
 scenario_name = args.scenario_name
@@ -31,27 +33,31 @@ except ImportError:
         print("Cannot import " + scenario_name + ". Check if the path is correct")
         sys.exit(1)
 
-# kill already existing instance of the viewer/server
-subprocess.run(["killall", "gepetto-gui"])
+# kill already existing instance of the server
 subprocess.run(["killall", "hpp-rbprm-server"])
-# run the viewer/server in background
+# run the server in background :
 # stdout and stderr outputs of the child process are redirected to devnull (hidden).
 # preexec_fn is used to ignore ctrl-c signal send to the main script (otherwise they are forwarded to the child process)
-process_viewer = subprocess.Popen("gepetto-gui",
-                                  stdout=subprocess.PIPE,
-                                  stderr=subprocess.DEVNULL,
-                                  preexec_fn=os.setpgrp)
 process_server = subprocess.Popen("hpp-rbprm-server",
                                   stdout=subprocess.PIPE,
                                   stderr=subprocess.DEVNULL,
                                   preexec_fn=os.setpgrp)
+# register cleanup methods to kill server when exiting python interpreter
+atexit.register(process_server.kill)
+
+# do the same for the viewer, exept if --no-viewer flag is set
+if not args.no_viewer:
+    subprocess.run(["killall", "gepetto-gui"])
+    process_viewer = subprocess.Popen("gepetto-gui",
+                                      stdout=subprocess.PIPE,
+                                      stderr=subprocess.DEVNULL,
+                                      preexec_fn=os.setpgrp)
+    atexit.register(process_viewer.kill)
+
+
 # wait a little for the initialization of the server/viewer
 time.sleep(3)
 
-# register cleanup methods to kill viewer/server when exiting python interpreter
-atexit.register(process_server.kill)
-atexit.register(process_viewer.kill)
-
 # Get ContactGenerator or PathPlanner class from the imported module and run it
 if hasattr(module_scenario, 'ContactGenerator'):
     print("# Run contact generation script ...")

src/hpp/corbaserver/rbprm/scenarios/abstract_contact_generator.py

@@ -83,7 +83,7 @@ class AbstractContactGenerator:
         self.fullbody.setPostureWeights(self.weight_postural)
         self.fullbody.usePosturalTaskContactCreation(use_postural_task)
 
-    def load_limbs(self, heuristic="fixedStep06", analysis=None, nb_samples=None, octree_size=None):
+    def load_limbs(self, heuristic="fixedStep06", analysis="ReferenceConfiguration", nb_samples=None, octree_size=None):
         """
         Generate the samples used for each limbs in 'used_limbs'
         :param heuristic: the name of the heuristic used,
@@ -170,7 +170,7 @@ class AbstractContactGenerator:
         self.v(self.q_init)
         print("Generate contact plan ...")
         t_start = time.time()
-        self.configs = self.fullbody.interpolate(0.01,
+        self.configs = self.fullbody.interpolate(self.dt,
                                                  pathId=self.pid,
                                                  robustnessTreshold=self.robustness,
                                                  filterStates=self.filter_states,

src/hpp/corbaserver/rbprm/scenarios/abstract_path_planner.py

@@ -109,27 +109,11 @@ class AbstractPathPlanner:
         vf = ViewerFactory(self.ps)
         self.afftool = AffordanceTool()
         self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
-        self.afftool.loadObstacleModel(env_package, env_name, "planning", vf, reduceSizes=reduce_sizes)
-        try:
-            self.v = vf.createViewer(displayArrows=True)
-        except Exception:
-            print("No viewer started !")
-
-            class FakeViewer():
-                def __init__(self):
-                    return
-
-                def __call__(self, q):
-                    return
-
-                def addLandmark(self, a, b):
-                    return
-
-            self.v = FakeViewer()
-        try:
-            self.pp = PathPlayer(self.v)
-        except Exception:
-            pass
+        self.afftool.loadObstacleModel("package://"+env_package + "/urdf/" + env_name + ".urdf",
+                                       "planning", vf, reduceSizes=reduce_sizes)
+
+        self.v = vf.createViewer(ghost = True, displayArrows=True)
+        self.pp = PathPlayer(self.v)
         for aff_type in visualize_affordances:
             self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
 

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil.py

@@ -5,10 +5,12 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
 class ContactGenerator(TalosContactGenerator):
     def __init__(self):
         super().__init__(PathPlanner())
+        self.robustness = 2.
 
-    def set_reference(self):
-        super().set_reference()
-        self.q_ref = self.fullbody.referenceConfig_elbowsUp[::] + [0] * self.path_planner.extra_dof
+    def load_fullbody(self):
+        super().load_fullbody()
+        self.fullbody.nbSamples = 100000
+        self.fullbody.limbs_names=[self.fullbody.lLegId, self.fullbody.rLegId] # left feet first on this scenario
 
 
 if __name__ == "__main__":

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_hard.py

@@ -5,10 +5,13 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
 class ContactGenerator(TalosContactGenerator):
     def __init__(self):
         super().__init__(PathPlanner())
+        self.robustness = 2.
+
+    def load_fullbody(self):
+        super().load_fullbody()
+        self.fullbody.nbSamples = 100000
+        self.fullbody.limbs_names=[self.fullbody.lLegId, self.fullbody.rLegId] # left feet first on this scenario
 
-    def set_reference(self):
-        super().set_reference()
-        self.q_ref = self.fullbody.referenceConfig_elbowsUp[::] + [0] * self.path_planner.extra_dof
 
     def load_limbs(self, heuristic):
         super().load_limbs(heuristic)

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_hard_path.py

@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
 class PathPlanner(TalosPathPlanner):
     def load_rbprm(self):
         from talos_rbprm.talos_abstract import Robot
-        Robot.urdfName += "_large"  # load the model with conservative bounding boxes for trunk
+        Robot.urdfName = "talos_trunk_large"  # load the model with conservative bounding boxes for trunk
+        self.robot_node_name = "talos_trunk_large"
         self.rbprmBuilder = Robot()
 
     def init_problem(self):

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_obstacles.py

+from hpp.corbaserver.rbprm.scenarios.demos.talos_navBauzil_obstacles_path import PathPlanner
+from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContactGenerator
+
+
+class ContactGenerator(TalosContactGenerator):
+    def __init__(self):
+        super().__init__(PathPlanner())
+        self.robustness = 2.
+
+    def load_fullbody(self):
+        super().load_fullbody()
+        self.fullbody.nbSamples = 100000
+        self.fullbody.limbs_names = [self.fullbody.lLegId, self.fullbody.rLegId]  # left feet first on this scenario
+
+if __name__ == "__main__":
+    cg = ContactGenerator()
+    cg.run()

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_obstacles_path.py

+from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
+
+
+class PathPlanner(TalosPathPlanner):
+    def load_rbprm(self):
+        from talos_rbprm.talos_abstract import Robot
+        Robot.urdfName = "talos_trunk_large"  # load the model with conservative bounding boxes for trunk
+        self.robot_node_name = "talos_trunk_large"
+        self.rbprmBuilder = Robot()
+
+    def init_problem(self):
+        super().init_problem()
+        # greatly increase the number of loops of the random shortcut
+        self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
+        # force the base orientation to follow the direction of motion along the Z axis
+        self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
+
+    def run(self):
+        self.init_problem()
+        self.root_translation_bounds = [-1.5, 4, 0., 3.3, self.rbprmBuilder.ref_height, self.rbprmBuilder.ref_height]
+        self.set_joints_bounds()
+
+        self.q_init[:2] = [-0.9, 1.7]
+        # Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
+        self.q_init[-6:-3] = [0.07, 0, 0]
+        self.q_goal[:2] = [2, 2.6]
+
+        self.init_viewer("multicontact/floor_bauzil_obstacles", visualize_affordances=["Support"],
+                         reduce_sizes=[0.02, 0., 0.])
+        self.init_planner()
+        self.solve()
+        self.display_path()
+        # self.play_path()
+        self.hide_rom()
+
+
+if __name__ == "__main__":
+    planner = PathPlanner()
+    planner.run()

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_path.py

@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
 class PathPlanner(TalosPathPlanner):
     def load_rbprm(self):
         from talos_rbprm.talos_abstract import Robot
-        Robot.urdfName += "_large"  # load the model with conservative bounding boxes for trunk
+        Robot.urdfName = "talos_trunk_large"  # load the model with conservative bounding boxes for trunk
+        self.robot_node_name = "talos_trunk_large"
         self.rbprmBuilder = Robot()
 
     def init_problem(self):

src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs10_path.py

@@ -5,7 +5,8 @@ class PathPlanner(TalosPathPlanner):
     def load_rbprm(self):
         from talos_rbprm.talos_abstract import Robot
         # select ROM model with really conservative ROM shapes
-        Robot.urdfName += "_large_reducedROM"
+        Robot.urdfName = "talos_trunk_large_reducedROM"
+        self.robot_node_name = "talos_trunk_large_reducedROM"
         Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
         self.used_limbs = Robot.urdfNameRom
         self.rbprmBuilder = Robot()

src/hpp/corbaserver/rbprm/scenarios/hrp2_contact_generator.py

@@ -13,6 +13,8 @@ class HRP2ContactGenerator(AbstractContactGenerator):
         self.fullbody = Robot()
         self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
         self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
+        self.fullbody.limbs_names=[self.fullbody.rLegId, self.fullbody.lLegId]
+
 
     def set_joints_bounds(self):
         super().set_joints_bounds()

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_bauzil_with_stairs_path.py

@@ -23,7 +23,8 @@ class PathPlanner(TalosPathPlanner):
 
     def load_rbprm(self):
         # select model with conservative collision geometry for trunk
-        Robot.urdfName += "_large_reducedROM"
+        Robot.urdfName = "talos_trunk_large_reducedROM"
+        self.robot_node_name = "talos_trunk_large_reducedROM"
         # select conservative ROM for feet
         Robot.urdfNameRom += ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
         self.rbprmBuilder = Robot()

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_mazeEas_path.py

@@ -13,7 +13,8 @@ class PathPlanner(TalosPathPlanner):
     def load_rbprm(self):
         from talos_rbprm.talos_abstract import Robot
         # select model with conservative collision geometry for trunk
-        Robot.urdfName += "_large"
+        Robot.urdfName = "talos_trunk_large"
+        self.robot_node_name = "talos_trunk_large"
         self.rbprmBuilder = Robot()
 
     def init_problem(self):

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_navBauzil_path.py

@@ -8,7 +8,8 @@ class PathPlanner(TalosPathPlanner):
 
     def load_rbprm(self):
         from talos_rbprm.talos_abstract import Robot
-        Robot.urdfName += "_large"  # load the model with conservative bounding boxes for trunk
+        Robot.urdfName = "talos_trunk_large"  # load the model with conservative bounding boxes for trunk
+        self.robot_node_name = "talos_trunk_large"
         self.rbprmBuilder = Robot()
 
     def init_problem(self):

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_stairs10_random_path.py

@@ -35,7 +35,8 @@ class PathPlanner(TalosPathPlanner):
     def load_rbprm(self):
         from talos_rbprm.talos_abstract import Robot
         # select ROM model with really conservative ROM shapes
-        Robot.urdfName += "_large_reducedROM"
+        Robot.urdfName = "talos_trunk_large_reducedROM"
+        self.robot_node_name = "talos_trunk_large_reducedROM"
         Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
         self.used_limbs = Robot.urdfNameRom
         self.rbprmBuilder = Robot()

src/hpp/corbaserver/rbprm/scenarios/talos_contact_generator.py

@@ -13,6 +13,7 @@ class TalosContactGenerator(AbstractContactGenerator):
         self.fullbody = Robot()
         self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
         self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
+        self.fullbody.limbs_names=[self.fullbody.rLegId, self.fullbody.lLegId]
 
     def init_viewer(self):
         super().init_viewer()

src/hpp/corbaserver/rbprm/tools/display_tools.py

@@ -51,10 +51,11 @@ def displaySurfaceFromPoints(viewer, p_list, color=[0, 0, 0, 1], name=None):
     if len(p_list) < 2:
         return
     gui = viewer.client.gui
-    if name is None:
+    node_list = gui.getNodeList()
+    if name is None and node_list is not None:
         i = 0
         name = 'surface_' + str(i)
-        while name in gui.getNodeList():
+        while name in node_list:
             i = i + 1
             name = 'surface_' + str(i)
     gui.addCurve(name, p_list, color)

src/rbprmbuilder.impl.cc

@@ -975,7 +975,7 @@ short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name,
       if (rep.success_) {
         lastStatesComputed_.push_back(rep.result_);
         lastStatesComputedTime_.push_back(std::make_pair(-1., rep.result_));
-        return lastStatesComputed_.size() - 1;
+        return (CORBA::Short)(lastStatesComputed_.size() - 1);
       }
     }
     return -1;
@@ -2787,7 +2787,7 @@ CORBA::Short RbprmBuilder::isLimbInContactIntermediary(const char* limbName, uns
   }
 }
 
-CORBA::Short RbprmBuilder::getNumStates() throw(hpp::Error) { return lastStatesComputed_.size(); }
+CORBA::Short RbprmBuilder::getNumStates() throw(hpp::Error) { return (CORBA::Short)lastStatesComputed_.size(); }
 
 CORBA::Short RbprmBuilder::computeIntermediary(unsigned short stateFrom,
                                                unsigned short stateTo) throw(hpp::Error) try {