Merge pull request #245 from domrachev03/fix/motor_inertias

[fix] missing const in arguments of `RobotWrapper.set_rotor_inertia` and `RobotWrapper.set_gear_ratios` python bindings
stack-of-tasks · Nov 21, 2024 · 0d62ba2 · 0d62ba2
2 parents 36e3562 + cc10df3
commit 0d62ba2
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 2 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -7,6 +7,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+- Fix missing `const` specifier in python bindings for methods `RobotInertia.set_rotor_inertia()`
+ and `RobotInertia.set_gear_ratios()`
 ## [1.7.1] - 2024-08-26
 
 - Fix a typo in ex_4_walking

diff --git a/include/tsid/bindings/python/robots/robot-wrapper.hpp b/include/tsid/bindings/python/robots/robot-wrapper.hpp
@@ -129,10 +129,11 @@ struct RobotPythonVisitor
   static Eigen::VectorXd gear_ratios(const Robot &self) {
     return self.gear_ratios();
   }
-  static bool set_rotor_inertias(Robot &self, Eigen::VectorXd &rotor_inertias) {
+  static bool set_rotor_inertias(Robot &self,
+                                 const Eigen::VectorXd &rotor_inertias) {
     return self.rotor_inertias(rotor_inertias);
   }
-  static bool set_gear_ratios(Robot &self, Eigen::VectorXd &gear_ratios) {
+  static bool set_gear_ratios(Robot &self, const Eigen::VectorXd &gear_ratios) {
     return self.gear_ratios(gear_ratios);
   }
 

diff --git a/tests/python/test_RobotWrapper.py b/tests/python/test_RobotWrapper.py
@@ -29,9 +29,31 @@
 q = se3.randomConfiguration(robot.model(), lb, ub)
 print(q.transpose())
 
+
 data = robot.data()
 v = np.ones(robot.nv)
 robot.computeAllTerms(data, q, v)
 print(robot.com(data))
+base_mass_matrix = robot.mass(data)
+
+# Adding motor inertia
+rng = np.random.default_rng()
+rotor_inertia = rng.uniform(0, 1, size=(robot.nq - 7))
+gear_ratio = rng.uniform(0, 1, size=(robot.nq - 7))
+expected_inertia = rotor_inertia * gear_ratio**2
+print(f"Expected motors inertia: {expected_inertia}")
+
+robot.set_rotor_inertias(rotor_inertia)
+robot.set_gear_ratios(gear_ratio)
+
+robot.computeAllTerms(data, q, v)
+
+mass_matrix_with_motor_inertia = robot.mass(data)
+
+# Assert that mass matrices are different by motor's inertia
+np.testing.assert_allclose(
+    np.diag(mass_matrix_with_motor_inertia - base_mass_matrix)[6:], expected_inertia
+)
+
 
 print("All test is done")