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 {

tests/CMakeLists.txt

+ADD_SUBDIRECTORY(python)

tests/python/CMakeLists.txt

+SET(${PROJECT_NAME}_PYTHON_TESTS
+  test_talos_walk_contacts
+  test_talos_walk_path
+  test_rbprm_state_6d
+  test_rbprm_state_3d
+  )
+
+FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
+  ADD_PYTHON_UNIT_TEST("py-${TEST}" "tests/python/${TEST}.py")
+  SET_TESTS_PROPERTIES("py-${TEST}" PROPERTIES RUN_SERIAL "ON")
+ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})

tests/python/test_rbprm_state_3d.py

+# Copyright (c) 2020, CNRS
+# Authors: Pierre Fernbach <pfernbac@laas.fr>
+import unittest
+import subprocess
+import time
+from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
+from hpp.corbaserver.rbprm.hyq import Robot
+
+
+class TestRBPRMstate3D(unittest.TestCase):
+    def test_contacts_3d(self):
+        subprocess.run(["killall", "hpp-rbprm-server"])
+        process = subprocess.Popen("hpp-rbprm-server")
+        time.sleep(3)
+        fullbody = Robot()
+        fullbody.client.robot.setDimensionExtraConfigSpace(6)
+        fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
+        fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
+        fullbody.loadAllLimbs("static", nbSamples=10000)
+        q = fullbody.referenceConfig[::] + [0] * 6
+        fullbody.setCurrentConfig(q)
+        com = fullbody.getCenterOfMass()
+        contacts = [fullbody.rLegId, fullbody.lLegId, fullbody.rArmId, fullbody.lArmId]
+        state = State(fullbody, q=q, limbsIncontact=contacts)
+        self.assertTrue(state.isBalanced())
+        self.assertTrue(state.isValid()[0])
+        self.assertTrue(state.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state.isLimbInContact(fullbody.lLegId))
+        self.assertTrue(state.isLimbInContact(fullbody.rArmId))
+        self.assertTrue(state.isLimbInContact(fullbody.lArmId))
+        self.assertEqual(com, state.getCenterOfMass())
+
+        # remove rf contact :
+        state1, success = StateHelper.removeContact(state, fullbody.rLegId)
+        self.assertTrue(success)
+        self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
+        self.assertTrue(state.isLimbInContact(fullbody.rArmId))
+        self.assertTrue(state.isLimbInContact(fullbody.lArmId))
+        self.assertEqual(com, state1.getCenterOfMass())
+
+        # create a new contact :
+        n = [0, 0, 1]
+        p = [0.45, -0.2, 0.002]
+        state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
+        self.assertTrue(success)
+        self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
+        self.assertTrue(state.isLimbInContact(fullbody.rArmId))
+        self.assertTrue(state.isLimbInContact(fullbody.lArmId))
+        self.assertTrue(state2.isBalanced())
+
+        process.kill()
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/python/test_rbprm_state_6d.py

+# Copyright (c) 2020, CNRS
+# Authors: Pierre Fernbach <pfernbac@laas.fr>
+import unittest
+import subprocess
+import time
+from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
+from talos_rbprm.talos import Robot
+
+
+class TestRBPRMstate6D(unittest.TestCase):
+    def test_contacts_6d(self):
+        subprocess.run(["killall", "hpp-rbprm-server"])
+        process = subprocess.Popen("hpp-rbprm-server")
+        time.sleep(3)
+        fullbody = Robot()
+        fullbody.client.robot.setDimensionExtraConfigSpace(6)
+        fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
+        fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
+        fullbody.loadAllLimbs("static", nbSamples=10000)
+        q = fullbody.referenceConfig[::] + [0] * 6
+        fullbody.setCurrentConfig(q)
+        com = fullbody.getCenterOfMass()
+        contacts = [fullbody.rLegId, fullbody.lLegId]
+        state = State(fullbody, q=q, limbsIncontact=contacts)
+        self.assertTrue(state.isBalanced())
+        self.assertTrue(state.isValid()[0])
+        self.assertTrue(state.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state.isLimbInContact(fullbody.lLegId))
+        self.assertEqual(com, state.getCenterOfMass())
+
+        # remove rf contact :
+        state1, success = StateHelper.removeContact(state, fullbody.rLegId)
+        self.assertTrue(success)
+        self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
+        self.assertFalse(state1.isBalanced())
+        self.assertEqual(com, state1.getCenterOfMass())
+
+        # create a new contact :
+        n = [0, 0, 1]
+        p = [0.15, -0.085, 0.002]
+        state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
+        self.assertTrue(success)
+        self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
+        self.assertTrue(state2.isBalanced())
+        p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId)
+        self.assertEqual(n, n2)
+        for i in range(3):
+            self.assertAlmostEqual(p[i], p2[i], 2)
+
+        # try to replace a contact :
+        p = [0.2, 0.085, 0.002]
+        state3, success = StateHelper.addNewContact(state2, fullbody.lLegId, p, n)
+        self.assertTrue(success)
+        self.assertTrue(state3.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state3.isLimbInContact(fullbody.lLegId))
+        self.assertTrue(state3.isBalanced())
+
+        ## try com projection:
+        com_target = com[::]
+        com_target[2] -= 0.08
+        success = state.projectToCOM(com_target)
+        self.assertTrue(success)
+        fullbody.setCurrentConfig(state.q())
+        com_state = fullbody.getCenterOfMass()
+        self.assertEqual(com_state, state.getCenterOfMass())
+        for i in range(3):
+            self.assertAlmostEqual(com_state[i], com_target[i], 4)
+
+        # try lockOtherJoints:
+        p = [0.1, -0.085, 0.002]
+        state4, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, lockOtherJoints=True)
+        self.assertTrue(success)
+        self.assertTrue(state4.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state4.isLimbInContact(fullbody.lLegId))
+        self.assertTrue(state4.isBalanced())
+        for i in range(7, 13):
+            self.assertAlmostEqual(state.q()[i], state4.q()[i])
+        for i in range(19, len(q)):
+            self.assertAlmostEqual(state.q()[i], state4.q()[i])
+
+        # try with a rotation
+        rot = [0.259, 0, 0, 0.966]
+        state5, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, rotation=rot)
+        self.assertTrue(success)
+        self.assertTrue(state5.isLimbInContact(fullbody.rLegId))
+        self.assertTrue(state5.isLimbInContact(fullbody.lLegId))
+        self.assertTrue(state5.isBalanced())
+        fullbody.setCurrentConfig(state5.q())
+        rf_pose = fullbody.getJointPosition(fullbody.rfoot)
+        for i in range(4):
+            self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)
+
+        process.kill()
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/python/test_talos_walk_contacts.py

+# Copyright (c) 2020, CNRS
+# Authors: Pierre Fernbach <pfernbac@laas.fr>
+import subprocess
+from importlib import import_module
+import unittest
+import time
+
+PATH = "hpp.corbaserver.rbprm.scenarios.demos"
+
+
+class TestTalosWalkContact(unittest.TestCase):
+    def test_talos_walk_contacts(self):
+        subprocess.run(["killall", "hpp-rbprm-server"])
+        process = subprocess.Popen("hpp-rbprm-server")
+        time.sleep(3)
+        module_scenario = import_module(PATH + ".talos_flatGround")
+        if not hasattr(module_scenario, 'ContactGenerator'):
+            self.assertTrue(False)
+        ContactGenerator = getattr(module_scenario, 'ContactGenerator')
+        cg = ContactGenerator()
+        cg.run()
+        self.assertTrue(len(cg.configs) > 5)
+        self.assertTrue(len(cg.configs) < 10)
+        self.assertEqual(cg.q_init, cg.configs[0])
+        self.assertEqual(cg.q_goal, cg.configs[-1])
+        process.kill()
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/python/test_talos_walk_path.py

+# Copyright (c) 2020, CNRS
+# Authors: Pierre Fernbach <pfernbac@laas.fr>
+import subprocess
+from importlib import import_module
+import os
+import unittest
+import time
+
+PATH = "hpp.corbaserver.rbprm.scenarios.demos"
+
+
+class TestTalosWalkPath(unittest.TestCase):
+    def test_talos_walk_path(self):
+        subprocess.run(["killall", "hpp-rbprm-server"])
+        process = subprocess.Popen("hpp-rbprm-server")
+        time.sleep(3)
+        module_scenario = import_module(PATH + ".talos_flatGround_path")
+        if not hasattr(module_scenario, 'PathPlanner'):
+            self.assertTrue(False)
+        PathPlanner = getattr(module_scenario, 'PathPlanner')
+        planner = PathPlanner()
+        planner.run()
+        ps = planner.ps
+        self.assertEqual(ps.numberPaths(), 2)
+        self.assertEqual(ps.pathLength(0), ps.pathLength(1))
+        self.assertTrue(ps.pathLength(1) > 6.)
+        self.assertTrue(ps.pathLength(1) < 7.)
+        self.assertEqual(planner.q_init, ps.configAtParam(1, 0))
+        self.assertEqual(planner.q_goal, ps.configAtParam(1, ps.pathLength(1)))
+        process.kill()
+
+
+if __name__ == '__main__':
+    unittest.main()