diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h index 9bc9f161..3cc126f3 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h @@ -106,8 +106,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 db6cc999..96bc6fad 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h @@ -122,8 +122,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 79f0bc04..47ddadae 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -174,6 +174,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 c03eba2f..6917762e 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) @@ -126,7 +126,7 @@ bool DampedLeastSquaresSolver::init(std::shared_ptr nh, m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); m_jnt_jacobian.resize(m_number_joints); - nh->declare_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 d0d10513..285d605a 100644 --- a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp +++ b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp @@ -140,7 +140,7 @@ bool ForwardDynamicsSolver::init(std::shared_ptr nh, 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(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 4d8e00d2..a937bc16 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -103,14 +103,15 @@ void IKSolver::synchronizeJointPositions( } #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -bool IKSolver::init(std::shared_ptr /*nh*/, +bool IKSolver::init(std::shared_ptr nh, #else -bool IKSolver::init(std::shared_ptr /*nh*/, +bool IKSolver::init(std::shared_ptr nh, #endif 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);