[unittest] Tested API without contact

cmastalli · Jan 27, 2024 · 6102545 · 6102545
1 parent 244c790
commit 6102545
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 2 deletions.
diff --git a/unittest/test_whole_body_state.py b/unittest/test_whole_body_state.py
@@ -3,7 +3,7 @@
 ###############################################################################
 # BSD 3-Clause License
 #
-# Copyright (C) 2023-2023, Heriot-Watt University
+# Copyright (C) 2023-2024, Heriot-Watt University
 # Copyright note valid unless otherwise stated in individual files.
 # All rights reserved.
 ###############################################################################
@@ -80,6 +80,21 @@ def setUp(self):
             "rleg_effector_body": [np.random.rand(3), random.uniform(0, 1)],
         }
 
+    def test_publisher_without_contact(self):
+        model = pinocchio.buildSampleModelHumanoid()
+        sub = WholeBodyStateRosSubscriber(model, "whole_body_state")
+        pub = WholeBodyStateRosPublisher(model, "whole_body_state")
+        time.sleep(1)
+        # publish whole-body state messages
+        q = pinocchio.randomConfiguration(model)
+        q[:3] = np.random.rand(3)
+        v = np.random.rand(model.nv)
+        tau = np.random.rand(model.nv - 6)
+        while True:
+            pub.publish(self.t, q, v, tau)
+            if sub.has_new_msg():
+                break
+
     def test_communication(self):
         model = pinocchio.buildSampleModelHumanoid()
         sub = WholeBodyStateRosSubscriber(model, "whole_body_state")

diff --git a/unittest/test_whole_body_trajectory.py b/unittest/test_whole_body_trajectory.py
@@ -3,7 +3,7 @@
 ###############################################################################
 # BSD 3-Clause License
 #
-# Copyright (C) 2023-2023, Heriot-Watt University
+# Copyright (C) 2023-2024, Heriot-Watt University
 # Copyright note valid unless otherwise stated in individual files.
 # All rights reserved.
 ###############################################################################
@@ -92,6 +92,25 @@ def setUp(self) -> None:
                 }
             )
 
+    def test_publisher_without_contact(self):
+        model = pinocchio.buildSampleModelHumanoid()
+        sub = WholeBodyTrajectoryRosSubscriber(model, "whole_body_trajectory")
+        pub = WholeBodyTrajectoryRosPublisher(model, "whole_body_trajectory")
+        time.sleep(1)
+        # publish whole-body trajectory messages
+        N = len(self.ts)
+        xs = []
+        for _ in range(N):
+            q = pinocchio.randomConfiguration(model)
+            q[:3] = np.random.rand(3)
+            v = np.random.rand(model.nv)
+            xs.append(np.hstack([q, v]))
+        us = [np.random.rand(model.nv - 6) for _ in range(N)]
+        while True:
+            pub.publish(self.ts, xs, us)
+            if sub.has_new_msg():
+                break
+
     def test_communication(self):
         model = pinocchio.buildSampleModelHumanoid()
         sub = WholeBodyTrajectoryRosSubscriber(model, "whole_body_trajectory")