diff --git a/tests/unit/test_static_stability.py b/tests/unit/test_static_stability.py new file mode 100644 index 0000000..babde7d --- /dev/null +++ b/tests/unit/test_static_stability.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3 + +from pyhpp.manipulation import Device, Problem, urdf +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) + + +def load_romeo(): + romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" + romeo_srdf = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" + ) + + robot = Device("romeo") + romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf, romeo_pose) + + robot.setJointBounds( + "romeo/root_joint", + [-1, 1, -1, 1, 0, 2, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2], + ) + + return robot + + +class TestStaticStabilityConstraintsFactory(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.robot = load_romeo() + cls.problem = Problem(cls.robot) + cls.factory = StaticStabilityConstraintsFactory(cls.problem, cls.robot) + cls.q0 = cls.robot.currentConfiguration() + cls.left_ankle = "romeo/LAnkleRoll" + cls.right_ankle = "romeo/RAnkleRoll" + + def test_create_static_stability_constraint_returns_dict(self): + constraints = self.factory.createStaticStabilityConstraint( + "test_", "", self.left_ankle, self.right_ankle, self.q0 + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("test_relative-com", constraints) + self.assertIn("test_pose-left-foot", constraints) + self.assertIn("test_pose-right-foot", constraints) + self.assertEqual(len(constraints), 3) + + def test_create_sliding_stability_constraint_returns_dict(self): + constraints = self.factory.createSlidingStabilityConstraint( + "slide_", "", self.left_ankle, self.right_ankle, self.q0 + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("slide_relative-com", constraints) + self.assertIn("slide_relative-pose", constraints) + self.assertIn("slide_pose-left-foot", constraints) + self.assertIn("slide_pose-left-foot-complement", constraints) + self.assertEqual(len(constraints), 4) + + def test_create_aligned_com_stability_constraint_returns_dict(self): + constraints = self.factory.createAlignedCOMStabilityConstraint( + "aligned_", "", self.left_ankle, self.right_ankle, self.q0, sliding=False + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("aligned_com-between-feet", constraints) + self.assertIn("aligned_pose-left-foot", constraints) + self.assertIn("aligned_pose-right-foot", constraints) + self.assertEqual(len(constraints), 3) + + def test_create_aligned_com_stability_constraint_sliding(self): + constraints = self.factory.createAlignedCOMStabilityConstraint( + "aligned_slide_", + "", + self.left_ankle, + self.right_ankle, + self.q0, + sliding=True, + ) + + self.assertIsInstance(constraints, dict) + self.assertEqual(len(constraints), 3) + + +if __name__ == "__main__": + unittest.main()