diff --git a/unittest/test_whole_body_state.py b/unittest/test_whole_body_state.py index 05b9c9f..7c6039b 100755 --- a/unittest/test_whole_body_state.py +++ b/unittest/test_whole_body_state.py @@ -82,8 +82,8 @@ def setUp(self): def test_publisher_without_contact(self): model = pinocchio.buildSampleModelHumanoid() - sub = WholeBodyStateRosSubscriber(model, "whole_body_state") - pub = WholeBodyStateRosPublisher(model, "whole_body_state") + sub = WholeBodyStateRosSubscriber(model, "whole_body_state_without_contact") + pub = WholeBodyStateRosPublisher(model, "whole_body_state_without_contact") time.sleep(1) # publish whole-body state messages q = pinocchio.randomConfiguration(model) @@ -94,6 +94,12 @@ def test_publisher_without_contact(self): pub.publish(self.t, q, v, tau) if sub.has_new_msg(): break + # get whole-body state + _t, _q, _v, _tau, _, _, _, _ = sub.get_state() + self.assertEqual(self.t, _t, "Wrong time interval") + self.assertTrue(np.allclose(q, _q, atol=1e-9), "Wrong q") + self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v") + self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau") def test_communication(self): model = pinocchio.buildSampleModelHumanoid() diff --git a/unittest/test_whole_body_trajectory.py b/unittest/test_whole_body_trajectory.py index 77397f0..35bf6cf 100755 --- a/unittest/test_whole_body_trajectory.py +++ b/unittest/test_whole_body_trajectory.py @@ -94,8 +94,8 @@ 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") + sub = WholeBodyTrajectoryRosSubscriber(model, "whole_body_trajectory_without_contact") + pub = WholeBodyTrajectoryRosPublisher(model, "whole_body_trajectory_without_contact") time.sleep(1) # publish whole-body trajectory messages N = len(self.ts) @@ -110,6 +110,14 @@ def test_publisher_without_contact(self): pub.publish(self.ts, xs, us) if sub.has_new_msg(): break + # get whole-body trajectory + _ts, _xs, _us, _, _, _, _ = sub.get_trajectory() + for i in range(N): + self.assertEqual(self.ts[i], _ts[i], "Wrong time interval at " + str(i)) + self.assertTrue( + np.allclose(xs[i], _xs[i], atol=1e-9), "Wrong x at " + str(i) + ) + self.assertTrue(np.allclose(us, _us, atol=1e-9), "Wrong u at " + str(i)) def test_communication(self): model = pinocchio.buildSampleModelHumanoid()