Merge pull request fzi-forschungszentrum-informatik#196 from MarcoMagriDev/fix-ik-solver-params

Allow to set IKSolver parameters

Author: stefanscherzinger

.pre-commit-config.yaml

@@ -19,15 +19,20 @@ repos:
 
   # Python
   - repo: https://github.com/pycqa/flake8
-    rev: 6.0.0
+    rev: '88a4f9b2f48fc44b025a48fa6a8ac7cc89ef70e0'  # 7.0.0
     hooks:
     -   id: flake8
 
   - repo: https://github.com/psf/black.git
-    rev: 23.7.0
+    rev: '6fdf8a4af28071ed1d079c01122b34c5d587207a' # 24.2.0
     hooks:
     -   id: black
 
+  - repo: https://github.com/pre-commit/mirrors-mypy
+    rev: '9db9854e3041219b1eb619872a2dfaf58adfb20b'  # v1.9.0
+    hooks:
+    -   id: mypy
+
   # C++
   - repo: local
     hooks:

cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h

@@ -103,8 +103,7 @@ class DampedLeastSquaresSolver : public IKSolver
   KDL::Jacobian m_jnt_jacobian;
 
   // Dynamic parameters
-  std::shared_ptr<rclcpp::Node> m_handle;  ///< handle for dynamic parameter interaction
-  const std::string m_params = "solver/damped_least_squares";  ///< namespace for parameter access
+  const std::string m_params = "solver.damped_least_squares";  ///< namespace for parameter access
   double m_alpha;                                              ///< damping coefficient
 };
 

cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h

@@ -119,8 +119,7 @@ class ForwardDynamicsSolver : public IKSolver
   KDL::JntSpaceInertiaMatrix m_jnt_space_inertia;
 
   // Dynamic parameters
-  std::shared_ptr<rclcpp::Node> m_handle;  ///< handle for dynamic parameter interaction
-  const std::string m_params = "solver/forward_dynamics";  ///< namespace for parameter access
+  const std::string m_params = "solver.forward_dynamics";  ///< namespace for parameter access
 
   /**
      * Virtual link mass

cartesian_controller_base/include/cartesian_controller_base/IKSolver.h

@@ -170,6 +170,21 @@ class IKSolver
      */
   void applyJointLimits();
 
+  template <typename ParameterT>
+  auto auto_declare(const std::string & name, const ParameterT & default_value)
+  {
+    if (!m_handle->has_parameter(name))
+    {
+      return m_handle->declare_parameter<ParameterT>(name, default_value);
+    }
+    else
+    {
+      return m_handle->get_parameter(name).get_value<ParameterT>();
+    }
+  }
+
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_handle;
+
   //! The underlying physical system
   KDL::Chain m_chain;
 

cartesian_controller_base/src/DampedLeastSquaresSolver.cpp

@@ -79,7 +79,7 @@ trajectory_msgs::msg::JointTrajectoryPoint DampedLeastSquaresSolver::getJointCon
   // \f$ \dot{q} = ( J^T J + \alpha^2 I )^{-1} J^T f \f$
   ctrl::MatrixND identity;
   identity.setIdentity(m_number_joints, m_number_joints);
-  m_handle->get_parameter(m_params + "/alpha", m_alpha);
+  m_handle->get_parameter(m_params + ".alpha", m_alpha);
 
   m_current_velocities.data =
     (m_jnt_jacobian.data.transpose() * m_jnt_jacobian.data + m_alpha * m_alpha * identity)
@@ -122,7 +122,7 @@ bool DampedLeastSquaresSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleN
   m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
   m_jnt_jacobian.resize(m_number_joints);
 
-  nh->declare_parameter<double>(m_params + "/alpha", 1.0);
+  auto_declare(m_params + ".alpha", 1.0);
 
   return true;
 }

cartesian_controller_base/src/ForwardDynamicsSolver.cpp

@@ -136,7 +136,7 @@ bool ForwardDynamicsSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode
   m_jnt_space_inertia.resize(m_number_joints);
 
   // Set the initial value if provided at runtime, else use default value.
-  m_min = nh->declare_parameter<double>(m_params + "/link_mass", 0.1);
+  m_min = auto_declare(m_params + ".link_mass", 0.1);
 
   RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver initialized");
   RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver has control over %i joints",

cartesian_controller_base/src/IKSolver.cpp

@@ -102,11 +102,11 @@ void IKSolver::synchronizeJointPositions(
   }
 }
 
-bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
-                    const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
-                    const KDL::JntArray & lower_pos_limits)
+bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
+                    const KDL::JntArray & upper_pos_limits, const KDL::JntArray & lower_pos_limits)
 {
   // Initialize
+  m_handle = nh;
   m_chain = chain;
   m_number_joints = m_chain.getNrOfJoints();
   m_current_positions.data = ctrl::VectorND::Zero(m_number_joints);

cartesian_controller_tests/integration_tests/integration_tests.py

@@ -10,10 +10,14 @@
 import time
 import rclpy
 from rclpy.node import Node
+from rclpy.client import Client
+from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue
 from controller_manager_msgs.srv import ListControllers
 from controller_manager_msgs.srv import SwitchController
 from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import WrenchStamped
+from rcl_interfaces.srv import GetParameters, SetParameters
+from typing import Any
 
 
 def generate_test_description():
@@ -52,7 +56,6 @@ def __init__(self, *args):
     def setUpClass(cls):
         rclpy.init()
         cls.node = Node("test_startup")
-        cls.setup_interfaces(cls)
 
         cls.our_controllers = [
             "cartesian_motion_controller",
@@ -65,6 +68,8 @@ def setUpClass(cls):
             "invalid_cartesian_compliance_controller",
         ]
 
+        cls.setup_interfaces(cls)
+
     @classmethod
     def tearDownClass(cls):
         cls.node.destroy_node()
@@ -86,6 +91,20 @@ def setup_interfaces(self):
         if not self.switch_controller.wait_for_service(timeout.nanoseconds / 1e9):
             self.fail("Service switch_controllers not available")
 
+        self.get_parameter_clients = {
+            controller: self.node.create_client(
+                GetParameters, f"/{controller}/get_parameters"
+            )
+            for controller in self.our_controllers[0:3]
+        }
+
+        self.set_parameter_clients = {
+            controller: self.node.create_client(
+                SetParameters, f"/{controller}/set_parameters"
+            )
+            for controller in self.our_controllers[0:3]
+        }
+
         self.target_pose_pub = self.node.create_publisher(
             PoseStamped, "target_frame", 3
         )
@@ -192,6 +211,31 @@ def publish_nan_targets():
             )
             self.stop_controller(name)
 
+    def test_solver_parameters(self):
+        """Check whether we can set and get nested solver parameters"""
+        example_param = "solver.forward_dynamics.link_mass"
+        default_value = 0.1
+        new_value = 0.7
+
+        for client in self.get_parameter_clients.values():
+            result = self.get_parameters(client, [example_param])
+            result = result.values[0].double_value
+            self.assertTrue(result == default_value)
+
+        for client in self.set_parameter_clients.values():
+            param = Parameter(
+                name=example_param,
+                value=ParameterValue(
+                    double_value=new_value, type=ParameterType.PARAMETER_DOUBLE
+                ),
+            )
+            self.set_parameters(client, [param])
+
+        for client in self.get_parameter_clients.values():
+            result = self.get_parameters(client, [example_param])
+            result = result.values[0].double_value
+            self.assertTrue(result == new_value)
+
     def check_state(self, controller, state):
         """Check the controller's state
 
@@ -223,3 +267,20 @@ def perform_switch(self, req):
         req.strictness = req.BEST_EFFORT
         future = self.switch_controller.call_async(req)
         rclpy.spin_until_future_complete(self.node, future)
+
+    def set_parameters(self, client: Client, params: list[Parameter]) -> None:
+        req = SetParameters.Request()
+        req.parameters = params
+        future = client.call_async(req)
+        rclpy.spin_until_future_complete(
+            self.node, future  # type: ignore[attr-defined]
+        )
+
+    def get_parameters(self, client: Client, names: list[str]) -> Any:
+        req = GetParameters.Request()
+        req.names = names
+        future = client.call_async(req)
+        rclpy.spin_until_future_complete(
+            self.node, future  # type: ignore[attr-defined]
+        )
+        return future.result()