From 7776b76ae11f19609d113fc8e74f73f9c41ba935 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 19 Jan 2026 08:32:06 +0100 Subject: [PATCH 1/2] Add static stability constraint creation tests --- tests/unit/test_static_stability.py | 88 +++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 tests/unit/test_static_stability.py diff --git a/tests/unit/test_static_stability.py b/tests/unit/test_static_stability.py new file mode 100644 index 0000000..42366fc --- /dev/null +++ b/tests/unit/test_static_stability.py @@ -0,0 +1,88 @@ +#!/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() From 397af2d7c5216e76c3db1ec8dbcdb2a06e6a7d00 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Jan 2026 08:43:50 +0000 Subject: [PATCH 2/2] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- tests/unit/test_static_stability.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/tests/unit/test_static_stability.py b/tests/unit/test_static_stability.py index 42366fc..babde7d 100644 --- a/tests/unit/test_static_stability.py +++ b/tests/unit/test_static_stability.py @@ -16,7 +16,9 @@ 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" + 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])) @@ -31,7 +33,6 @@ def load_romeo(): class TestStaticStabilityConstraintsFactory(unittest.TestCase): - @classmethod def setUpClass(cls): cls.robot = load_romeo() @@ -77,7 +78,12 @@ def test_create_aligned_com_stability_constraint_returns_dict(self): 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 + "aligned_slide_", + "", + self.left_ankle, + self.right_ankle, + self.q0, + sliding=True, ) self.assertIsInstance(constraints, dict)