diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c035447..e86d1d5 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_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h index 79d8580..4a50bef 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 8528977..9d8ab45 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 5bfcb35..98e6521 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -170,6 +170,21 @@ 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(); + } + } + + std::shared_ptr m_handle; + //! 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 21e13c0..c38b535 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 7a11cd3..429ba63 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 20443fd..b149ea2 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -102,11 +102,11 @@ 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; m_chain = chain; m_number_joints = m_chain.getNrOfJoints(); m_current_positions.data = ctrl::VectorND::Zero(m_number_joints); diff --git a/cartesian_controller_tests/integration_tests/integration_tests.py b/cartesian_controller_tests/integration_tests/integration_tests.py index ad87b6b..a71e094 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,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()