From 7b19e3bb88e70feb7427146782c20a9b8add6b54 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Sun, 19 May 2024 09:11:07 +0200 Subject: [PATCH 1/5] fix: adapts IKSolver params names to ROS2 convention (`.` instead of `/`) so that they can be correctly set from yaml params file --- .../DampedLeastSquaresSolver.h | 3 +-- .../ForwardDynamicsSolver.h | 3 +-- .../cartesian_controller_base/IKSolver.h | 19 +++++++++++++++++++ .../src/DampedLeastSquaresSolver.cpp | 4 ++-- .../src/ForwardDynamicsSolver.cpp | 2 +- cartesian_controller_base/src/IKSolver.cpp | 3 ++- 6 files changed, 26 insertions(+), 8 deletions(-) diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h index 79d8580e..4a50bef6 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h +++ b/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 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 }; diff --git a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h index 85289772..9d8ab45f 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h +++ b/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 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 diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h index 5bfcb357..babc22f3 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -170,6 +170,25 @@ class IKSolver */ void applyJointLimits(); + template + auto auto_declare(const std::string & name, const ParameterT & default_value) + { + if (!m_handle->has_parameter(name)) + { + return m_handle->declare_parameter(name, default_value); + } + else + { + return m_handle->get_parameter(name).get_value(); + } + } + +#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + std::shared_ptr m_handle; +#else + std::shared_ptr m_handle; +#endif + //! The underlying physical system KDL::Chain m_chain; diff --git a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp index 21e13c0a..c38b535a 100644 --- a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp +++ b/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_ptrdeclare_parameter(m_params + "/alpha", 1.0); + auto_declare(m_params + ".alpha", 1.0); return true; } diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp index 7a11cd30..429ba63b 100644 --- a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp +++ b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp @@ -136,7 +136,7 @@ bool ForwardDynamicsSolver::init(std::shared_ptrdeclare_parameter(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", diff --git a/cartesian_controller_base/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp index 20443fda..522f66b1 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -102,11 +102,12 @@ void IKSolver::synchronizeJointPositions( } } -bool IKSolver::init(std::shared_ptr /*nh*/, +bool IKSolver::init(std::shared_ptr 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); From b809006e2e1487b03f1a1bd753d9358fe360a1ff Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Wed, 30 Oct 2024 11:54:49 +0100 Subject: [PATCH 2/5] Remove version-dependent pre processor switch That's no longer needed. --- .../include/cartesian_controller_base/IKSolver.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h index babc22f3..98e65216 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -183,11 +183,7 @@ class IKSolver } } -#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON std::shared_ptr m_handle; -#else - std::shared_ptr m_handle; -#endif //! The underlying physical system KDL::Chain m_chain; From f57243812494d0f8eeb460bb76e481bd1a7d7727 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Wed, 30 Oct 2024 12:21:40 +0100 Subject: [PATCH 3/5] Format previous changes --- cartesian_controller_base/src/IKSolver.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/cartesian_controller_base/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp index 522f66b1..b149ea27 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -102,9 +102,8 @@ void IKSolver::synchronizeJointPositions( } } -bool IKSolver::init(std::shared_ptr nh, - const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, - const KDL::JntArray & lower_pos_limits) +bool IKSolver::init(std::shared_ptr nh, const KDL::Chain & chain, + const KDL::JntArray & upper_pos_limits, const KDL::JntArray & lower_pos_limits) { // Initialize m_handle = nh; From 5ee470225632ca018cc40d6612b10cab9cfec76a Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Wed, 30 Oct 2024 16:32:04 +0100 Subject: [PATCH 4/5] Add a regression test for nested solver parameters --- .../integration_tests/integration_tests.py | 59 ++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/cartesian_controller_tests/integration_tests/integration_tests.py b/cartesian_controller_tests/integration_tests/integration_tests.py index ad87b6ba..0fd733c2 100755 --- a/cartesian_controller_tests/integration_tests/integration_tests.py +++ b/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,16 @@ 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) + + 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) + return future.result() From 504d9d48cc6348caf00c6605bd3ac98cc1402440 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Wed, 30 Oct 2024 16:48:26 +0100 Subject: [PATCH 5/5] Add `mypy` to our pre-commit config Also use explicit hashes for pre-commit hooks. That's a little safer. --- .pre-commit-config.yaml | 9 +++++++-- .../integration_tests/integration_tests.py | 8 ++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c0354474..e86d1d59 100644 --- a/.pre-commit-config.yaml +++ b/.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: diff --git a/cartesian_controller_tests/integration_tests/integration_tests.py b/cartesian_controller_tests/integration_tests/integration_tests.py index 0fd733c2..a71e094b 100755 --- a/cartesian_controller_tests/integration_tests/integration_tests.py +++ b/cartesian_controller_tests/integration_tests/integration_tests.py @@ -272,11 +272,15 @@ 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) + 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) + rclpy.spin_until_future_complete( + self.node, future # type: ignore[attr-defined] + ) return future.result()