From 8653e53130f9b4831bf4ed12cd290fa9cd3154f6 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Tue, 9 Jan 2024 17:53:15 +0100 Subject: [PATCH 1/4] Add code formatting config --- .clang-format | 20 +++++++++++++++++ .clang-tidy | 10 +++++++++ .flake8 | 4 ++++ .github/workflows/ci_format.yml | 18 +++++++++++++++ .pre-commit-config.yaml | 40 +++++++++++++++++++++++++++++++++ 5 files changed, 92 insertions(+) create mode 100644 .clang-format create mode 100644 .clang-tidy create mode 100644 .flake8 create mode 100644 .github/workflows/ci_format.yml create mode 100644 .pre-commit-config.yaml diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..a7d7c8c3 --- /dev/null +++ b/.clang-format @@ -0,0 +1,20 @@ +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterEnum: true +BreakBeforeBraces: Allman +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000..9e3c570d --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,10 @@ +Checks: '-*,readability-identifier-naming' +CheckOptions: + - { key: readability-identifier-naming.NamespaceCase, value: lower_case } + - { key: readability-identifier-naming.ClassCase, value: CamelCase } + - { key: readability-identifier-naming.PrivateMemberPrefix, value: m_ } + - { key: readability-identifier-naming.StructCase, value: CamelCase } + - { key: readability-identifier-naming.FunctionCase, value: camelBack } + - { key: readability-identifier-naming.VariableCase, value: lower_case } + - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ } + - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } diff --git a/.flake8 b/.flake8 new file mode 100644 index 00000000..f5784052 --- /dev/null +++ b/.flake8 @@ -0,0 +1,4 @@ + +[flake8] +# Use black's line length (default 88) instead of the flake8 default of 79: +max-line-length = 88 diff --git a/.github/workflows/ci_format.yml b/.github/workflows/ci_format.yml new file mode 100644 index 00000000..52c44c54 --- /dev/null +++ b/.github/workflows/ci_format.yml @@ -0,0 +1,18 @@ +# Pre-commit as a Github action. +# See: https://github.com/pre-commit/action +# This will run all hooks from the .pre-commit-config.yaml against all files. + +name: Format + +on: + pull_request: + push: + branches: [ros2] + +jobs: + pre-commit: + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v3 + - uses: pre-commit/action@v3.0.0 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..c0354474 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,40 @@ +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + - id: check-byte-order-marker # Forbid UTF-8 byte-order markers + + # Python + - repo: https://github.com/pycqa/flake8 + rev: 6.0.0 + hooks: + - id: flake8 + + - repo: https://github.com/psf/black.git + rev: 23.7.0 + hooks: + - id: black + + # C++ + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] From dd97f79e2a96c61e47a4ab7f4a64cc5393a255b2 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Tue, 9 Jan 2024 18:00:37 +0100 Subject: [PATCH 2/4] Apply formatting to the existing code base --- README.md | 3 +- .../cartesian_compliance_controller.h | 57 ++--- cartesian_compliance_controller/package.xml | 4 +- .../src/cartesian_compliance_controller.cpp | 61 ++--- .../DampedLeastSquaresSolver.h | 51 ++-- .../ForwardDynamicsSolver.h | 61 +++-- .../cartesian_controller_base/IKSolver.h | 121 +++++----- .../JacobianTransposeSolver.h | 39 ++-- .../cartesian_controller_base/PDController.h | 33 ++- .../ROS2VersionConfig.h.in | 2 +- .../SelectivelyDampedLeastSquaresSolver.h | 46 ++-- .../SpatialPDController.h | 23 +- .../cartesian_controller_base/Utility.h | 22 +- .../cartesian_controller_base.h | 173 +++++++------- cartesian_controller_base/package.xml | 4 +- .../src/DampedLeastSquaresSolver.cpp | 129 +++++------ .../src/ForwardDynamicsSolver.cpp | 217 ++++++++---------- cartesian_controller_base/src/IKSolver.cpp | 198 ++++++++-------- .../src/JacobianTransposeSolver.cpp | 99 ++++---- .../src/PDController.cpp | 26 +-- .../SelectivelyDampedLeastSquaresSolver.cpp | 171 +++++++------- .../src/SpatialPDController.cpp | 17 +- .../src/cartesian_controller_base.cpp | 189 +++++++-------- cartesian_controller_examples/package.xml | 4 +- .../motion_control_handle.h | 115 +++++----- cartesian_controller_handles/package.xml | 4 +- .../src/motion_control_handle.cpp | 109 +++++---- cartesian_controller_simulation/README.md | 1 - .../config/controller_manager.yaml | 3 +- .../etc/robot_mujoco.xml | 2 +- .../mujoco_simulator.h | 39 ++-- .../system_interface.h | 41 ++-- .../launch/simulation.launch.py | 50 ++-- .../scripts/test_joint_control.py | 45 ++-- .../src/mujoco_simulator.cpp | 32 +-- .../src/system_interface.cpp | 79 ++++--- .../integration_tests/integration_tests.py | 160 +++++++------ cartesian_controller_tests/package.xml | 20 +- .../etc/button_cmds.yaml | 6 +- .../launch/spacenav.launch.py | 5 +- .../launch/spacenav_buttons.launch.py | 1 + .../launch/spacenav_to_pose.launch.py | 2 +- .../launch/spacenav_to_wrench.launch.py | 2 +- cartesian_controller_utilities/package.xml | 2 +- .../scripts/buttons.py | 35 +-- .../scripts/converter.py | 38 +-- .../scripts/pose.py | 46 ++-- .../cartesian_force_controller.h | 75 +++--- cartesian_force_controller/package.xml | 4 +- .../src/cartesian_force_controller.cpp | 106 +++++---- cartesian_motion_controller/README.md | 1 - .../cartesian_motion_controller.h | 58 ++--- .../src/cartesian_motion_controller.cpp | 92 ++++---- .../JointControllerAdapter.h | 39 ++-- .../joint_to_cartesian_controller.h | 47 ++-- joint_to_cartesian_controller/package.xml | 4 +- .../src/JointControllerAdapter.cpp | 35 +-- .../src/joint_to_cartesian_controller.cpp | 92 ++++---- resources/doc/Solver_details.md | 1 - 59 files changed, 1553 insertions(+), 1588 deletions(-) diff --git a/README.md b/README.md index 358802e3..d0f80566 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,7 @@ slides](https://roscon.ros.org/2019/talks/roscon2019_cartesiancontrollers.pdf) to get an overview. ## Why this package? -Users may refer to `MoveIt` for end-effector motion planning, but +Users may refer to `MoveIt` for end-effector motion planning, but integrating a full planning stack is often unnecessary for simple applications. Additionally, there are a lot of use cases where direct control in task space is mandatory: dynamic following of target poses, such as **visual servoing**, **teleoperation**, **Cartesian teaching,** or @@ -88,4 +88,3 @@ If you are interested in more details, have a look at - *Virtual Forward Dynamics Models for Cartesian Robot Control* ([Paper](https://arxiv.org/pdf/2009.11888.pdf)) - *Contact Skill Imitation Learning for Robot-Independent Assembly Programming* ([Paper](https://arxiv.org/pdf/1908.06272.pdf)) - *Human-Inspired Compliant Controllers for Robotic Assembly* ([PhD Thesis](https://publikationen.bibliothek.kit.edu/1000139834), especially Chapter 4) - diff --git a/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h b/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h index 97135984..dab0986f 100644 --- a/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h +++ b/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h @@ -44,11 +44,11 @@ #include #include #include + #include namespace cartesian_compliance_controller { - /** * @brief A ROS2-control controller for Cartesian compliance control * @@ -69,51 +69,52 @@ namespace cartesian_compliance_controller * where the additional forces are applied. * */ -class CartesianComplianceController -: public cartesian_motion_controller::CartesianMotionController -, public cartesian_force_controller::CartesianForceController +class CartesianComplianceController : public cartesian_motion_controller::CartesianMotionController, + public cartesian_force_controller::CartesianForceController { - public: - CartesianComplianceController(); +public: + CartesianComplianceController(); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - virtual LifecycleNodeInterface::CallbackReturn on_init() override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + virtual LifecycleNodeInterface::CallbackReturn on_init() override; #elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; + virtual controller_interface::return_type init(const std::string & controller_name) override; #endif - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + controller_interface::return_type update(const rclcpp::Time & time, + const rclcpp::Duration & period) override; #elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; + controller_interface::return_type update() override; #endif - using Base = cartesian_controller_base::CartesianControllerBase; - using MotionBase = cartesian_motion_controller::CartesianMotionController; - using ForceBase = cartesian_force_controller::CartesianForceController; + using Base = cartesian_controller_base::CartesianControllerBase; + using MotionBase = cartesian_motion_controller::CartesianMotionController; + using ForceBase = cartesian_force_controller::CartesianForceController; - private: - /** +private: + /** * @brief Compute the net force of target wrench and stiffness-related pose offset * * @return The remaining error wrench, given in robot base frame */ - ctrl::Vector6D computeComplianceError(); - - ctrl::Matrix6D m_stiffness; - std::string m_compliance_ref_link; + ctrl::Vector6D computeComplianceError(); + ctrl::Matrix6D m_stiffness; + std::string m_compliance_ref_link; }; -} +} // namespace cartesian_compliance_controller #endif diff --git a/cartesian_compliance_controller/package.xml b/cartesian_compliance_controller/package.xml index b25e3986..87debe07 100644 --- a/cartesian_compliance_controller/package.xml +++ b/cartesian_compliance_controller/package.xml @@ -6,8 +6,8 @@ Control your robot through Cartesian target poses and target wrenches scherzin BSD-3-Clause - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - Stefan Scherzinger + https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers + Stefan Scherzinger ament_cmake diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp index 1069908a..6865e3fb 100644 --- a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp +++ b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp @@ -37,13 +37,13 @@ */ //----------------------------------------------------------------------------- +#include + #include "cartesian_controller_base/Utility.h" #include "controller_interface/controller_interface.hpp" -#include namespace cartesian_compliance_controller { - CartesianComplianceController::CartesianComplianceController() // Base constructor won't be called in diamond inheritance, so call that // explicitly @@ -53,8 +53,10 @@ CartesianComplianceController::CartesianComplianceController() { } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_init() +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianComplianceController::on_init() { using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; if (MotionBase::on_init() != TYPE::SUCCESS || ForceBase::on_init() != TYPE::SUCCESS) @@ -76,7 +78,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes return TYPE::SUCCESS; } #elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianComplianceController::init(const std::string & controller_name) +controller_interface::return_type CartesianComplianceController::init( + const std::string & controller_name) { using TYPE = controller_interface::return_type; if (MotionBase::init(controller_name) != TYPE::OK || ForceBase::init(controller_name) != TYPE::OK) @@ -90,23 +93,24 @@ controller_interface::return_type CartesianComplianceController::init(const std: } #endif -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_configure( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianComplianceController::on_configure(const rclcpp_lifecycle::State & previous_state) { using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - if (MotionBase::on_configure(previous_state) != TYPE::SUCCESS || ForceBase::on_configure(previous_state) != TYPE::SUCCESS) + if (MotionBase::on_configure(previous_state) != TYPE::SUCCESS || + ForceBase::on_configure(previous_state) != TYPE::SUCCESS) { return TYPE::ERROR; } // Make sure compliance link is part of the robot chain m_compliance_ref_link = get_node()->get_parameter("compliance_ref_link").as_string(); - if(!Base::robotChainContains(m_compliance_ref_link)) + if (!Base::robotChainContains(m_compliance_ref_link)) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - m_compliance_ref_link << " is not part of the kinematic chain from " - << Base::m_robot_base_link << " to " - << Base::m_end_effector_link); + RCLCPP_ERROR_STREAM(get_node()->get_logger(), m_compliance_ref_link + << " is not part of the kinematic chain from " + << Base::m_robot_base_link << " to " + << Base::m_end_effector_link); return TYPE::ERROR; } @@ -116,33 +120,36 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes return TYPE::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_activate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianComplianceController::on_activate(const rclcpp_lifecycle::State & previous_state) { // Base::on_activation(..) will get called twice, // but that's fine. using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - if (MotionBase::on_activate(previous_state) != TYPE::SUCCESS || ForceBase::on_activate(previous_state) != TYPE::SUCCESS) + if (MotionBase::on_activate(previous_state) != TYPE::SUCCESS || + ForceBase::on_activate(previous_state) != TYPE::SUCCESS) { return TYPE::ERROR; } return TYPE::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianComplianceController::on_deactivate(const rclcpp_lifecycle::State & previous_state) { using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - if (MotionBase::on_deactivate(previous_state) != TYPE::SUCCESS || ForceBase::on_deactivate(previous_state) != TYPE::SUCCESS) + if (MotionBase::on_deactivate(previous_state) != TYPE::SUCCESS || + ForceBase::on_deactivate(previous_state) != TYPE::SUCCESS) { return TYPE::ERROR; } return TYPE::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -controller_interface::return_type CartesianComplianceController::update(const rclcpp::Time& time, - const rclcpp::Duration& period) +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +controller_interface::return_type CartesianComplianceController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) #elif defined CARTESIAN_CONTROLLERS_FOXY controller_interface::return_type CartesianComplianceController::update() #endif @@ -162,7 +169,7 @@ controller_interface::return_type CartesianComplianceController::update() ctrl::Vector6D error = computeComplianceError(); // Turn Cartesian error into joint motion - Base::computeJointControlCmds(error,internal_period); + Base::computeJointControlCmds(error, internal_period); } // Write final commands to the hardware interface @@ -186,7 +193,7 @@ ctrl::Vector6D CartesianComplianceController::computeComplianceError() ctrl::Vector6D net_force = // Spring force in base orientation - Base::displayInBaseLink(m_stiffness,m_compliance_ref_link) * MotionBase::computeMotionError() + Base::displayInBaseLink(m_stiffness, m_compliance_ref_link) * MotionBase::computeMotionError() // Sensor and target force in base orientation + ForceBase::computeForceError(); @@ -194,10 +201,10 @@ ctrl::Vector6D CartesianComplianceController::computeComplianceError() return net_force; } -} // namespace - +} // namespace cartesian_compliance_controller // Pluginlib #include -PLUGINLIB_EXPORT_CLASS(cartesian_compliance_controller::CartesianComplianceController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(cartesian_compliance_controller::CartesianComplianceController, + controller_interface::ControllerInterface) diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h index b3a18673..9bc9f161 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h @@ -40,14 +40,16 @@ #ifndef DAMPED_LEAST_SQUARES_SOLVER_H_INCLUDED #define DAMPED_LEAST_SQUARES_SOLVER_H_INCLUDED -#include "rclcpp/node.hpp" #include + #include #include -namespace cartesian_controller_base{ +#include "rclcpp/node.hpp" - /** +namespace cartesian_controller_base +{ +/** * \brief A damped least squares IK solver for Cartesian controllers * * @@ -62,15 +64,15 @@ namespace cartesian_controller_base{ * with a unit stiffness. * * The damped least squares formulation is according to Wampler - * https://ieeexplore.ieee.org/abstract/document/4075580 + * https://ieeexplore.ieee.org/abstract/document/4075580 */ class DampedLeastSquaresSolver : public IKSolver { - public: - DampedLeastSquaresSolver(); - ~DampedLeastSquaresSolver(); +public: + DampedLeastSquaresSolver(); + ~DampedLeastSquaresSolver(); - /** + /** * \brief Compute joint target commands with damped least squares * * \param period The duration in sec for this simulation step @@ -78,11 +80,10 @@ class DampedLeastSquaresSolver : public IKSolver * * \return A point holding positions, velocities and accelerations of each joint */ - trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) override; + trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) override; - /** + /** * \brief Initialize the solver * * \param nh A node handle for namespace-local parameter management @@ -93,25 +94,23 @@ class DampedLeastSquaresSolver : public IKSolver * \return True, if everything went well */ #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool init(std::shared_ptr nh, + bool init(std::shared_ptr nh, #else - bool init(std::shared_ptr nh, + bool init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) override; - - private: - std::shared_ptr m_jnt_jacobian_solver; - KDL::Jacobian m_jnt_jacobian; + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; - // 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 - double m_alpha; ///< damping coefficient +private: + std::shared_ptr m_jnt_jacobian_solver; + 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 + double m_alpha; ///< damping coefficient }; -} +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h index ddcce705..db6cc999 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h @@ -37,13 +37,12 @@ */ //----------------------------------------------------------------------------- - #ifndef FORWARD_DYNAMICS_SOLVER_H_INCLUDED #define FORWARD_DYNAMICS_SOLVER_H_INCLUDED -#include "rclcpp/node.hpp" #include #include + #include #include #include @@ -54,8 +53,10 @@ #include #include -namespace cartesian_controller_base{ +#include "rclcpp/node.hpp" +namespace cartesian_controller_base +{ /*! \brief The default IK solver for Cartesian controllers * * This class computes manipulator joint motion from Cartesian force inputs. @@ -74,11 +75,11 @@ namespace cartesian_controller_base{ */ class ForwardDynamicsSolver : public IKSolver { - public: - ForwardDynamicsSolver(); - ~ForwardDynamicsSolver(); +public: + ForwardDynamicsSolver(); + ~ForwardDynamicsSolver(); - /** + /** * @brief Compute joint target commands with approximate forward dynamics * * The resulting motion is the output of a forward dynamics simulation. It @@ -89,11 +90,10 @@ class ForwardDynamicsSolver : public IKSolver * * @return A point holding positions, velocities and accelerations of each joint */ - trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) override; + trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) override; - /** + /** * @brief Initialize the solver * * @param nh A node handle for namespace-local parameter management @@ -104,39 +104,36 @@ class ForwardDynamicsSolver : public IKSolver * @return True, if everything went well */ #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool init(std::shared_ptr nh, + bool init(std::shared_ptr nh, #else - bool init(std::shared_ptr nh, + bool init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) override; - - private: + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; - //! Build a generic robot model for control - bool buildGenericModel(); +private: + //! Build a generic robot model for control + bool buildGenericModel(); - // Forward dynamics - std::shared_ptr m_jnt_jacobian_solver; - std::shared_ptr m_jnt_space_inertia_solver; - KDL::Jacobian m_jnt_jacobian; - KDL::JntSpaceInertiaMatrix m_jnt_space_inertia; + // Forward dynamics + std::shared_ptr m_jnt_jacobian_solver; + std::shared_ptr m_jnt_space_inertia_solver; + KDL::Jacobian m_jnt_jacobian; + 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 + // Dynamic parameters + std::shared_ptr m_handle; ///< handle for dynamic parameter interaction + const std::string m_params = "solver/forward_dynamics"; ///< namespace for parameter access - /** + /** * Virtual link mass * Virtual mass of the manipulator's links. The smaller this value, the * more does the end-effector (which has a unit mass of 1.0) dominate dynamic * behavior. Near singularities, a bigger value leads to smoother motion. */ - std::atomic m_min = 0.1; + std::atomic m_min = 0.1; }; - -} // namespace +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h index 9423dc90..4415114a 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -37,13 +37,11 @@ */ //----------------------------------------------------------------------------- - #ifndef IKSOLVER_H_INCLUDED #define IKSOLVER_H_INCLUDED -#include "rclcpp/node.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include + #include #include #include @@ -59,8 +57,11 @@ #include #include -namespace cartesian_controller_base{ +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +namespace cartesian_controller_base +{ /*! \brief Base class to compute manipulator joint motion from Cartesian force inputs. * * This is a base class for solvers, whose child classes will implement @@ -68,11 +69,11 @@ namespace cartesian_controller_base{ */ class IKSolver { - public: - IKSolver(); - virtual ~IKSolver(); +public: + IKSolver(); + virtual ~IKSolver(); - /** + /** * @brief Compute joint target commands, using specific IK algorithms * * The resulting motion will be forwarded as reference to the low-level @@ -83,11 +84,10 @@ class IKSolver * * @return A point holding positions, velocities and accelerations of each joint */ - virtual trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) = 0; + virtual trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) = 0; - /** + /** * @brief Get the current end effector pose of the simulated robot * * The last link in the chain from the init() function is taken as end @@ -97,9 +97,9 @@ class IKSolver * @return The end effector pose with respect to the robot base link. This * link is the same as the one implicitly given in the init() function. */ - const KDL::Frame& getEndEffectorPose() const; + const KDL::Frame & getEndEffectorPose() const; - /** + /** * @brief Get the current end effector velocity of the simulated robot * * The last link in the chain from the init() function is taken as end @@ -108,21 +108,21 @@ class IKSolver * @return The end effector vel with respect to the robot base link. The * order is first translation, then rotation. */ - const ctrl::Vector6D& getEndEffectorVel() const; + const ctrl::Vector6D & getEndEffectorVel() const; - /** + /** * @brief Get the current joint positions of the simulated robot * * @return The current joint positions */ - const KDL::JntArray& getPositions() const; + const KDL::JntArray & getPositions() const; - //! Set initial joint configuration - bool setStartState( - const std::vector >& - joint_pos_handles); + //! Set initial joint configuration + bool setStartState( + const std::vector > & + joint_pos_handles); - /** + /** * @brief Synchronize joint positions with the real robot * * Call this periodically in the controller's update() function. @@ -132,11 +132,11 @@ class IKSolver * * @param joint_pos_handles Read handles to the joint positions. */ - void synchronizeJointPositions( - const std::vector >& - joint_pos_handles); + void synchronizeJointPositions( + const std::vector > & + joint_pos_handles); - /** + /** * @brief Initialize the solver * * @param nh A handle to the node's parameter management @@ -147,25 +147,23 @@ class IKSolver * @return True, if everything went well */ #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - virtual bool init(std::shared_ptr nh, + virtual bool init(std::shared_ptr nh, #else - virtual bool init(std::shared_ptr nh, + virtual bool init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits); + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits); - /** + /** * @brief Update the robot kinematics of the solver * * Call this periodically to update the internal simulation's forward * kinematics. */ - void updateKinematics(); + void updateKinematics(); - protected: - - /** +protected: + /** * @brief Make sure positions stay in allowed margins * * Limit internal joint buffers to the position limits provided by URDF. @@ -173,33 +171,32 @@ class IKSolver * If both upper and lower limits are zero, the joint appears fixed. Note * that this is the default urdf initializer if limits are omitted. */ - void applyJointLimits(); - - //! The underlying physical system - KDL::Chain m_chain; - - //! Number of controllable joint - int m_number_joints; - - // Internal buffers - KDL::JntArray m_current_positions; - KDL::JntArray m_current_velocities; - KDL::JntArray m_current_accelerations; - KDL::JntArray m_last_positions; - KDL::JntArray m_last_velocities; - - // Joint limits - KDL::JntArray m_upper_pos_limits; - KDL::JntArray m_lower_pos_limits; - - // Forward kinematics - std::shared_ptr m_fk_pos_solver; - std::shared_ptr m_fk_vel_solver; - KDL::Frame m_end_effector_pose; - ctrl::Vector6D m_end_effector_vel; + void applyJointLimits(); + + //! The underlying physical system + KDL::Chain m_chain; + + //! Number of controllable joint + int m_number_joints; + + // Internal buffers + KDL::JntArray m_current_positions; + KDL::JntArray m_current_velocities; + KDL::JntArray m_current_accelerations; + KDL::JntArray m_last_positions; + KDL::JntArray m_last_velocities; + + // Joint limits + KDL::JntArray m_upper_pos_limits; + KDL::JntArray m_lower_pos_limits; + + // Forward kinematics + std::shared_ptr m_fk_pos_solver; + std::shared_ptr m_fk_vel_solver; + KDL::Frame m_end_effector_pose; + ctrl::Vector6D m_end_effector_vel; }; - -} // namespace +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h index 2a60ecd4..28529e19 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h @@ -41,12 +41,13 @@ #define JACOBIAN_TRANSPOSE_SOLVER_H_INCLUDED #include + #include #include -namespace cartesian_controller_base{ - - /** +namespace cartesian_controller_base +{ +/** * \brief A Jacobian transpose IK solver for Cartesian controllers * * @@ -62,11 +63,11 @@ namespace cartesian_controller_base{ */ class JacobianTransposeSolver : public IKSolver { - public: - JacobianTransposeSolver(); - ~JacobianTransposeSolver(); +public: + JacobianTransposeSolver(); + ~JacobianTransposeSolver(); - /** + /** * \brief Compute joint target commands with the Jacobian transpose * * \param period The duration in sec for this simulation step @@ -74,11 +75,10 @@ class JacobianTransposeSolver : public IKSolver * * \return A point holding positions, velocities and accelerations of each joint */ - trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) override; + trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) override; - /** + /** * \brief Initialize the solver * * \param chain The kinematic chain of the robot @@ -88,19 +88,18 @@ class JacobianTransposeSolver : public IKSolver * \return True, if everything went well */ #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool init(std::shared_ptr /*nh*/, + bool init(std::shared_ptr /*nh*/, #else - bool init(std::shared_ptr /*nh*/, + bool init(std::shared_ptr /*nh*/, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) override; + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; - private: - std::shared_ptr m_jnt_jacobian_solver; - KDL::Jacobian m_jnt_jacobian; +private: + std::shared_ptr m_jnt_jacobian_solver; + KDL::Jacobian m_jnt_jacobian; }; -} +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/PDController.h b/cartesian_controller_base/include/cartesian_controller_base/PDController.h index 4da4ad77..75d237ef 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/PDController.h +++ b/cartesian_controller_base/include/cartesian_controller_base/PDController.h @@ -28,7 +28,6 @@ // POSSIBILITY OF SUCH DAMAGE. //////////////////////////////////////////////////////////////////////////////// - //----------------------------------------------------------------------------- /*!\file PDController.h * @@ -47,7 +46,6 @@ namespace cartesian_controller_base { - /** * @brief A proportional, derivative controller * @@ -60,33 +58,32 @@ namespace cartesian_controller_base */ class PDController { - public: - PDController(); - ~PDController(); +public: + PDController(); + ~PDController(); #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - void init(const std::string& params, std::shared_ptr handle); + void init(const std::string & params, std::shared_ptr handle); #else - void init(const std::string& params, std::shared_ptr handle); + void init(const std::string & params, std::shared_ptr handle); #endif - double operator()(const double& error, const rclcpp::Duration& period); + double operator()(const double & error, const rclcpp::Duration & period); - private: +private: #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - std::shared_ptr m_handle; + std::shared_ptr m_handle; #else - std::shared_ptr m_handle; ///< handle for dynamic parameter interaction + std::shared_ptr m_handle; ///< handle for dynamic parameter interaction #endif - std::string m_params; ///< namespace for parameter access - - // Gain parameters - double m_p; ///< proportional gain - double m_d; ///< derivative gain - double m_last_p_error; + std::string m_params; ///< namespace for parameter access + // Gain parameters + double m_p; ///< proportional gain + double m_d; ///< derivative gain + double m_last_p_error; }; -} +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/ROS2VersionConfig.h.in b/cartesian_controller_base/include/cartesian_controller_base/ROS2VersionConfig.h.in index 72ca4a31..eae574b8 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/ROS2VersionConfig.h.in +++ b/cartesian_controller_base/include/cartesian_controller_base/ROS2VersionConfig.h.in @@ -4,4 +4,4 @@ #cmakedefine CARTESIAN_CONTROLLERS_FOXY #cmakedefine CARTESIAN_CONTROLLERS_GALACTIC #cmakedefine CARTESIAN_CONTROLLERS_HUMBLE -#cmakedefine CARTESIAN_CONTROLLERS_IRON \ No newline at end of file +#cmakedefine CARTESIAN_CONTROLLERS_IRON diff --git a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h index a89151f3..a5b44be2 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h @@ -41,12 +41,13 @@ #define SELECTIVELY_DAMPED_LEAST_SQUARES_SOLVER_H_INCLUDED #include + #include #include -namespace cartesian_controller_base{ - - /** +namespace cartesian_controller_base +{ +/** * \brief A selectively damped least squares (SDLS) IK solver for Cartesian controllers * * This implements the SDLS method by Buss and Kim from 2005. @@ -65,11 +66,11 @@ namespace cartesian_controller_base{ */ class SelectivelyDampedLeastSquaresSolver : public IKSolver { - public: - SelectivelyDampedLeastSquaresSolver(); - ~SelectivelyDampedLeastSquaresSolver(); +public: + SelectivelyDampedLeastSquaresSolver(); + ~SelectivelyDampedLeastSquaresSolver(); - /** + /** * \brief Compute joint target commands with selectively damped least squares * * \param period The duration in sec for this simulation step @@ -77,11 +78,10 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver * * \return A point holding positions, velocities and accelerations of each joint */ - trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) override; + trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) override; - /** + /** * \brief Initialize the solver * * \param nh A node handle for namespace-local parameter management @@ -92,16 +92,15 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver * \return True, if everything went well */ #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool init(std::shared_ptr nh, + bool init(std::shared_ptr nh, #else - bool init(std::shared_ptr nh, + bool init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) override; + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) override; - private: - /** +private: + /** * @brief Helper function to clamp a column vector * * This literally implements ClampMaxAbs() from Buss' and Kim's paper. @@ -111,14 +110,13 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver * * @return The clamped vector */ - Eigen::Matrix - clampMaxAbs(const Eigen::Matrix& w, double d); - - std::shared_ptr m_jnt_jacobian_solver; - KDL::Jacobian m_jnt_jacobian; + Eigen::Matrix clampMaxAbs( + const Eigen::Matrix & w, double d); + std::shared_ptr m_jnt_jacobian_solver; + KDL::Jacobian m_jnt_jacobian; }; -} +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h b/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h index 9713ea9c..36dc8d69 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h +++ b/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h @@ -42,11 +42,11 @@ #include #include + #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace cartesian_controller_base { - /** * @brief A 6-dimensional PD controller class * @@ -55,16 +55,16 @@ namespace cartesian_controller_base */ class SpatialPDController { - public: - SpatialPDController(); +public: + SpatialPDController(); #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool init(std::shared_ptr params); + bool init(std::shared_ptr params); #else - bool init(std::shared_ptr params); + bool init(std::shared_ptr params); #endif - /** + /** * @brief Call operator for one control cycle * * @param error The control error to reduce. Target - current. @@ -72,14 +72,13 @@ class SpatialPDController * * @return The controlled 6-dim vector (translational, rotational). */ - ctrl::Vector6D operator()(const ctrl::Vector6D& error, const rclcpp::Duration& period); - - private: - ctrl::Vector6D m_cmd; - std::vector m_pd_controllers; + ctrl::Vector6D operator()(const ctrl::Vector6D & error, const rclcpp::Duration & period); +private: + ctrl::Vector6D m_cmd; + std::vector m_pd_controllers; }; -} // namespace +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/Utility.h b/cartesian_controller_base/include/cartesian_controller_base/Utility.h index 5b8bf85a..0e61f620 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/Utility.h +++ b/cartesian_controller_base/include/cartesian_controller_base/Utility.h @@ -42,26 +42,26 @@ #include - /*! \brief Convenience typedefs +/*! \brief Convenience typedefs * * Note: For 6D vectors the order is first linear, then angular */ - namespace ctrl{ +namespace ctrl +{ +typedef Eigen::Matrix Vector6D; - typedef Eigen::Matrix Vector6D; +typedef Eigen::VectorXd VectorND; - typedef Eigen::VectorXd VectorND; +typedef Eigen::Vector3d Vector3D; - typedef Eigen::Vector3d Vector3D; +typedef Eigen::MatrixXd MatrixND; - typedef Eigen::MatrixXd MatrixND; +typedef Eigen::Matrix3d Matrix3D; - typedef Eigen::Matrix3d Matrix3D; +typedef Eigen::Matrix Matrix6D; - typedef Eigen::Matrix Matrix6D; +typedef Eigen::Matrix MatrixND; - typedef Eigen::Matrix MatrixND; - - } +} // namespace ctrl #endif diff --git a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h index e72587d8..6699835d 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h +++ b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h @@ -40,11 +40,11 @@ #ifndef CARTESIAN_CONTROLLER_BASE_H_INCLUDED #define CARTESIAN_CONTROLLER_BASE_H_INCLUDED -#include "ROS2VersionConfig.h" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include #include #include +#include + #include #include #include @@ -56,14 +56,15 @@ #include #include #include -#include #include #include #include +#include "ROS2VersionConfig.h" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + namespace cartesian_controller_base { - /** * @brief Base class for each cartesian controller * @@ -76,42 +77,45 @@ namespace cartesian_controller_base */ class CartesianControllerBase : public controller_interface::ControllerInterface { - public: - CartesianControllerBase(); - virtual ~CartesianControllerBase(){}; +public: + CartesianControllerBase(); + virtual ~CartesianControllerBase(){}; - virtual controller_interface::InterfaceConfiguration command_interface_configuration() const override; + virtual controller_interface::InterfaceConfiguration command_interface_configuration() + const override; - virtual controller_interface::InterfaceConfiguration state_interface_configuration() const override; + virtual controller_interface::InterfaceConfiguration state_interface_configuration() + const override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - virtual LifecycleNodeInterface::CallbackReturn on_init() override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + virtual LifecycleNodeInterface::CallbackReturn on_init() override; #elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; + virtual controller_interface::return_type init(const std::string & controller_name) override; #endif - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_shutdown(const rclcpp_lifecycle::State& previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; - protected: - /** +protected: + /** * @brief Write joint control commands to the real hardware * * Depending on the hardware interface used, this is either joint positions * or velocities. */ - void writeJointControlCmds(); + void writeJointControlCmds(); - /** + /** * @brief Compute one control step using forward dynamics simulation * * Check \ref ForwardDynamicsSolver for details. @@ -119,9 +123,9 @@ class CartesianControllerBase : public controller_interface::ControllerInterface * @param error The error to minimize * @param period The period for this control cycle */ - void computeJointControlCmds(const ctrl::Vector6D& error, const rclcpp::Duration& period); + void computeJointControlCmds(const ctrl::Vector6D & error, const rclcpp::Duration & period); - /** + /** * @brief Display the given vector in the given robot base link * * @param vector The quantity to transform @@ -129,9 +133,9 @@ class CartesianControllerBase : public controller_interface::ControllerInterface * * @return The quantity in the robot base frame */ - ctrl::Vector6D displayInBaseLink(const ctrl::Vector6D& vector, const std::string& from); + ctrl::Vector6D displayInBaseLink(const ctrl::Vector6D & vector, const std::string & from); - /** + /** * @brief Display the given tensor in the robot base frame * * @param tensor The quantity to transform @@ -139,9 +143,9 @@ class CartesianControllerBase : public controller_interface::ControllerInterface * * @return The quantity in the robot base frame */ - ctrl::Matrix6D displayInBaseLink(const ctrl::Matrix6D& tensor, const std::string& from); + ctrl::Matrix6D displayInBaseLink(const ctrl::Matrix6D & tensor, const std::string & from); - /** + /** * @brief Display a given vector in a new reference frame * * The vector is assumed to be given in the robot base frame. @@ -151,59 +155,58 @@ class CartesianControllerBase : public controller_interface::ControllerInterface * * @return The quantity in the new frame */ - ctrl::Vector6D displayInTipLink(const ctrl::Vector6D& vector, const std::string& to); + ctrl::Vector6D displayInTipLink(const ctrl::Vector6D & vector, const std::string & to); - /** + /** * @brief Check if specified links are part of the robot chain * * @param s Link to check for existence * * @return True if existent, false otherwise */ - bool robotChainContains(const std::string& s) + bool robotChainContains(const std::string & s) + { + for (const auto & segment : this->m_robot_chain.segments) { - for (const auto& segment : this->m_robot_chain.segments) + if (segment.getName() == s) { - if (segment.getName() == s) - { - return true; - } + return true; } - return false; } + return false; + } - KDL::Chain m_robot_chain; + KDL::Chain m_robot_chain; - std::shared_ptr m_forward_kinematics_solver; + std::shared_ptr m_forward_kinematics_solver; - /** + /** * @brief Allow users to choose the IK solver type on startup */ - std::shared_ptr > m_solver_loader; - std::shared_ptr m_ik_solver; + std::shared_ptr> m_solver_loader; + std::shared_ptr m_ik_solver; - // Dynamic parameters - std::string m_end_effector_link; - std::string m_robot_base_link; - int m_iterations; + // Dynamic parameters + std::string m_end_effector_link; + std::string m_robot_base_link; + int m_iterations; - std::vector > - m_joint_state_pos_handles; + std::vector> + m_joint_state_pos_handles; - private: - - /** +private: + /** * @brief Stop joint motion when in velocity control */ - void stopCurrentMotion() + void stopCurrentMotion() + { + for (size_t i = 0; i < m_joint_cmd_vel_handles.size(); ++i) { - for (size_t i = 0; i < m_joint_cmd_vel_handles.size(); ++i) - { - m_joint_cmd_vel_handles[i].get().set_value(0.0); - } + m_joint_cmd_vel_handles[i].get().set_value(0.0); } + } - /** + /** * @brief Publish the controller's end-effector pose and twist * * The data are w.r.t. the specified robot base link. @@ -212,32 +215,34 @@ class CartesianControllerBase : public controller_interface::ControllerInterface * right after the error computation, and corresponds to the new target * state that will be send to the actuators in this control cycle. */ - void publishStateFeedback(); - - realtime_tools::RealtimePublisherSharedPtr - m_feedback_pose_publisher; - realtime_tools::RealtimePublisherSharedPtr - m_feedback_twist_publisher; - - std::vector m_cmd_interface_types; - std::vector> m_joint_cmd_pos_handles; - std::vector> m_joint_cmd_vel_handles; - - std::vector m_joint_names; - trajectory_msgs::msg::JointTrajectoryPoint m_simulated_joint_motion; - SpatialPDController m_spatial_controller; - ctrl::Vector6D m_cartesian_input; - - // Against multi initialization in multi inheritance scenarios - bool m_initialized = {false}; - bool m_configured = {false}; - bool m_active = {false}; - - // Dynamic parameters - double m_error_scale; - std::string m_robot_description; + void publishStateFeedback(); + + realtime_tools::RealtimePublisherSharedPtr + m_feedback_pose_publisher; + realtime_tools::RealtimePublisherSharedPtr + m_feedback_twist_publisher; + + std::vector m_cmd_interface_types; + std::vector> + m_joint_cmd_pos_handles; + std::vector> + m_joint_cmd_vel_handles; + + std::vector m_joint_names; + trajectory_msgs::msg::JointTrajectoryPoint m_simulated_joint_motion; + SpatialPDController m_spatial_controller; + ctrl::Vector6D m_cartesian_input; + + // Against multi initialization in multi inheritance scenarios + bool m_initialized = {false}; + bool m_configured = {false}; + bool m_active = {false}; + + // Dynamic parameters + double m_error_scale; + std::string m_robot_description; }; -} +} // namespace cartesian_controller_base #endif diff --git a/cartesian_controller_base/package.xml b/cartesian_controller_base/package.xml index c3b9ee19..3b300e97 100644 --- a/cartesian_controller_base/package.xml +++ b/cartesian_controller_base/package.xml @@ -6,8 +6,8 @@ The cartesian_controller_base package scherzin BSD-3-Clause - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - Stefan Scherzinger + https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers + Stefan Scherzinger ament_cmake diff --git a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp index 7cccd6d1..c03eba2f 100644 --- a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp +++ b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp @@ -38,10 +38,11 @@ //----------------------------------------------------------------------------- #include + #include /** - * \class cartesian_controller_base::DampedLeastSquaresSolver + * \class cartesian_controller_base::DampedLeastSquaresSolver * * Users may explicitly specify this solver with \a "damped_least_squares" as \a * ik_solver in their controllers.yaml configuration file for each controller: @@ -59,81 +60,75 @@ * \endcode * */ -PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::DampedLeastSquaresSolver, cartesian_controller_base::IKSolver) - - - - - -namespace cartesian_controller_base{ - - DampedLeastSquaresSolver::DampedLeastSquaresSolver() - : m_alpha(0.01) +PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::DampedLeastSquaresSolver, + cartesian_controller_base::IKSolver) + +namespace cartesian_controller_base +{ +DampedLeastSquaresSolver::DampedLeastSquaresSolver() : m_alpha(0.01) {} + +DampedLeastSquaresSolver::~DampedLeastSquaresSolver() {} + +trajectory_msgs::msg::JointTrajectoryPoint DampedLeastSquaresSolver::getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) +{ + // Compute joint jacobian + m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian); + + // Compute joint velocities according to: + // \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_current_velocities.data = + (m_jnt_jacobian.data.transpose() * m_jnt_jacobian.data + m_alpha * m_alpha * identity) + .inverse() * + m_jnt_jacobian.data.transpose() * net_force; + + // Integrate once, starting with zero motion + m_current_positions.data = + m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds(); + + // Make sure positions stay in allowed margins + applyJointLimits(); + + // Apply results + trajectory_msgs::msg::JointTrajectoryPoint control_cmd; + for (int i = 0; i < m_number_joints; ++i) { + control_cmd.positions.push_back(m_current_positions(i)); + control_cmd.velocities.push_back(m_current_velocities(i)); + + // Accelerations should be left empty. Those values will be interpreted + // by most hardware joint drivers as max. tolerated values. As a + // consequence, the robot will move very slowly. } + control_cmd.time_from_start = period; // valid for this duration - DampedLeastSquaresSolver::~DampedLeastSquaresSolver(){} + // Update for the next cycle + m_last_positions = m_current_positions; - trajectory_msgs::msg::JointTrajectoryPoint DampedLeastSquaresSolver::getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) - { - // Compute joint jacobian - m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian); - - // Compute joint velocities according to: - // \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_current_velocities.data = - (m_jnt_jacobian.data.transpose() * m_jnt_jacobian.data - + m_alpha * m_alpha * identity).inverse() * m_jnt_jacobian.data.transpose() * net_force; - - // Integrate once, starting with zero motion - m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds(); - - // Make sure positions stay in allowed margins - applyJointLimits(); - - // Apply results - trajectory_msgs::msg::JointTrajectoryPoint control_cmd; - for (int i = 0; i < m_number_joints; ++i) - { - control_cmd.positions.push_back(m_current_positions(i)); - control_cmd.velocities.push_back(m_current_velocities(i)); - - // Accelerations should be left empty. Those values will be interpreted - // by most hardware joint drivers as max. tolerated values. As a - // consequence, the robot will move very slowly. - } - control_cmd.time_from_start = period; // valid for this duration - - // Update for the next cycle - m_last_positions = m_current_positions; - - return control_cmd; - } + return control_cmd; +} #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool DampedLeastSquaresSolver::init(std::shared_ptr nh, +bool DampedLeastSquaresSolver::init(std::shared_ptr nh, #else - bool DampedLeastSquaresSolver::init(std::shared_ptr nh, +bool DampedLeastSquaresSolver::init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) - { - IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); - - m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); - m_jnt_jacobian.resize(m_number_joints); + const KDL::Chain & chain, + const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) +{ + IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); - nh->declare_parameter(m_params + "/alpha", 1.0); + m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); + m_jnt_jacobian.resize(m_number_joints); - return true; - } + nh->declare_parameter(m_params + "/alpha", 1.0); + return true; +} -} // namespace +} // namespace cartesian_controller_base diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp index 21bc2cea..d0d10513 100644 --- a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp +++ b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp @@ -37,17 +37,17 @@ */ //----------------------------------------------------------------------------- -#include #include + +#include #include #include #include #include #include - /** - * \class cartesian_controller_base::ForwardDynamicsSolver + * \class cartesian_controller_base::ForwardDynamicsSolver * * Users may explicitly specify it with \a "forward_dynamics" as \a ik_solver * in their controllers.yaml configuration file for each controller: @@ -65,135 +65,122 @@ * \endcode * */ -PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::ForwardDynamicsSolver, cartesian_controller_base::IKSolver) - - - - - -namespace cartesian_controller_base{ - - ForwardDynamicsSolver::ForwardDynamicsSolver() - { - } - - ForwardDynamicsSolver::~ForwardDynamicsSolver(){} - - trajectory_msgs::msg::JointTrajectoryPoint ForwardDynamicsSolver::getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) +PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::ForwardDynamicsSolver, + cartesian_controller_base::IKSolver) + +namespace cartesian_controller_base +{ +ForwardDynamicsSolver::ForwardDynamicsSolver() {} + +ForwardDynamicsSolver::~ForwardDynamicsSolver() {} + +trajectory_msgs::msg::JointTrajectoryPoint ForwardDynamicsSolver::getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) +{ + // Compute joint space inertia matrix with actualized link masses + buildGenericModel(); + m_jnt_space_inertia_solver->JntToMass(m_current_positions, m_jnt_space_inertia); + + // Compute joint jacobian + m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian); + + // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$ + m_current_accelerations.data = + m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force; + + // Numerical time integration with the Euler forward method + m_current_positions.data = m_last_positions.data + m_last_velocities.data * period.seconds(); + m_current_velocities.data = + m_last_velocities.data + m_current_accelerations.data * period.seconds(); + m_current_velocities.data *= 0.9; // 10 % global damping against unwanted null space motion. + // Will cause exponential slow-down without input. + // Make sure positions stay in allowed margins + applyJointLimits(); + + // Apply results + trajectory_msgs::msg::JointTrajectoryPoint control_cmd; + for (int i = 0; i < m_number_joints; ++i) { + control_cmd.positions.push_back(m_current_positions(i)); + control_cmd.velocities.push_back(m_current_velocities(i)); - // Compute joint space inertia matrix with actualized link masses - buildGenericModel(); - m_jnt_space_inertia_solver->JntToMass(m_current_positions,m_jnt_space_inertia); - - // Compute joint jacobian - m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian); - - // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$ - m_current_accelerations.data = m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force; - - // Numerical time integration with the Euler forward method - m_current_positions.data = m_last_positions.data + m_last_velocities.data * period.seconds(); - m_current_velocities.data = m_last_velocities.data + m_current_accelerations.data * period.seconds(); - m_current_velocities.data *= 0.9; // 10 % global damping against unwanted null space motion. - // Will cause exponential slow-down without input. - // Make sure positions stay in allowed margins - applyJointLimits(); - - // Apply results - trajectory_msgs::msg::JointTrajectoryPoint control_cmd; - for (int i = 0; i < m_number_joints; ++i) - { - control_cmd.positions.push_back(m_current_positions(i)); - control_cmd.velocities.push_back(m_current_velocities(i)); - - // Accelerations should be left empty. Those values will be interpreted - // by most hardware joint drivers as max. tolerated values. As a - // consequence, the robot will move very slowly. - } - control_cmd.time_from_start = period; // valid for this duration - - // Update for the next cycle - m_last_positions = m_current_positions; - m_last_velocities = m_current_velocities; - - return control_cmd; + // Accelerations should be left empty. Those values will be interpreted + // by most hardware joint drivers as max. tolerated values. As a + // consequence, the robot will move very slowly. } + control_cmd.time_from_start = period; // valid for this duration + + // Update for the next cycle + m_last_positions = m_current_positions; + m_last_velocities = m_current_velocities; + return control_cmd; +} #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool ForwardDynamicsSolver::init(std::shared_ptr nh, +bool ForwardDynamicsSolver::init(std::shared_ptr nh, #else - bool ForwardDynamicsSolver::init(std::shared_ptr nh, +bool ForwardDynamicsSolver::init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) - { - IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) +{ + IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); - if (!buildGenericModel()) - { - RCLCPP_ERROR(nh->get_logger(), "Something went wrong in setting up the internal model."); - return false; - } + if (!buildGenericModel()) + { + RCLCPP_ERROR(nh->get_logger(), "Something went wrong in setting up the internal model."); + return false; + } - // Forward dynamics - m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); - m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero())); - m_jnt_jacobian.resize(m_number_joints); - m_jnt_space_inertia.resize(m_number_joints); + // Forward dynamics + m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); + m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain, KDL::Vector::Zero())); + m_jnt_jacobian.resize(m_number_joints); + 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); + // Set the initial value if provided at runtime, else use default value. + m_min = nh->declare_parameter(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", m_number_joints); + RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver initialized"); + RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver has control over %i joints", + m_number_joints); - return true; - } + return true; +} - bool ForwardDynamicsSolver::buildGenericModel() +bool ForwardDynamicsSolver::buildGenericModel() +{ + // Set all masses and inertias to minimal (yet stable) values. + double ip_min = 0.000001; + for (size_t i = 0; i < m_chain.segments.size(); ++i) { - // Set all masses and inertias to minimal (yet stable) values. - double ip_min = 0.000001; - for (size_t i = 0; i < m_chain.segments.size(); ++i) + // Fixed joint segment + if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None) { - // Fixed joint segment - if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None) - { - m_chain.segments[i].setInertia( - KDL::RigidBodyInertia::Zero()); - } - else // relatively moving segment - { - m_chain.segments[i].setInertia( - KDL::RigidBodyInertia( - m_min, // mass - KDL::Vector::Zero(), // center of gravity - KDL::RotationalInertia( - ip_min, // ixx - ip_min, // iyy - ip_min // izz - // ixy, ixy, iyz default to 0.0 - ))); - } + m_chain.segments[i].setInertia(KDL::RigidBodyInertia::Zero()); + } + else // relatively moving segment + { + m_chain.segments[i].setInertia( + KDL::RigidBodyInertia(m_min, // mass + KDL::Vector::Zero(), // center of gravity + KDL::RotationalInertia(ip_min, // ixx + ip_min, // iyy + ip_min // izz + // ixy, ixy, iyz default to 0.0 + ))); } - - // Only give the last segment a generic mass and inertia. - // See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting. - double m = 1; - double ip = 1; - m_chain.segments[m_chain.segments.size()-1].setInertia( - KDL::RigidBodyInertia( - m, - KDL::Vector::Zero(), - KDL::RotationalInertia(ip, ip, ip))); - - return true; } + // Only give the last segment a generic mass and inertia. + // See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting. + double m = 1; + double ip = 1; + m_chain.segments[m_chain.segments.size() - 1].setInertia( + KDL::RigidBodyInertia(m, KDL::Vector::Zero(), KDL::RotationalInertia(ip, ip, ip))); + + return true; +} -} // namespace +} // namespace cartesian_controller_base diff --git a/cartesian_controller_base/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp index 77a0b4a7..4d8e00d2 100644 --- a/cartesian_controller_base/src/IKSolver.cpp +++ b/cartesian_controller_base/src/IKSolver.cpp @@ -37,139 +37,125 @@ */ //----------------------------------------------------------------------------- -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/node.hpp" -#include #include + +#include #include #include #include #include #include -namespace cartesian_controller_base{ - - IKSolver::IKSolver() - { - } - - IKSolver::~IKSolver(){} +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/node.hpp" +namespace cartesian_controller_base +{ +IKSolver::IKSolver() {} - const KDL::Frame& IKSolver::getEndEffectorPose() const - { - return m_end_effector_pose; - } +IKSolver::~IKSolver() {} - const ctrl::Vector6D& IKSolver::getEndEffectorVel() const - { - return m_end_effector_vel; - } +const KDL::Frame & IKSolver::getEndEffectorPose() const { return m_end_effector_pose; } - const KDL::JntArray& IKSolver::getPositions() const - { - return m_current_positions; - } +const ctrl::Vector6D & IKSolver::getEndEffectorVel() const { return m_end_effector_vel; } +const KDL::JntArray & IKSolver::getPositions() const { return m_current_positions; } - bool IKSolver::setStartState( - const std::vector >& - joint_pos_handles) +bool IKSolver::setStartState( + const std::vector > & + joint_pos_handles) +{ + // Copy into internal buffers. + for (size_t i = 0; i < joint_pos_handles.size(); ++i) { - // Copy into internal buffers. - for (size_t i = 0; i < joint_pos_handles.size(); ++i) + // Interface type should be checked by the caller. + // Add additional plausibility check just in case. + if (joint_pos_handles[i].get().get_interface_name() == hardware_interface::HW_IF_POSITION) { - // Interface type should be checked by the caller. - // Add additional plausibility check just in case. - if (joint_pos_handles[i].get().get_interface_name() == hardware_interface::HW_IF_POSITION) - { - m_current_positions(i) = joint_pos_handles[i].get().get_value(); - m_current_velocities(i) = 0.0; - m_current_accelerations(i) = 0.0; - m_last_positions(i) = m_current_positions(i); - m_last_velocities(i) = m_current_velocities(i); - } - else - { - return false; - } + m_current_positions(i) = joint_pos_handles[i].get().get_value(); + m_current_velocities(i) = 0.0; + m_current_accelerations(i) = 0.0; + m_last_positions(i) = m_current_positions(i); + m_last_velocities(i) = m_current_velocities(i); + } + else + { + return false; } - return true; } - - - void IKSolver::synchronizeJointPositions( - const std::vector >& - joint_pos_handles) + return true; +} + +void IKSolver::synchronizeJointPositions( + const std::vector > & + joint_pos_handles) +{ + for (size_t i = 0; i < joint_pos_handles.size(); ++i) { - for (size_t i = 0; i < joint_pos_handles.size(); ++i) + // Interface type should be checked by the caller. + // Add additional plausibility check just in case. + if (joint_pos_handles[i].get().get_interface_name() == hardware_interface::HW_IF_POSITION) { - // Interface type should be checked by the caller. - // Add additional plausibility check just in case. - if (joint_pos_handles[i].get().get_interface_name() == hardware_interface::HW_IF_POSITION) - { - m_current_positions(i) = joint_pos_handles[i].get().get_value(); - m_last_positions(i) = m_current_positions(i); - } + m_current_positions(i) = joint_pos_handles[i].get().get_value(); + m_last_positions(i) = m_current_positions(i); } } - +} #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_chain = chain; - m_number_joints = m_chain.getNrOfJoints(); - m_current_positions.data = ctrl::VectorND::Zero(m_number_joints); - m_current_velocities.data = ctrl::VectorND::Zero(m_number_joints); - m_current_accelerations.data = ctrl::VectorND::Zero(m_number_joints); - m_last_positions.data = ctrl::VectorND::Zero(m_number_joints); - m_last_velocities.data = ctrl::VectorND::Zero(m_number_joints); - m_upper_pos_limits = upper_pos_limits; - m_lower_pos_limits = lower_pos_limits; - - // Forward kinematics - m_fk_pos_solver.reset(new KDL::ChainFkSolverPos_recursive(m_chain)); - m_fk_vel_solver.reset(new KDL::ChainFkSolverVel_recursive(m_chain)); - - return true; - } - - void IKSolver::updateKinematics() - { - // Pose w. r. t. base - m_fk_pos_solver->JntToCart(m_current_positions,m_end_effector_pose); - - // Absolute velocity w. r. t. base - KDL::FrameVel vel; - m_fk_vel_solver->JntToCart(KDL::JntArrayVel(m_current_positions,m_current_velocities),vel); - m_end_effector_vel[0] = vel.deriv().vel.x(); - m_end_effector_vel[1] = vel.deriv().vel.y(); - m_end_effector_vel[2] = vel.deriv().vel.z(); - m_end_effector_vel[3] = vel.deriv().rot.x(); - m_end_effector_vel[4] = vel.deriv().rot.y(); - m_end_effector_vel[5] = vel.deriv().rot.z(); - } - - void IKSolver::applyJointLimits() + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) +{ + // Initialize + m_chain = chain; + m_number_joints = m_chain.getNrOfJoints(); + m_current_positions.data = ctrl::VectorND::Zero(m_number_joints); + m_current_velocities.data = ctrl::VectorND::Zero(m_number_joints); + m_current_accelerations.data = ctrl::VectorND::Zero(m_number_joints); + m_last_positions.data = ctrl::VectorND::Zero(m_number_joints); + m_last_velocities.data = ctrl::VectorND::Zero(m_number_joints); + m_upper_pos_limits = upper_pos_limits; + m_lower_pos_limits = lower_pos_limits; + + // Forward kinematics + m_fk_pos_solver.reset(new KDL::ChainFkSolverPos_recursive(m_chain)); + m_fk_vel_solver.reset(new KDL::ChainFkSolverVel_recursive(m_chain)); + + return true; +} + +void IKSolver::updateKinematics() +{ + // Pose w. r. t. base + m_fk_pos_solver->JntToCart(m_current_positions, m_end_effector_pose); + + // Absolute velocity w. r. t. base + KDL::FrameVel vel; + m_fk_vel_solver->JntToCart(KDL::JntArrayVel(m_current_positions, m_current_velocities), vel); + m_end_effector_vel[0] = vel.deriv().vel.x(); + m_end_effector_vel[1] = vel.deriv().vel.y(); + m_end_effector_vel[2] = vel.deriv().vel.z(); + m_end_effector_vel[3] = vel.deriv().rot.x(); + m_end_effector_vel[4] = vel.deriv().rot.y(); + m_end_effector_vel[5] = vel.deriv().rot.z(); +} + +void IKSolver::applyJointLimits() +{ + for (int i = 0; i < m_number_joints; ++i) { - for (int i = 0; i < m_number_joints; ++i) + if (std::isnan(m_lower_pos_limits(i)) || std::isnan(m_upper_pos_limits(i))) { - if (std::isnan(m_lower_pos_limits(i)) || std::isnan(m_upper_pos_limits(i))) - { - // Joint marked as continuous. - continue; - } - m_current_positions(i) = std::clamp( - m_current_positions(i),m_lower_pos_limits(i),m_upper_pos_limits(i)); + // Joint marked as continuous. + continue; } + m_current_positions(i) = + std::clamp(m_current_positions(i), m_lower_pos_limits(i), m_upper_pos_limits(i)); } +} -} // namespace +} // namespace cartesian_controller_base diff --git a/cartesian_controller_base/src/JacobianTransposeSolver.cpp b/cartesian_controller_base/src/JacobianTransposeSolver.cpp index 50c8d75c..5d5c7426 100644 --- a/cartesian_controller_base/src/JacobianTransposeSolver.cpp +++ b/cartesian_controller_base/src/JacobianTransposeSolver.cpp @@ -38,10 +38,11 @@ //----------------------------------------------------------------------------- #include + #include /** - * \class cartesian_controller_base::JacobianTransposeSolver + * \class cartesian_controller_base::JacobianTransposeSolver * * Users may explicitly specify this solver with \a "jacobian_transpose" as \a * ik_solver in their controllers.yaml configuration file for each controller: @@ -54,72 +55,66 @@ * \endcode * */ -PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::JacobianTransposeSolver, cartesian_controller_base::IKSolver) +PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::JacobianTransposeSolver, + cartesian_controller_base::IKSolver) +namespace cartesian_controller_base +{ +JacobianTransposeSolver::JacobianTransposeSolver() {} +JacobianTransposeSolver::~JacobianTransposeSolver() {} +trajectory_msgs::msg::JointTrajectoryPoint JacobianTransposeSolver::getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) +{ + // Compute joint jacobian + m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian); + // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$ + m_current_accelerations.data = m_jnt_jacobian.data.transpose() * net_force; -namespace cartesian_controller_base{ + // Integrate once, starting with zero motion + m_current_velocities.data = 0.5 * m_current_accelerations.data * period.seconds(); - JacobianTransposeSolver::JacobianTransposeSolver() - { - } + // Integrate twice, starting with zero motion + m_current_positions.data = + m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds(); - JacobianTransposeSolver::~JacobianTransposeSolver(){} + // Make sure positions stay in allowed margins + applyJointLimits(); - trajectory_msgs::msg::JointTrajectoryPoint JacobianTransposeSolver::getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) + // Apply results + trajectory_msgs::msg::JointTrajectoryPoint control_cmd; + for (int i = 0; i < m_number_joints; ++i) { - // Compute joint jacobian - m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian); - - // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$ - m_current_accelerations.data = m_jnt_jacobian.data.transpose() * net_force; - - // Integrate once, starting with zero motion - m_current_velocities.data = 0.5 * m_current_accelerations.data * period.seconds(); - - // Integrate twice, starting with zero motion - m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds(); + control_cmd.positions.push_back(m_current_positions(i)); + control_cmd.velocities.push_back(m_current_velocities(i)); - // Make sure positions stay in allowed margins - applyJointLimits(); - - // Apply results - trajectory_msgs::msg::JointTrajectoryPoint control_cmd; - for (int i = 0; i < m_number_joints; ++i) - { - control_cmd.positions.push_back(m_current_positions(i)); - control_cmd.velocities.push_back(m_current_velocities(i)); - - // Accelerations should be left empty. Those values will be interpreted - // by most hardware joint drivers as max. tolerated values. As a - // consequence, the robot will move very slowly. - } - control_cmd.time_from_start = period; // valid for this duration + // Accelerations should be left empty. Those values will be interpreted + // by most hardware joint drivers as max. tolerated values. As a + // consequence, the robot will move very slowly. + } + control_cmd.time_from_start = period; // valid for this duration - // Update for the next cycle - m_last_positions = m_current_positions; + // Update for the next cycle + m_last_positions = m_current_positions; - return control_cmd; - } + return control_cmd; +} #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool JacobianTransposeSolver::init(std::shared_ptr nh, +bool JacobianTransposeSolver::init(std::shared_ptr nh, #else - bool JacobianTransposeSolver::init(std::shared_ptr nh, +bool JacobianTransposeSolver::init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) - { - IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); + const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) +{ + IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); - m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); - m_jnt_jacobian.resize(m_number_joints); + m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); + m_jnt_jacobian.resize(m_number_joints); - return true; - } -} // namespace + return true; +} +} // namespace cartesian_controller_base diff --git a/cartesian_controller_base/src/PDController.cpp b/cartesian_controller_base/src/PDController.cpp index cdd1f2cc..efb41ae6 100644 --- a/cartesian_controller_base/src/PDController.cpp +++ b/cartesian_controller_base/src/PDController.cpp @@ -28,7 +28,6 @@ // POSSIBILITY OF SUCH DAMAGE. //////////////////////////////////////////////////////////////////////////////// - //----------------------------------------------------------------------------- /*!\file PDController.cpp * @@ -39,31 +38,26 @@ //----------------------------------------------------------------------------- #include + #include namespace cartesian_controller_base { +PDController::PDController() : m_last_p_error(0.0) {} -PDController::PDController() - : m_last_p_error(0.0) -{ -} - -PDController::~PDController() -{ -} - +PDController::~PDController() {} #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -void PDController::init(const std::string& params, std::shared_ptr handle) +void PDController::init(const std::string & params, + std::shared_ptr handle) #else -void PDController::init(const std::string& params, std::shared_ptr handle) +void PDController::init(const std::string & params, std::shared_ptr handle) #endif { m_params = params; m_handle = std::move(handle); - auto auto_declare = [this](const std::string& s) + auto auto_declare = [this](const std::string & s) { if (!m_handle->has_parameter(s)) { @@ -76,8 +70,7 @@ void PDController::init(const std::string& params, std::shared_ptr auto_declare(m_params + ".d"); } - -double PDController::operator()(const double& error, const rclcpp::Duration& period) +double PDController::operator()(const double & error, const rclcpp::Duration & period) { if (period == rclcpp::Duration::from_seconds(0.0)) { @@ -93,5 +86,4 @@ double PDController::operator()(const double& error, const rclcpp::Duration& per return result; } - -} +} // namespace cartesian_controller_base diff --git a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp index b338f846..d9eb0b61 100644 --- a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp +++ b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp @@ -38,11 +38,12 @@ //----------------------------------------------------------------------------- #include + #include #include /** - * \class cartesian_controller_base::SelectivelyDampedLeastSquaresSolver + * \class cartesian_controller_base::SelectivelyDampedLeastSquaresSolver * * Users may explicitly specify this solver with \a "selectively_damped_least_squares" as \a * ik_solver in their controllers.yaml configuration file for each controller: @@ -55,118 +56,114 @@ * \endcode * */ -PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::SelectivelyDampedLeastSquaresSolver, cartesian_controller_base::IKSolver) +PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::SelectivelyDampedLeastSquaresSolver, + cartesian_controller_base::IKSolver) +namespace cartesian_controller_base +{ +SelectivelyDampedLeastSquaresSolver::SelectivelyDampedLeastSquaresSolver() {} +SelectivelyDampedLeastSquaresSolver::~SelectivelyDampedLeastSquaresSolver() {} +trajectory_msgs::msg::JointTrajectoryPoint SelectivelyDampedLeastSquaresSolver::getJointControlCmds( + rclcpp::Duration period, const ctrl::Vector6D & net_force) +{ + // Compute joint Jacobian + m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian); + Eigen::JacobiSVD > JSVD( + m_jnt_jacobian.data, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix U = JSVD.matrixU(); + Eigen::Matrix V = JSVD.matrixV(); + Eigen::Matrix s = JSVD.singularValues(); -namespace cartesian_controller_base{ + // Default recommendation by Buss and Kim. + const double gamma_max = 3.141592653 / 4; - SelectivelyDampedLeastSquaresSolver::SelectivelyDampedLeastSquaresSolver() - { - } + Eigen::Matrix sum_phi = + Eigen::Matrix::Zero(m_number_joints); - SelectivelyDampedLeastSquaresSolver::~SelectivelyDampedLeastSquaresSolver(){} + // Compute each joint velocity with the SDLS method. This implements the + // algorithm as described in the paper (but for only one end-effector). + // Also see Buss' own implementation: + // https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html - trajectory_msgs::msg::JointTrajectoryPoint SelectivelyDampedLeastSquaresSolver::getJointControlCmds( - rclcpp::Duration period, - const ctrl::Vector6D& net_force) + for (int i = 0; i < m_number_joints; ++i) { - // Compute joint Jacobian - m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian); - - Eigen::JacobiSVD > JSVD( - m_jnt_jacobian.data, Eigen::ComputeFullU | Eigen::ComputeFullV); - Eigen::Matrix U = JSVD.matrixU(); - Eigen::Matrix V = JSVD.matrixV(); - Eigen::Matrix s = JSVD.singularValues(); - - // Default recommendation by Buss and Kim. - const double gamma_max = 3.141592653 / 4; - - Eigen::Matrix sum_phi = - Eigen::Matrix::Zero(m_number_joints); + double alpha = U.col(i).transpose() * net_force; - // Compute each joint velocity with the SDLS method. This implements the - // algorithm as described in the paper (but for only one end-effector). - // Also see Buss' own implementation: - // https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html - - for (int i = 0; i < m_number_joints; ++i) + double N = U.col(i).head(3).norm(); + double M = 0; + for (int j = 0; j < m_number_joints; ++j) { - double alpha = U.col(i).transpose() * net_force; - - double N = U.col(i).head(3).norm(); - double M = 0; - for (int j = 0; j < m_number_joints; ++j) - { - double rho = m_jnt_jacobian.data.col(j).head(3).norm(); - M += std::abs(V.col(i)[j]) * rho; - } - M *= 1.0 / s[i]; - - double gamma = std::min(1.0, N / M) * gamma_max; - - Eigen::Matrix phi = clampMaxAbs(1.0 / s[i] * alpha * V.col(i), gamma); - sum_phi += phi; + double rho = m_jnt_jacobian.data.col(j).head(3).norm(); + M += std::abs(V.col(i)[j]) * rho; } + M *= 1.0 / s[i]; - m_current_velocities.data = clampMaxAbs(sum_phi, gamma_max); + double gamma = std::min(1.0, N / M) * gamma_max; - // Integrate once, starting with zero motion - m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds(); + Eigen::Matrix phi = + clampMaxAbs(1.0 / s[i] * alpha * V.col(i), gamma); + sum_phi += phi; + } - // Make sure positions stay in allowed margins - applyJointLimits(); + m_current_velocities.data = clampMaxAbs(sum_phi, gamma_max); - // Apply results - trajectory_msgs::msg::JointTrajectoryPoint control_cmd; - for (int i = 0; i < m_number_joints; ++i) - { - control_cmd.positions.push_back(m_current_positions(i)); - control_cmd.velocities.push_back(m_current_velocities(i)); + // Integrate once, starting with zero motion + m_current_positions.data = + m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds(); - // Accelerations should be left empty. Those values will be interpreted - // by most hardware joint drivers as max. tolerated values. As a - // consequence, the robot will move very slowly. - } - control_cmd.time_from_start = period; // valid for this duration + // Make sure positions stay in allowed margins + applyJointLimits(); - // Update for the next cycle - m_last_positions = m_current_positions; + // Apply results + trajectory_msgs::msg::JointTrajectoryPoint control_cmd; + for (int i = 0; i < m_number_joints; ++i) + { + control_cmd.positions.push_back(m_current_positions(i)); + control_cmd.velocities.push_back(m_current_velocities(i)); - return control_cmd; + // Accelerations should be left empty. Those values will be interpreted + // by most hardware joint drivers as max. tolerated values. As a + // consequence, the robot will move very slowly. } + control_cmd.time_from_start = period; // valid for this duration + + // Update for the next cycle + m_last_positions = m_current_positions; + + return control_cmd; +} #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr nh, +bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr nh, #else - bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr nh, +bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr nh, #endif - const KDL::Chain& chain, - const KDL::JntArray& upper_pos_limits, - const KDL::JntArray& lower_pos_limits) + const KDL::Chain & chain, + const KDL::JntArray & upper_pos_limits, + const KDL::JntArray & lower_pos_limits) +{ + IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); + + m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); + m_jnt_jacobian.resize(m_number_joints); + + return true; +} + +Eigen::Matrix SelectivelyDampedLeastSquaresSolver::clampMaxAbs( + const Eigen::Matrix & w, double d) +{ + if (w.cwiseAbs().maxCoeff() <= d) { - IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); - - m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); - m_jnt_jacobian.resize(m_number_joints); - - return true; + return w; } - - Eigen::Matrix SelectivelyDampedLeastSquaresSolver::clampMaxAbs( - const Eigen::Matrix& w, double d) + else { - if (w.cwiseAbs().maxCoeff() <= d) - { - return w; - } - else - { - return d * w / w.cwiseAbs().maxCoeff(); - } + return d * w / w.cwiseAbs().maxCoeff(); } +} -} // namespace +} // namespace cartesian_controller_base diff --git a/cartesian_controller_base/src/SpatialPDController.cpp b/cartesian_controller_base/src/SpatialPDController.cpp index 479bba04..8a6c7c49 100644 --- a/cartesian_controller_base/src/SpatialPDController.cpp +++ b/cartesian_controller_base/src/SpatialPDController.cpp @@ -38,21 +38,20 @@ //----------------------------------------------------------------------------- #include + #include namespace cartesian_controller_base { +SpatialPDController::SpatialPDController() {} -SpatialPDController::SpatialPDController() -{ -} - -ctrl::Vector6D SpatialPDController::operator()(const ctrl::Vector6D& error, const rclcpp::Duration& period) +ctrl::Vector6D SpatialPDController::operator()(const ctrl::Vector6D & error, + const rclcpp::Duration & period) { // Perform pd control separately on each Cartesian dimension - for (int i = 0; i < 6; ++i) // 3 transition, 3 rotation + for (int i = 0; i < 6; ++i) // 3 transition, 3 rotation { - m_cmd(i) = m_pd_controllers[i](error[i],period); + m_cmd(i) = m_pd_controllers[i](error[i], period); } return m_cmd; } @@ -64,7 +63,7 @@ bool SpatialPDController::init(std::shared_ptr handle) #endif { // Create pd controllers for each Cartesian dimension - for (int i = 0; i < 6; ++i) // 3 transition, 3 rotation + for (int i = 0; i < 6; ++i) // 3 transition, 3 rotation { m_pd_controllers.push_back(PDController()); } @@ -82,4 +81,4 @@ bool SpatialPDController::init(std::shared_ptr handle) return true; } -} // namespace +} // namespace cartesian_controller_base diff --git a/cartesian_controller_base/src/cartesian_controller_base.cpp b/cartesian_controller_base/src/cartesian_controller_base.cpp index aad4749e..b51441c4 100644 --- a/cartesian_controller_base/src/cartesian_controller_base.cpp +++ b/cartesian_controller_base/src/cartesian_controller_base.cpp @@ -37,33 +37,33 @@ */ //----------------------------------------------------------------------------- +#include +#include +#include + +#include +#include +#include +#include + #include "controller_interface/controller_interface.hpp" #include "controller_interface/helpers.hpp" #include "geometry_msgs/msg/detail/pose_stamped__struct.hpp" #include "geometry_msgs/msg/detail/twist_stamped__struct.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include -#include -#include -#include -#include -#include -#include namespace cartesian_controller_base { +CartesianControllerBase::CartesianControllerBase() {} -CartesianControllerBase::CartesianControllerBase() -{ -} - -controller_interface::InterfaceConfiguration CartesianControllerBase::command_interface_configuration() const +controller_interface::InterfaceConfiguration +CartesianControllerBase::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; conf.names.reserve(m_joint_names.size() * m_cmd_interface_types.size()); - for (const auto& type : m_cmd_interface_types) + for (const auto & type : m_cmd_interface_types) { for (const auto & joint_name : m_joint_names) { @@ -73,11 +73,12 @@ controller_interface::InterfaceConfiguration CartesianControllerBase::command_in return conf; } -controller_interface::InterfaceConfiguration CartesianControllerBase::state_interface_configuration() const +controller_interface::InterfaceConfiguration +CartesianControllerBase::state_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(m_joint_names.size()); // Only position + conf.names.reserve(m_joint_names.size()); // Only position for (const auto & joint_name : m_joint_names) { conf.names.push_back(joint_name + "/position"); @@ -85,8 +86,10 @@ controller_interface::InterfaceConfiguration CartesianControllerBase::state_inte return conf; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_init() +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianControllerBase::on_init() { if (!m_initialized) { @@ -132,8 +135,8 @@ controller_interface::return_type CartesianControllerBase::init(const std::strin } #endif -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_configure( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianControllerBase::on_configure(const rclcpp_lifecycle::State & previous_state) { if (m_configured) { @@ -148,7 +151,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes { m_ik_solver = m_solver_loader->createSharedInstance(ik_solver); } - catch (pluginlib::PluginlibException& ex) + catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR(get_node()->get_logger(), ex.what()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -156,7 +159,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes // Get kinematics specific configuration urdf::Model robot_model; - KDL::Tree robot_tree; + KDL::Tree robot_tree; m_robot_description = get_node()->get_parameter("robot_description").as_string(); if (m_robot_description.empty()) @@ -183,14 +186,15 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf model from 'robot_description'"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - if (!kdl_parser::treeFromUrdfModel(robot_model,robot_tree)) + if (!kdl_parser::treeFromUrdfModel(robot_model, robot_tree)) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from urdf model"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - if (!robot_tree.getChain(m_robot_base_link,m_end_effector_link,m_robot_chain)) + if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, m_robot_chain)) { - const std::string error = "" + const std::string error = + "" "Failed to parse robot chain from urdf model. " "Do robot_base_link and end_effector_link exist?"; RCLCPP_ERROR(get_node()->get_logger(), error.c_str()); @@ -212,7 +216,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes { if (!robot_model.getJoint(m_joint_names[i])) { - RCLCPP_ERROR(get_node()->get_logger(), "Joint %s does not appear in robot_description", m_joint_names[i].c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Joint %s does not appear in robot_description", + m_joint_names[i].c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } if (robot_model.getJoint(m_joint_names[i])->type == urdf::Joint::CONTINUOUS) @@ -229,9 +234,9 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes } // Initialize solvers - m_ik_solver->init(get_node(),m_robot_chain,upper_pos_limits,lower_pos_limits); + m_ik_solver->init(get_node(), m_robot_chain, upper_pos_limits, lower_pos_limits); KDL::Tree tmp("not_relevant"); - tmp.addChain(m_robot_chain,"not_relevant"); + tmp.addChain(m_robot_chain, "not_relevant"); m_forward_kinematics_solver.reset(new KDL::TreeFkSolverPos_recursive(tmp)); m_iterations = get_node()->get_parameter("solver.iterations").as_int(); m_error_scale = get_node()->get_parameter("solver.error_scale").as_double(); @@ -247,26 +252,24 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes RCLCPP_ERROR(get_node()->get_logger(), "No command_interfaces specified"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - for (const auto& type : m_cmd_interface_types) + for (const auto & type : m_cmd_interface_types) { if (type != hardware_interface::HW_IF_POSITION && type != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Unsupported command interface: %s. Choose position or velocity", - type.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), + "Unsupported command interface: %s. Choose position or velocity", type.c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } // Controller-internal state publishing m_feedback_pose_publisher = - std::make_shared >( + std::make_shared>( get_node()->create_publisher( std::string(get_node()->get_name()) + "/current_pose", 3)); m_feedback_twist_publisher = - std::make_shared >( + std::make_shared>( get_node()->create_publisher( std::string(get_node()->get_name()) + "/current_twist", 3)); @@ -275,8 +278,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_deactivate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianControllerBase::on_deactivate(const rclcpp_lifecycle::State & previous_state) { stopCurrentMotion(); @@ -291,8 +294,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_activate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianControllerBase::on_activate(const rclcpp_lifecycle::State & previous_state) { if (m_active) { @@ -300,36 +303,28 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes } // Get command handles. - for (const auto& type : m_cmd_interface_types) + for (const auto & type : m_cmd_interface_types) { - if (!controller_interface::get_ordered_interfaces(command_interfaces_, - m_joint_names, - type, + if (!controller_interface::get_ordered_interfaces(command_interfaces_, m_joint_names, type, (type == hardware_interface::HW_IF_POSITION) ? m_joint_cmd_pos_handles : m_joint_cmd_vel_handles)) { - RCLCPP_ERROR(get_node()->get_logger(), - "Expected %zu '%s' command interfaces, got %zu.", - m_joint_names.size(), - type.c_str(), - (type == hardware_interface::HW_IF_POSITION) - ? m_joint_cmd_pos_handles.size() - : m_joint_cmd_vel_handles.size()); + RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", + m_joint_names.size(), type.c_str(), + (type == hardware_interface::HW_IF_POSITION) ? m_joint_cmd_pos_handles.size() + : m_joint_cmd_vel_handles.size()); return CallbackReturn::ERROR; } } // Get state handles. - if (!controller_interface::get_ordered_interfaces(state_interfaces_, - m_joint_names, + if (!controller_interface::get_ordered_interfaces(state_interfaces_, m_joint_names, hardware_interface::HW_IF_POSITION, m_joint_state_pos_handles)) { - RCLCPP_ERROR(get_node()->get_logger(), - "Expected %zu '%s' state interfaces, got %zu.", - m_joint_names.size(), - hardware_interface::HW_IF_POSITION, + RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", + m_joint_names.size(), hardware_interface::HW_IF_POSITION, m_joint_state_pos_handles.size()); return CallbackReturn::ERROR; } @@ -351,7 +346,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -CartesianControllerBase::on_shutdown(const rclcpp_lifecycle::State& previous_state) +CartesianControllerBase::on_shutdown(const rclcpp_lifecycle::State & previous_state) { stopCurrentMotion(); @@ -366,7 +361,6 @@ CartesianControllerBase::on_shutdown(const rclcpp_lifecycle::State& previous_sta return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - void CartesianControllerBase::writeJointControlCmds() { if (get_node()->get_parameter("solver.publish_state_feedback").as_bool()) @@ -374,8 +368,9 @@ void CartesianControllerBase::writeJointControlCmds() publishStateFeedback(); } - auto nan_in = [](const auto& values) -> bool { - for (const auto& value : values) + auto nan_in = [](const auto & values) -> bool + { + for (const auto & value : values) { if (std::isnan(value)) { @@ -387,8 +382,9 @@ void CartesianControllerBase::writeJointControlCmds() if (nan_in(m_simulated_joint_motion.positions) || nan_in(m_simulated_joint_motion.velocities)) { - RCLCPP_ERROR(get_node()->get_logger(), - "NaN detected in internal model. It's unlikely to recover from this. Shutting down."); + RCLCPP_ERROR( + get_node()->get_logger(), + "NaN detected in internal model. It's unlikely to recover from this. Shutting down."); #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON get_node()->shutdown(); @@ -399,7 +395,7 @@ void CartesianControllerBase::writeJointControlCmds() } // Write all available types. - for (const auto& type : m_cmd_interface_types) + for (const auto & type : m_cmd_interface_types) { if (type == hardware_interface::HW_IF_POSITION) { @@ -418,21 +414,21 @@ void CartesianControllerBase::writeJointControlCmds() } } -void CartesianControllerBase::computeJointControlCmds(const ctrl::Vector6D& error, const rclcpp::Duration& period) +void CartesianControllerBase::computeJointControlCmds(const ctrl::Vector6D & error, + const rclcpp::Duration & period) { // PD controlled system input m_error_scale = get_node()->get_parameter("solver.error_scale").as_double(); - m_cartesian_input = m_error_scale * m_spatial_controller(error,period); + m_cartesian_input = m_error_scale * m_spatial_controller(error, period); // Simulate one step forward - m_simulated_joint_motion = m_ik_solver->getJointControlCmds( - period, - m_cartesian_input); + m_simulated_joint_motion = m_ik_solver->getJointControlCmds(period, m_cartesian_input); m_ik_solver->updateKinematics(); } -ctrl::Vector6D CartesianControllerBase::displayInBaseLink(const ctrl::Vector6D& vector, const std::string& from) +ctrl::Vector6D CartesianControllerBase::displayInBaseLink(const ctrl::Vector6D & vector, + const std::string & from) { // Adjust format KDL::Wrench wrench_kdl; @@ -442,10 +438,7 @@ ctrl::Vector6D CartesianControllerBase::displayInBaseLink(const ctrl::Vector6D& } KDL::Frame transform_kdl; - m_forward_kinematics_solver->JntToCart( - m_ik_solver->getPositions(), - transform_kdl, - from); + m_forward_kinematics_solver->JntToCart(m_ik_solver->getPositions(), transform_kdl, from); // Rotate into new reference frame wrench_kdl = transform_kdl.M * wrench_kdl; @@ -460,38 +453,29 @@ ctrl::Vector6D CartesianControllerBase::displayInBaseLink(const ctrl::Vector6D& return out; } -ctrl::Matrix6D CartesianControllerBase::displayInBaseLink(const ctrl::Matrix6D& tensor, const std::string& from) +ctrl::Matrix6D CartesianControllerBase::displayInBaseLink(const ctrl::Matrix6D & tensor, + const std::string & from) { // Get rotation to base KDL::Frame R_kdl; - m_forward_kinematics_solver->JntToCart( - m_ik_solver->getPositions(), - R_kdl, - from); + m_forward_kinematics_solver->JntToCart(m_ik_solver->getPositions(), R_kdl, from); // Adjust format ctrl::Matrix3D R; - R << - R_kdl.M.data[0], - R_kdl.M.data[1], - R_kdl.M.data[2], - R_kdl.M.data[3], - R_kdl.M.data[4], - R_kdl.M.data[5], - R_kdl.M.data[6], - R_kdl.M.data[7], - R_kdl.M.data[8]; + R << R_kdl.M.data[0], R_kdl.M.data[1], R_kdl.M.data[2], R_kdl.M.data[3], R_kdl.M.data[4], + R_kdl.M.data[5], R_kdl.M.data[6], R_kdl.M.data[7], R_kdl.M.data[8]; // Treat diagonal blocks as individual 2nd rank tensors. // Display in base frame. ctrl::Matrix6D tmp = ctrl::Matrix6D::Zero(); - tmp.topLeftCorner<3,3>() = R * tensor.topLeftCorner<3,3>() * R.transpose(); - tmp.bottomRightCorner<3,3>() = R * tensor.bottomRightCorner<3,3>() * R.transpose(); + tmp.topLeftCorner<3, 3>() = R * tensor.topLeftCorner<3, 3>() * R.transpose(); + tmp.bottomRightCorner<3, 3>() = R * tensor.bottomRightCorner<3, 3>() * R.transpose(); return tmp; } -ctrl::Vector6D CartesianControllerBase::displayInTipLink(const ctrl::Vector6D& vector, const std::string& to) +ctrl::Vector6D CartesianControllerBase::displayInTipLink(const ctrl::Vector6D & vector, + const std::string & to) { // Adjust format KDL::Wrench wrench_kdl; @@ -501,10 +485,7 @@ ctrl::Vector6D CartesianControllerBase::displayInTipLink(const ctrl::Vector6D& v } KDL::Frame transform_kdl; - m_forward_kinematics_solver->JntToCart( - m_ik_solver->getPositions(), - transform_kdl, - to); + m_forward_kinematics_solver->JntToCart(m_ik_solver->getPositions(), transform_kdl, to); // Rotate into new reference frame wrench_kdl = transform_kdl.M.Inverse() * wrench_kdl; @@ -523,26 +504,26 @@ void CartesianControllerBase::publishStateFeedback() { // End-effector pose auto pose = m_ik_solver->getEndEffectorPose(); - if (m_feedback_pose_publisher->trylock()){ + if (m_feedback_pose_publisher->trylock()) + { m_feedback_pose_publisher->msg_.header.stamp = get_node()->now(); m_feedback_pose_publisher->msg_.header.frame_id = m_robot_base_link; m_feedback_pose_publisher->msg_.pose.position.x = pose.p.x(); m_feedback_pose_publisher->msg_.pose.position.y = pose.p.y(); m_feedback_pose_publisher->msg_.pose.position.z = pose.p.z(); - pose.M.GetQuaternion( - m_feedback_pose_publisher->msg_.pose.orientation.x, - m_feedback_pose_publisher->msg_.pose.orientation.y, - m_feedback_pose_publisher->msg_.pose.orientation.z, - m_feedback_pose_publisher->msg_.pose.orientation.w - ); + pose.M.GetQuaternion(m_feedback_pose_publisher->msg_.pose.orientation.x, + m_feedback_pose_publisher->msg_.pose.orientation.y, + m_feedback_pose_publisher->msg_.pose.orientation.z, + m_feedback_pose_publisher->msg_.pose.orientation.w); m_feedback_pose_publisher->unlockAndPublish(); } // End-effector twist auto twist = m_ik_solver->getEndEffectorVel(); - if (m_feedback_twist_publisher->trylock()){ + if (m_feedback_twist_publisher->trylock()) + { m_feedback_twist_publisher->msg_.header.stamp = get_node()->now(); m_feedback_twist_publisher->msg_.header.frame_id = m_robot_base_link; m_feedback_twist_publisher->msg_.twist.linear.x = twist[0]; @@ -554,8 +535,6 @@ void CartesianControllerBase::publishStateFeedback() m_feedback_twist_publisher->unlockAndPublish(); } - } - -} // namespace +} // namespace cartesian_controller_base diff --git a/cartesian_controller_examples/package.xml b/cartesian_controller_examples/package.xml index de2c4ff4..52a9cc5f 100644 --- a/cartesian_controller_examples/package.xml +++ b/cartesian_controller_examples/package.xml @@ -17,13 +17,13 @@ - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers + https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - Stefan Scherzinger + Stefan Scherzinger diff --git a/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h b/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h index 6f27116f..18dbe85f 100644 --- a/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h +++ b/cartesian_controller_handles/include/cartesian_controller_handles/motion_control_handle.h @@ -40,12 +40,6 @@ #ifndef MOTION_CONTROL_HANDLE_H_INCLUDED #define MOTION_CONTROL_HANDLE_H_INCLUDED -#include "cartesian_controller_base/ROS2VersionConfig.h" -#include "geometry_msgs/msg/detail/pose_stamped__struct.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "rclcpp/publisher.hpp" -#include "visualization_msgs/msg/interactive_marker.hpp" -#include "visualization_msgs/msg/interactive_marker_feedback.hpp" #include #include #include @@ -55,9 +49,15 @@ #include #include +#include "cartesian_controller_base/ROS2VersionConfig.h" +#include "geometry_msgs/msg/detail/pose_stamped__struct.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/publisher.hpp" +#include "visualization_msgs/msg/interactive_marker.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" + namespace cartesian_controller_handles { - /** * @brief Implements a drag-and-drop control handle in RViz * @@ -71,53 +71,57 @@ namespace cartesian_controller_handles */ class MotionControlHandle : public controller_interface::ControllerInterface { - public: - MotionControlHandle(); - ~MotionControlHandle(); +public: + MotionControlHandle(); + ~MotionControlHandle(); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - virtual LifecycleNodeInterface::CallbackReturn on_init() override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + virtual LifecycleNodeInterface::CallbackReturn on_init() override; #elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; + virtual controller_interface::return_type init(const std::string & controller_name) override; #endif - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + controller_interface::return_type update(const rclcpp::Time & time, + const rclcpp::Duration & period) override; #elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; + controller_interface::return_type update() override; #endif - - private: - /** +private: + /** * @brief Move visual marker in RViz according to user interaction * * This function also stores the marker pose internally. * * @param feedback The message containing the current pose of the marker */ - void updateMotionControlCallback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + void updateMotionControlCallback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); - /** + /** * @brief React to changes in the interactive marker menu * * @param feedback The message containing the current menu configuration */ - void updateMarkerMenuCallback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + void updateMarkerMenuCallback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); - /** + /** * @brief Add all relevant marker controls for interaction in RViz * * You must call \a applyChanges() on the marker server for the controls to @@ -125,9 +129,9 @@ class MotionControlHandle : public controller_interface::ControllerInterface * * @param marker The marker to add the controls to */ - static void prepareMarkerControls(visualization_msgs::msg::InteractiveMarker& marker); + static void prepareMarkerControls(visualization_msgs::msg::InteractiveMarker & marker); - /** + /** * @brief Adds interactive controls (arrows) to a marker. * * Both move and rotate controls are added along the specified @@ -138,47 +142,46 @@ class MotionControlHandle : public controller_interface::ControllerInterface * @param y Y-axis component * @param z Z-axis component */ - static void addAxisControl(visualization_msgs::msg::InteractiveMarker& marker, double x, double y, double z); + static void addAxisControl(visualization_msgs::msg::InteractiveMarker & marker, double x, + double y, double z); - /** + /** * @brief Add a sphere visualization to the interactive marker * * @param marker The marker to add the visualization to * @param scale The scale of the visualization. Bounding box in meter. */ - static void addMarkerVisualization(visualization_msgs::msg::InteractiveMarker& marker, double scale); + static void addMarkerVisualization(visualization_msgs::msg::InteractiveMarker & marker, + double scale); - /** + /** * @brief Get the current pose of the specified end-effector * * @return The current end-effector pose with respect to the specified base link */ - geometry_msgs::msg::PoseStamped getEndEffectorPose(); - - // Handles to the joints - std::vector> m_joint_handles; + geometry_msgs::msg::PoseStamped getEndEffectorPose(); - std::vector m_joint_names; + // Handles to the joints + std::vector> m_joint_handles; - // Kinematics - std::string m_robot_base_link; - std::string m_end_effector_link; - std::string m_target_frame_topic; - KDL::Chain m_robot_chain; - std::shared_ptr< - KDL::ChainFkSolverPos_recursive> m_fk_solver; + std::vector m_joint_names; - geometry_msgs::msg::PoseStamped m_current_pose; - rclcpp::Publisher::SharedPtr m_pose_publisher; + // Kinematics + std::string m_robot_base_link; + std::string m_end_effector_link; + std::string m_target_frame_topic; + KDL::Chain m_robot_chain; + std::shared_ptr m_fk_solver; - // Interactive marker - std::shared_ptr< - interactive_markers::InteractiveMarkerServer> m_server; + geometry_msgs::msg::PoseStamped m_current_pose; + rclcpp::Publisher::SharedPtr m_pose_publisher; - visualization_msgs::msg::InteractiveMarker m_marker; //!< Controller handle for RViz + // Interactive marker + std::shared_ptr m_server; + visualization_msgs::msg::InteractiveMarker m_marker; //!< Controller handle for RViz }; -} // cartesian_controller_handles +} // namespace cartesian_controller_handles #endif diff --git a/cartesian_controller_handles/package.xml b/cartesian_controller_handles/package.xml index 57ea0b3e..1d4632dc 100644 --- a/cartesian_controller_handles/package.xml +++ b/cartesian_controller_handles/package.xml @@ -6,8 +6,8 @@ The cartesian_controller_handles package scherzin BSD-3-Clause - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - Stefan Scherzinger + https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers + Stefan Scherzinger ament_cmake diff --git a/cartesian_controller_handles/src/motion_control_handle.cpp b/cartesian_controller_handles/src/motion_control_handle.cpp index b8a2c28a..18cc210f 100644 --- a/cartesian_controller_handles/src/motion_control_handle.cpp +++ b/cartesian_controller_handles/src/motion_control_handle.cpp @@ -37,35 +37,33 @@ */ //----------------------------------------------------------------------------- +#include +#include + +#include +#include + #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/node.hpp" #include "rclcpp/time.hpp" #include "visualization_msgs/msg/detail/interactive_marker_feedback__struct.hpp" -#include -#include -#include -#include - - -namespace cartesian_controller_handles { +namespace cartesian_controller_handles +{ MotionControlHandle::MotionControlHandle() {} MotionControlHandle::~MotionControlHandle() {} rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -MotionControlHandle::on_activate(const rclcpp_lifecycle::State& previous_state) +MotionControlHandle::on_activate(const rclcpp_lifecycle::State & previous_state) { // Get state handles. if (!controller_interface::get_ordered_interfaces( state_interfaces_, m_joint_names, hardware_interface::HW_IF_POSITION, m_joint_handles)) { - RCLCPP_ERROR(get_node()->get_logger(), - "Expected %zu '%s' state interfaces, got %zu.", - m_joint_names.size(), - hardware_interface::HW_IF_POSITION, - m_joint_handles.size()); + RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", + m_joint_names.size(), hardware_interface::HW_IF_POSITION, m_joint_handles.size()); return CallbackReturn::ERROR; } @@ -76,22 +74,23 @@ MotionControlHandle::on_activate(const rclcpp_lifecycle::State& previous_state) } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -MotionControlHandle::on_deactivate(const rclcpp_lifecycle::State& previous_state) +MotionControlHandle::on_deactivate(const rclcpp_lifecycle::State & previous_state) { m_joint_handles.clear(); this->release_interfaces(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -controller_interface::return_type MotionControlHandle::update(const rclcpp::Time& time, - const rclcpp::Duration& period) +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +controller_interface::return_type MotionControlHandle::update(const rclcpp::Time & time, + const rclcpp::Duration & period) #elif defined CARTESIAN_CONTROLLERS_FOXY controller_interface::return_type MotionControlHandle::update() #endif { // Publish marker pose - m_current_pose.header.stamp = get_node()->now(); + m_current_pose.header.stamp = get_node()->now(); m_current_pose.header.frame_id = m_robot_base_link; m_pose_publisher->publish(m_current_pose); m_server->applyChanges(); @@ -99,34 +98,34 @@ controller_interface::return_type MotionControlHandle::update() return controller_interface::return_type::OK; } -controller_interface::InterfaceConfiguration -MotionControlHandle::command_interface_configuration() const +controller_interface::InterfaceConfiguration MotionControlHandle::command_interface_configuration() + const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::NONE; return conf; } -controller_interface::InterfaceConfiguration -MotionControlHandle::state_interface_configuration() const +controller_interface::InterfaceConfiguration MotionControlHandle::state_interface_configuration() + const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(m_joint_names.size()); // Only position - for (const auto& joint_name : m_joint_names) + conf.names.reserve(m_joint_names.size()); // Only position + for (const auto & joint_name : m_joint_names) { conf.names.push_back(joint_name + "/position"); } return conf; } - -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MotionControlHandle::on_init() { #elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type MotionControlHandle::init(const std::string& controller_name) +controller_interface::return_type MotionControlHandle::init(const std::string & controller_name) { // Initialize lifecycle node const auto ret = ControllerInterface::init(controller_name); @@ -141,7 +140,8 @@ controller_interface::return_type MotionControlHandle::init(const std::string& c auto_declare("end_effector_link", ""); auto_declare >("joints", std::vector()); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; #elif defined CARTESIAN_CONTROLLERS_FOXY return controller_interface::return_type::OK; @@ -149,7 +149,7 @@ controller_interface::return_type MotionControlHandle::init(const std::string& c } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -MotionControlHandle::on_configure(const rclcpp_lifecycle::State& previous_state) +MotionControlHandle::on_configure(const rclcpp_lifecycle::State & previous_state) { // Get kinematics specific configuration urdf::Model robot_model; @@ -187,9 +187,10 @@ MotionControlHandle::on_configure(const rclcpp_lifecycle::State& previous_state) } if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, m_robot_chain)) { - const std::string error = "" - "Failed to parse robot chain from urdf model. " - "Do robot_base_link and end_effector_link exist?"; + const std::string error = + "" + "Failed to parse robot chain from urdf model. " + "Do robot_base_link and end_effector_link exist?"; RCLCPP_ERROR(get_node()->get_logger(), "%s", error.c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -214,11 +215,11 @@ MotionControlHandle::on_configure(const rclcpp_lifecycle::State& previous_state) m_server.reset( new interactive_markers::InteractiveMarkerServer(get_node()->get_name(), get_node())); m_marker.header.frame_id = m_robot_base_link; - m_marker.header.stamp = get_node()->now(); - m_marker.scale = 0.1; - m_marker.name = "motion_control_handle"; - m_marker.pose = m_current_pose.pose; - m_marker.description = "6D control of link: " + m_end_effector_link; + m_marker.header.stamp = get_node()->now(); + m_marker.scale = 0.1; + m_marker.name = "motion_control_handle"; + m_marker.pose = m_current_pose.pose; + m_marker.description = "6D control of link: " + m_end_effector_link; prepareMarkerControls(m_marker); @@ -244,24 +245,23 @@ MotionControlHandle::on_configure(const rclcpp_lifecycle::State& previous_state) } void MotionControlHandle::updateMotionControlCallback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback) + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) { // Move marker in RViz m_server->setPose(feedback->marker_name, feedback->pose); m_server->applyChanges(); // Store for later broadcasting - m_current_pose.pose = feedback->pose; + m_current_pose.pose = feedback->pose; m_current_pose.header.stamp = get_node()->now(); } - void MotionControlHandle::updateMarkerMenuCallback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback) + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) { } -void MotionControlHandle::prepareMarkerControls(visualization_msgs::msg::InteractiveMarker& marker) +void MotionControlHandle::prepareMarkerControls(visualization_msgs::msg::InteractiveMarker & marker) { // Add colored sphere as visualization constexpr double marker_scale = 0.05; @@ -273,13 +273,13 @@ void MotionControlHandle::prepareMarkerControls(visualization_msgs::msg::Interac addAxisControl(marker, 0, 0, 1); } -void MotionControlHandle::addMarkerVisualization(visualization_msgs::msg::InteractiveMarker& marker, - double scale) +void MotionControlHandle::addMarkerVisualization( + visualization_msgs::msg::InteractiveMarker & marker, double scale) { // Create a sphere as a handle visualization_msgs::msg::Marker visual; - visual.type = visualization_msgs::msg::Marker::SPHERE; - visual.scale.x = scale; // bounding box in meter + visual.type = visualization_msgs::msg::Marker::SPHERE; + visual.scale.x = scale; // bounding box in meter visual.scale.y = scale; visual.scale.z = scale; visual.color.r = 1.0; @@ -294,10 +294,8 @@ void MotionControlHandle::addMarkerVisualization(visualization_msgs::msg::Intera marker.controls.push_back(visual_control); } -void MotionControlHandle::addAxisControl(visualization_msgs::msg::InteractiveMarker& marker, - double x, - double y, - double z) +void MotionControlHandle::addAxisControl(visualization_msgs::msg::InteractiveMarker & marker, + double x, double y, double z) { if (x == 0 && y == 0 && z == 0) { @@ -306,7 +304,7 @@ void MotionControlHandle::addAxisControl(visualization_msgs::msg::InteractiveMar visualization_msgs::msg::InteractiveMarkerControl control; - double norm = std::sqrt(1 + x * x + y * y + z * z); + double norm = std::sqrt(1 + x * x + y * y + z * z); control.orientation.w = 1 / norm; control.orientation.x = x / norm; control.orientation.y = y / norm; @@ -334,16 +332,13 @@ geometry_msgs::msg::PoseStamped MotionControlHandle::getEndEffectorPose() current.pose.position.x = tmp.p.x(); current.pose.position.y = tmp.p.y(); current.pose.position.z = tmp.p.z(); - tmp.M.GetQuaternion(current.pose.orientation.x, - current.pose.orientation.y, - current.pose.orientation.z, - current.pose.orientation.w); + tmp.M.GetQuaternion(current.pose.orientation.x, current.pose.orientation.y, + current.pose.orientation.z, current.pose.orientation.w); return current; } -} // namespace cartesian_controller_handles - +} // namespace cartesian_controller_handles // Pluginlib #include diff --git a/cartesian_controller_simulation/README.md b/cartesian_controller_simulation/README.md index dba7b1e1..6425add1 100644 --- a/cartesian_controller_simulation/README.md +++ b/cartesian_controller_simulation/README.md @@ -48,4 +48,3 @@ Exposed interfaces per joint: - `command_interfaces`: position, velocity, stiffness, damping - `state_interfaces`: position, velocity - diff --git a/cartesian_controller_simulation/config/controller_manager.yaml b/cartesian_controller_simulation/config/controller_manager.yaml index aff9752b..24165ae7 100644 --- a/cartesian_controller_simulation/config/controller_manager.yaml +++ b/cartesian_controller_simulation/config/controller_manager.yaml @@ -50,7 +50,7 @@ cartesian_compliance_controller: # This is the link that the robot feels compliant about. It does not need # to coincide with the end_effector_link, but for many use cases, this - # configuration is handy. When working with a screwdriver, for instance, + # configuration is handy. When working with a screwdriver, for instance, # setting compliance_ref_link == end_effector_link makes it easy to specify # downward pushing forces without generating unwanted offset moments. # On the other hand, an application could benefit from yielding a little in @@ -263,4 +263,3 @@ invalid_cartesian_force_controller: rot_x: {p: 0.0} rot_y: {p: 0.0} rot_z: {p: 0.0} - diff --git a/cartesian_controller_simulation/etc/robot_mujoco.xml b/cartesian_controller_simulation/etc/robot_mujoco.xml index d9b1ef42..b35acad5 100644 --- a/cartesian_controller_simulation/etc/robot_mujoco.xml +++ b/cartesian_controller_simulation/etc/robot_mujoco.xml @@ -7,7 +7,7 @@ visuals in the URDF to collisions before loading this, because MuJoCo discards visuals by default. The additional elements, such as actuators, assets, etc. were added by hand later, since they can't be specified in URDF. - --> + --> diff --git a/cartesian_controller_simulation/include/cartesian_controller_simulation/mujoco_simulator.h b/cartesian_controller_simulation/include/cartesian_controller_simulation/mujoco_simulator.h index c9ed90ce..610920b0 100644 --- a/cartesian_controller_simulation/include/cartesian_controller_simulation/mujoco_simulator.h +++ b/cartesian_controller_simulation/include/cartesian_controller_simulation/mujoco_simulator.h @@ -41,14 +41,14 @@ #include #include +#include #include #include -#include #include "mujoco.h" -namespace cartesian_controller_simulation { - +namespace cartesian_controller_simulation +{ /** * @brief MuJoCo's physics engine with rendering and basic window mouse interaction * @@ -70,21 +70,21 @@ class MuJoCoSimulator public: // Modern singleton approach - MuJoCoSimulator(const MuJoCoSimulator&) = delete; - MuJoCoSimulator& operator=(const MuJoCoSimulator &) = delete; + MuJoCoSimulator(const MuJoCoSimulator &) = delete; + MuJoCoSimulator & operator=(const MuJoCoSimulator &) = delete; MuJoCoSimulator(MuJoCoSimulator &&) = delete; MuJoCoSimulator & operator=(MuJoCoSimulator &&) = delete; // Use this in ROS2 code - static MuJoCoSimulator& getInstance() + static MuJoCoSimulator & getInstance() { static MuJoCoSimulator simulator; return simulator; } // MuJoCo data structures - mjModel* m = NULL; // MuJoCo model - mjData* d = NULL; // MuJoCo data + mjModel * m = NULL; // MuJoCo model + mjData * d = NULL; // MuJoCo data // Buffers for data exchange with ROS2-control std::vector pos_cmd; @@ -92,28 +92,25 @@ class MuJoCoSimulator std::vector pos_state; std::vector vel_state; std::vector eff_state; - std::vector stiff; // Proportional gain - std::vector damp; // Derivative gain + std::vector stiff; // Proportional gain + std::vector damp; // Derivative gain // Safety guards for buffers std::mutex state_mutex; std::mutex command_mutex; // Control input callback for the solver - static void controlCB(const mjModel* m, mjData* d); - void controlCBImpl(const mjModel* m, mjData* d); + static void controlCB(const mjModel * m, mjData * d); + void controlCBImpl(const mjModel * m, mjData * d); // Call this in a separate thread - static int simulate(const std::string& model_xml); - int simulateImpl(const std::string& model_xml); + static int simulate(const std::string & model_xml); + int simulateImpl(const std::string & model_xml); // Non-blocking - void read(std::vector& pos, std::vector& vel, std::vector& eff); - void write(const std::vector& pos, - const std::vector& vel, - const std::vector& stiff, - const std::vector& damp); - + void read(std::vector & pos, std::vector & vel, std::vector & eff); + void write(const std::vector & pos, const std::vector & vel, + const std::vector & stiff, const std::vector & damp); }; -} // namespace cartesian_controller_simulation +} // namespace cartesian_controller_simulation diff --git a/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h b/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h index b4972f05..b97c8d38 100644 --- a/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h +++ b/cartesian_controller_simulation/include/cartesian_controller_simulation/system_interface.h @@ -41,27 +41,27 @@ #include -#include "hardware_interface/system.hpp" -#include "hardware_interface/system_interface.hpp" +#include +#include + +#include "cartesian_controller_simulation/mujoco_simulator.h" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "cartesian_controller_simulation/mujoco_simulator.h" -#include -#include #if defined CARTESIAN_CONTROLLERS_FOXY #include "hardware_interface/base_interface.hpp" #endif - -namespace cartesian_controller_simulation { - +namespace cartesian_controller_simulation +{ // Two custom hardware interfaces for torque-actuated robots: // proportional gain (stiffness) and derivative gain (damping). constexpr char HW_IF_STIFFNESS[] = "stiffness"; -constexpr char HW_IF_DAMPING[] = "damping"; +constexpr char HW_IF_DAMPING[] = "damping"; /** * @brief A MuJoCo-based, standalone simulator for cartesian_controllersand ROS2-control @@ -71,7 +71,8 @@ constexpr char HW_IF_DAMPING[] = "damping"; * controller_manager coordinated library. * */ -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON class Simulator : public hardware_interface::SystemInterface #elif defined CARTESIAN_CONTROLLERS_FOXY class Simulator : public hardware_interface::BaseInterface @@ -83,23 +84,24 @@ class Simulator : public hardware_interface::BaseInterface export_state_interfaces() override; std::vector export_command_interfaces() override; - return_type prepare_command_mode_switch(const std::vector& start_interfaces, - const std::vector& stop_interfaces) override; - + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; - return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; #elif defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_FOXY return_type read() override; @@ -111,7 +113,6 @@ class Simulator : public hardware_interface::BaseInterface m_position_commands; @@ -133,4 +134,4 @@ class Simulator : public hardware_interface::BaseInterface", self.slider_changed) @@ -93,12 +103,13 @@ def slider_changed(self, event): rclpy.spin_once(self, timeout_sec=0) def publish(self, opening): - """ Publish a new joint trajectory with an interpolated state + """Publish a new joint trajectory with an interpolated state We scale linearly with opening=[0,1] between `fully_streched` and `home`. """ - jpos = opening * np.array(self.home) + \ - (self.percent - opening) * np.array(self.fully_streched) + jpos = opening * np.array(self.home) + (self.percent - opening) * np.array( + self.fully_streched + ) jpos = jpos / self.percent msg = JointTrajectory() msg.joint_names = self.joint_names @@ -117,5 +128,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/cartesian_controller_simulation/src/mujoco_simulator.cpp b/cartesian_controller_simulation/src/mujoco_simulator.cpp index 8a4b9ef5..89208aa7 100644 --- a/cartesian_controller_simulation/src/mujoco_simulator.cpp +++ b/cartesian_controller_simulation/src/mujoco_simulator.cpp @@ -38,36 +38,37 @@ //----------------------------------------------------------------------------- #include "cartesian_controller_simulation/mujoco_simulator.h" -#include -namespace cartesian_controller_simulation { +#include -MuJoCoSimulator::MuJoCoSimulator(){} +namespace cartesian_controller_simulation +{ +MuJoCoSimulator::MuJoCoSimulator() {} -void MuJoCoSimulator::controlCB(const mjModel* m, mjData* d) +void MuJoCoSimulator::controlCB(const mjModel * m, mjData * d) { getInstance().controlCBImpl(m, d); } -void MuJoCoSimulator::controlCBImpl([[maybe_unused]] const mjModel* m, mjData* d) +void MuJoCoSimulator::controlCBImpl([[maybe_unused]] const mjModel * m, mjData * d) { command_mutex.lock(); for (size_t i = 0; i < pos_cmd.size(); ++i) { // Joint-level impedance control - d->ctrl[i] = stiff[i] * (pos_cmd[i] - d->qpos[i]) + // stiffness - damp[i] * (vel_cmd[i] - d->actuator_velocity[i]); // damping + d->ctrl[i] = stiff[i] * (pos_cmd[i] - d->qpos[i]) + // stiffness + damp[i] * (vel_cmd[i] - d->actuator_velocity[i]); // damping } command_mutex.unlock(); } -int MuJoCoSimulator::simulate(const std::string& model_xml) +int MuJoCoSimulator::simulate(const std::string & model_xml) { return getInstance().simulateImpl(model_xml); } -int MuJoCoSimulator::simulateImpl(const std::string& model_xml) +int MuJoCoSimulator::simulateImpl(const std::string & model_xml) { // Make sure that the ROS2-control system_interface only gets valid data in read(). // We lock until we are done with simulation setup. @@ -75,7 +76,7 @@ int MuJoCoSimulator::simulateImpl(const std::string& model_xml) // load and compile model char error[1000] = "Could not load binary model"; - m = mj_loadXML(model_xml.c_str(), nullptr, error, 1000); + m = mj_loadXML(model_xml.c_str(), nullptr, error, 1000); if (!m) { mju_error_s("Load model error: %s", error); @@ -116,7 +117,8 @@ int MuJoCoSimulator::simulateImpl(const std::string& model_xml) return 0; } -void MuJoCoSimulator::read(std::vector& pos, std::vector& vel, std::vector& eff) +void MuJoCoSimulator::read(std::vector & pos, std::vector & vel, + std::vector & eff) { // Realtime in ROS2-control is more important than fresh data exchange. if (state_mutex.try_lock()) @@ -128,10 +130,8 @@ void MuJoCoSimulator::read(std::vector& pos, std::vector& vel, s } } -void MuJoCoSimulator::write(const std::vector& pos, - const std::vector& vel, - const std::vector& stiff, - const std::vector& damp) +void MuJoCoSimulator::write(const std::vector & pos, const std::vector & vel, + const std::vector & stiff, const std::vector & damp) { // Realtime in ROS2-control is more important than fresh data exchange. if (command_mutex.try_lock()) @@ -154,4 +154,4 @@ void MuJoCoSimulator::syncStates() } } -} // namespace cartesian_controller_simulation +} // namespace cartesian_controller_simulation diff --git a/cartesian_controller_simulation/src/system_interface.cpp b/cartesian_controller_simulation/src/system_interface.cpp index ca92d7bd..2d3ae9bd 100644 --- a/cartesian_controller_simulation/src/system_interface.cpp +++ b/cartesian_controller_simulation/src/system_interface.cpp @@ -37,7 +37,6 @@ */ //----------------------------------------------------------------------------- - #include "cartesian_controller_simulation/system_interface.h" #include @@ -45,20 +44,21 @@ #include #include #include -#include #include +#include +#include "cartesian_controller_simulation/mujoco_simulator.h" #include "hardware_interface/handle.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#include "cartesian_controller_simulation/mujoco_simulator.h" -namespace cartesian_controller_simulation { - -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareInfo& info) +namespace cartesian_controller_simulation +{ +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareInfo & info) { // Keep an internal copy of the given configuration if (hardware_interface::SystemInterface::on_init(info) != Simulator::CallbackReturn::SUCCESS) @@ -66,7 +66,7 @@ Simulator::CallbackReturn Simulator::on_init(const hardware_interface::HardwareI return Simulator::CallbackReturn::ERROR; } #elif defined CARTESIAN_CONTROLLERS_FOXY -Simulator::return_type Simulator::configure(const hardware_interface::HardwareInfo& info) +Simulator::return_type Simulator::configure(const hardware_interface::HardwareInfo & info) { // Keep an internal copy of the given configuration if (configure_default(info) != return_type::OK) @@ -95,17 +95,17 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn for (size_t i = 0; i < info_.joints.size(); ++i) { m_stiffness[i] = std::stod(info_.joints[i].parameters.at("p")); - m_damping[i] = std::stod(info_.joints[i].parameters.at("d")); + m_damping[i] = std::stod(info_.joints[i].parameters.at("d")); } - for (const hardware_interface::ComponentInfo& joint : info_.joints) + for (const hardware_interface::ComponentInfo & joint : info_.joints) { if (joint.command_interfaces.size() != 2) { RCLCPP_ERROR(rclcpp::get_logger("Simulator"), - "Joint '%s' needs two possible command interfaces.", - joint.name.c_str()); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + "Joint '%s' needs two possible command interfaces.", joint.name.c_str()); +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON return Simulator::CallbackReturn::ERROR; #elif defined CARTESIAN_CONTROLLERS_FOXY return Simulator::return_type::ERROR; @@ -117,10 +117,10 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn { RCLCPP_ERROR(rclcpp::get_logger("Simulator"), "Joint '%s' needs the following command interfaces in that order: %s, %s.", - joint.name.c_str(), - hardware_interface::HW_IF_POSITION, + joint.name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON return Simulator::CallbackReturn::ERROR; #elif defined CARTESIAN_CONTROLLERS_FOXY return Simulator::return_type::ERROR; @@ -129,10 +129,10 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn if (joint.state_interfaces.size() != 3) { - RCLCPP_ERROR(rclcpp::get_logger("Simulator"), - "Joint '%s' needs 3 state interfaces.", + RCLCPP_ERROR(rclcpp::get_logger("Simulator"), "Joint '%s' needs 3 state interfaces.", joint.name.c_str()); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON return Simulator::CallbackReturn::ERROR; #elif defined CARTESIAN_CONTROLLERS_FOXY return Simulator::return_type::ERROR; @@ -145,11 +145,10 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn { RCLCPP_ERROR(rclcpp::get_logger("Simulator"), "Joint '%s' needs the following state interfaces in that order: %s, %s, and %s.", - joint.name.c_str(), - hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_EFFORT); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON + joint.name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT); +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON return Simulator::CallbackReturn::ERROR; #elif defined CARTESIAN_CONTROLLERS_FOXY return Simulator::return_type::ERROR; @@ -157,7 +156,8 @@ Simulator::return_type Simulator::configure(const hardware_interface::HardwareIn } } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON return Simulator::CallbackReturn::SUCCESS; #elif defined CARTESIAN_CONTROLLERS_FOXY return Simulator::return_type::OK; @@ -198,9 +198,9 @@ std::vector Simulator::export_command_inte return command_interfaces; } -Simulator::return_type -Simulator::prepare_command_mode_switch([[maybe_unused]] const std::vector& start_interfaces, - [[maybe_unused]] const std::vector& stop_interfaces) +Simulator::return_type Simulator::prepare_command_mode_switch( + [[maybe_unused]] const std::vector & start_interfaces, + [[maybe_unused]] const std::vector & stop_interfaces) { // TODO: Exclusive OR for position and velocity commands @@ -221,10 +221,9 @@ Simulator::return_type Simulator::stop() } #endif - #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -Simulator::return_type Simulator::read([[maybe_unused]] const rclcpp::Time& time, - [[maybe_unused]] const rclcpp::Duration& period) +Simulator::return_type Simulator::read([[maybe_unused]] const rclcpp::Time & time, + [[maybe_unused]] const rclcpp::Duration & period) #elif defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_FOXY Simulator::return_type Simulator::read() #endif @@ -233,9 +232,8 @@ Simulator::return_type Simulator::read() // Start with the current positions as safe default, but let active // controllers overrride them in each cycle. - if (std::any_of(m_position_commands.begin(), m_position_commands.end(), [](double i) { - return std::isnan(i); - })) + if (std::any_of(m_position_commands.begin(), m_position_commands.end(), + [](double i) { return std::isnan(i); })) { m_position_commands = m_positions; } @@ -250,19 +248,20 @@ Simulator::return_type Simulator::read() } #if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -Simulator::return_type Simulator::write([[maybe_unused]] const rclcpp::Time& time, - [[maybe_unused]] const rclcpp::Duration& period) +Simulator::return_type Simulator::write([[maybe_unused]] const rclcpp::Time & time, + [[maybe_unused]] const rclcpp::Duration & period) #elif defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_FOXY Simulator::return_type Simulator::write() #endif { - MuJoCoSimulator::getInstance().write( - m_position_commands, m_velocity_commands, m_stiffness, m_damping); + MuJoCoSimulator::getInstance().write(m_position_commands, m_velocity_commands, m_stiffness, + m_damping); return return_type::OK; } -} // namespace cartesian_controller_simulation +} // namespace cartesian_controller_simulation #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(cartesian_controller_simulation::Simulator, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(cartesian_controller_simulation::Simulator, + hardware_interface::SystemInterface) diff --git a/cartesian_controller_tests/integration_tests/integration_tests.py b/cartesian_controller_tests/integration_tests/integration_tests.py index 829682a1..8b9dbcae 100755 --- a/cartesian_controller_tests/integration_tests/integration_tests.py +++ b/cartesian_controller_tests/integration_tests/integration_tests.py @@ -1,8 +1,6 @@ #!/usr/bin/env python3 import unittest -import launch -import launch.actions import launch_testing.actions from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, TimerAction @@ -18,28 +16,38 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import WrenchStamped -import os +distro = os.environ["ROS_DISTRO"] -distro = os.environ['ROS_DISTRO'] def generate_test_description(): - setup = IncludeLaunchDescription( PathJoinSubstitution( - [FindPackageShare("cartesian_controller_simulation"), "launch", "simulation.launch.py"] + [ + FindPackageShare("cartesian_controller_simulation"), + "launch", + "simulation.launch.py", + ] ) ) - until_ready = 10.0 # sec - return LaunchDescription([setup, TimerAction(period=until_ready, actions=[launch_testing.actions.ReadyToTest()])]) + until_ready = 10.0 # sec + return LaunchDescription( + [ + setup, + TimerAction( + period=until_ready, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ) class IntegrationTest(unittest.TestCase): - """ Integration tests for the cartesian controllers in a simulation setting + """Integration tests for the cartesian controllers in a simulation setting We check if each controller successfully performs the life cycle of `initialized` - `active` - `update` - `inactive`, and whether it behaves correctly in selected use cases. """ + def __init__(self, *args): super().__init__(*args) @@ -50,64 +58,78 @@ def setUpClass(cls): cls.setup_interfaces(cls) cls.our_controllers = [ - 'cartesian_motion_controller', - 'cartesian_force_controller', - 'cartesian_compliance_controller', - 'motion_control_handle', + "cartesian_motion_controller", + "cartesian_force_controller", + "cartesian_compliance_controller", + "motion_control_handle", ] cls.invalid_controllers = [ - 'invalid_cartesian_force_controller', - 'invalid_cartesian_compliance_controller', + "invalid_cartesian_force_controller", + "invalid_cartesian_compliance_controller", ] - @classmethod def tearDownClass(cls): cls.node.destroy_node() rclpy.shutdown() def setup_interfaces(self): - """ Setup interfaces for ROS2 services and topics """ + """Setup interfaces for ROS2 services and topics""" timeout = rclpy.time.Duration(seconds=5) - self.list_controllers = self.node.create_client(ListControllers, "/controller_manager/list_controllers") - if not self.list_controllers.wait_for_service(timeout.nanoseconds/1e9): + self.list_controllers = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + if not self.list_controllers.wait_for_service(timeout.nanoseconds / 1e9): self.fail("Service list_controllers not available") - self.switch_controller = self.node.create_client(SwitchController, '/controller_manager/switch_controller') - if not self.switch_controller.wait_for_service(timeout.nanoseconds/1e9): + self.switch_controller = self.node.create_client( + SwitchController, "/controller_manager/switch_controller" + ) + if not self.switch_controller.wait_for_service(timeout.nanoseconds / 1e9): self.fail("Service switch_controllers not available") - self.target_pose_pub = self.node.create_publisher(PoseStamped, "target_frame", 3) - self.target_wrench_pub = self.node.create_publisher(WrenchStamped, "target_wrench", 3) - self.ft_sensor_wrench_pub = self.node.create_publisher(WrenchStamped, "ft_sensor_wrench", 3) + self.target_pose_pub = self.node.create_publisher( + PoseStamped, "target_frame", 3 + ) + self.target_wrench_pub = self.node.create_publisher( + WrenchStamped, "target_wrench", 3 + ) + self.ft_sensor_wrench_pub = self.node.create_publisher( + WrenchStamped, "ft_sensor_wrench", 3 + ) def test_controller_initialization(self): - """ Test whether every controller got initialized correctly + """Test whether every controller got initialized correctly We check if the list of all controllers currently managed by the controller manager contains our controllers and if they have `state: inactive`. """ for name in self.our_controllers: - self.assertTrue(self.check_state(name, 'inactive'), f"{name} is initialized correctly") + self.assertTrue( + self.check_state(name, "inactive"), f"{name} is initialized correctly" + ) def test_invalid_controller_initialization(self): - """ Test whether the invalid controllers' initialization fails as expected + """Test whether the invalid controllers' initialization fails as expected We check if the list of all controllers currently managed by the controller manager contains our controllers and if they have the expected state. """ - if os.environ['ROS_DISTRO'] == 'humble' or os.environ['ROS_DISTRO'] == 'iron': - expected_state = 'unconfigured' + if os.environ["ROS_DISTRO"] == "humble" or os.environ["ROS_DISTRO"] == "iron": + expected_state = "unconfigured" else: # galactic, foxy - expected_state = 'finalized' + expected_state = "finalized" for name in self.invalid_controllers: - self.assertTrue(self.check_state(name, expected_state), f"{name} initializes although it should not.") + self.assertTrue( + self.check_state(name, expected_state), + f"{name} initializes although it should not.", + ) def test_controller_switches(self): - """ Test whether every controller starts, runs, and stops correctly + """Test whether every controller starts, runs, and stops correctly We start each of our controllers individually and check if its state is `active`. We then switch it off and check whether its state is @@ -115,60 +137,68 @@ def test_controller_switches(self): """ for name in self.our_controllers: self.start_controller(name) - self.assertTrue(self.check_state(name, 'active'), "{} is starting correctly".format(name)) - time.sleep(3) # process some update() cycles + self.assertTrue( + self.check_state(name, "active"), + "{} is starting correctly".format(name), + ) + time.sleep(3) # process some update() cycles self.stop_controller(name) - self.assertTrue(self.check_state(name, 'inactive'), "{} is stopping correctly".format(name)) + self.assertTrue( + self.check_state(name, "inactive"), + "{} is stopping correctly".format(name), + ) def test_inputs_with_nans(self): - """ Test whether every controller survives inputs with NaNs + """Test whether every controller survives inputs with NaNs We publish invalid values to all input topics. - The controllers' callbacks store them even when in inactive state (=default) after startup. + The controllers' callbacks store them even when in inactive state after startup. We then check if every controller can be activated normally. """ # Target pose target_pose = PoseStamped() target_pose.header.frame_id = "base_link" - target_pose.pose.position.x = float('nan') - target_pose.pose.position.y = float('nan') - target_pose.pose.position.z = float('nan') - target_pose.pose.orientation.x = float('nan') - target_pose.pose.orientation.y = float('nan') - target_pose.pose.orientation.z = float('nan') - target_pose.pose.orientation.w = float('nan') + target_pose.pose.position.x = float("nan") + target_pose.pose.position.y = float("nan") + target_pose.pose.position.z = float("nan") + target_pose.pose.orientation.x = float("nan") + target_pose.pose.orientation.y = float("nan") + target_pose.pose.orientation.z = float("nan") + target_pose.pose.orientation.w = float("nan") self.target_pose_pub.publish(target_pose) # Force-torque sensor ft_sensor_wrench = WrenchStamped() - ft_sensor_wrench.wrench.force.x = float('nan') - ft_sensor_wrench.wrench.force.y = float('nan') - ft_sensor_wrench.wrench.force.z = float('nan') - ft_sensor_wrench.wrench.torque.x = float('nan') - ft_sensor_wrench.wrench.torque.y = float('nan') - ft_sensor_wrench.wrench.torque.z = float('nan') + ft_sensor_wrench.wrench.force.x = float("nan") + ft_sensor_wrench.wrench.force.y = float("nan") + ft_sensor_wrench.wrench.force.z = float("nan") + ft_sensor_wrench.wrench.torque.x = float("nan") + ft_sensor_wrench.wrench.torque.y = float("nan") + ft_sensor_wrench.wrench.torque.z = float("nan") self.ft_sensor_wrench_pub.publish(ft_sensor_wrench) # Target wrench target_wrench = WrenchStamped() - target_wrench.wrench.force.x = float('nan') - target_wrench.wrench.force.y = float('nan') - target_wrench.wrench.force.z = float('nan') - target_wrench.wrench.torque.x = float('nan') - target_wrench.wrench.torque.y = float('nan') - target_wrench.wrench.torque.z = float('nan') + target_wrench.wrench.force.x = float("nan") + target_wrench.wrench.force.y = float("nan") + target_wrench.wrench.force.z = float("nan") + target_wrench.wrench.torque.x = float("nan") + target_wrench.wrench.torque.y = float("nan") + target_wrench.wrench.torque.z = float("nan") self.target_wrench_pub.publish(target_wrench) - time.sleep(1) # give the controllers some time to process + time.sleep(1) # give the controllers some time to process for name in self.our_controllers: self.start_controller(name) - time.sleep(3) # process some update() cycles - self.assertTrue(self.check_state(name, 'active'), f"{name} survives inputs with NaNs") + time.sleep(3) # process some update() cycles + self.assertTrue( + self.check_state(name, "active"), f"{name} survives inputs with NaNs" + ) self.stop_controller(name) def check_state(self, controller, state): - """ Check the controller's state + """Check the controller's state Return True if the controller's state is `state`, else False. Return False if the controller is not listed. @@ -182,25 +212,25 @@ def check_state(self, controller, state): return False def start_controller(self, controller): - """ Start the given controller """ + """Start the given controller""" req = SwitchController.Request() - if distro in ['humble', 'iron']: + if distro in ["humble", "iron"]: req.activate_controllers = [controller] else: req.start_controllers = [controller] self.perform_switch(req) def stop_controller(self, controller): - """ Stop the given controller """ + """Stop the given controller""" req = SwitchController.Request() - if distro in ['humble', 'iron']: + if distro in ["humble", "iron"]: req.deactivate_controllers = [controller] else: req.stop_controllers = [controller] self.perform_switch(req) def perform_switch(self, req): - """ Trigger the controller switch with the given request """ + """Trigger the controller switch with the given request""" req.strictness = req.BEST_EFFORT future = self.switch_controller.call_async(req) rclpy.spin_until_future_complete(self.node, future) diff --git a/cartesian_controller_tests/package.xml b/cartesian_controller_tests/package.xml index e81ecca4..7b818790 100644 --- a/cartesian_controller_tests/package.xml +++ b/cartesian_controller_tests/package.xml @@ -6,17 +6,17 @@ Integration tests for the Cartesian controllers Stefan Scherzinger BSD-3-Clause - Stefan Scherzinger + Stefan Scherzinger - launch - launch_ros - cartesian_controller_base - cartesian_motion_controller - cartesian_force_controller - cartesian_compliance_controller - cartesian_controller_handles - cartesian_controller_simulation - controller_manager_msgs + launch + launch_ros + cartesian_controller_base + cartesian_motion_controller + cartesian_force_controller + cartesian_compliance_controller + cartesian_controller_handles + cartesian_controller_simulation + controller_manager_msgs launch_testing_ament_cmake diff --git a/cartesian_controller_utilities/etc/button_cmds.yaml b/cartesian_controller_utilities/etc/button_cmds.yaml index 8e645f6b..51d966a1 100644 --- a/cartesian_controller_utilities/etc/button_cmds.yaml +++ b/cartesian_controller_utilities/etc/button_cmds.yaml @@ -6,11 +6,11 @@ spacenav_buttons: # Can be used to force alternation between to buttons, e.g. when doing `start recording` # - `stop recording` tasks. repeat_same_button : False - + # Minimum duration in seconds to pass between two buttons pressed. # Can be used to control the rate of pressing. button_sleep: 1.0 - + # Define which commands to execute on pressing buttons button_cmds : [ @@ -19,7 +19,7 @@ spacenav_buttons: # button 1 "echo 'stop recording' && screen -X stuff '^C'" ] - + # Directories where the commands are executed cmd_dirs : [ diff --git a/cartesian_controller_utilities/launch/spacenav.launch.py b/cartesian_controller_utilities/launch/spacenav.launch.py index a522c068..56426a16 100644 --- a/cartesian_controller_utilities/launch/spacenav.launch.py +++ b/cartesian_controller_utilities/launch/spacenav.launch.py @@ -40,6 +40,7 @@ from launch import LaunchDescription from launch_ros.actions import Node + def generate_launch_description(): spacenav_node = Node( package="spacenav", @@ -47,8 +48,8 @@ def generate_launch_description(): parameters=[ {"zero_when_static": True}, {"static_count_threshold": 30}, - {"linear_scale": [50,50,50]}, - {"angular_scale": [5,5,5]}, + {"linear_scale": [50, 50, 50]}, + {"angular_scale": [5, 5, 5]}, ], output="both", ) diff --git a/cartesian_controller_utilities/launch/spacenav_buttons.launch.py b/cartesian_controller_utilities/launch/spacenav_buttons.launch.py index f0a1f51c..8e9d9e99 100644 --- a/cartesian_controller_utilities/launch/spacenav_buttons.launch.py +++ b/cartesian_controller_utilities/launch/spacenav_buttons.launch.py @@ -41,6 +41,7 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): button_cmds = PathJoinSubstitution( [FindPackageShare("cartesian_controller_utilities"), "etc", "button_cmds.yaml"] diff --git a/cartesian_controller_utilities/launch/spacenav_to_pose.launch.py b/cartesian_controller_utilities/launch/spacenav_to_pose.launch.py index 949de592..0af7ab8d 100644 --- a/cartesian_controller_utilities/launch/spacenav_to_pose.launch.py +++ b/cartesian_controller_utilities/launch/spacenav_to_pose.launch.py @@ -39,6 +39,7 @@ from launch import LaunchDescription from launch_ros.actions import Node + def generate_launch_description(): spacenav_node = Node( package="spacenav", @@ -69,5 +70,4 @@ def generate_launch_description(): output="both", ) - return LaunchDescription([spacenav_node, converter_node]) diff --git a/cartesian_controller_utilities/launch/spacenav_to_wrench.launch.py b/cartesian_controller_utilities/launch/spacenav_to_wrench.launch.py index fa7576f1..6692d532 100644 --- a/cartesian_controller_utilities/launch/spacenav_to_wrench.launch.py +++ b/cartesian_controller_utilities/launch/spacenav_to_wrench.launch.py @@ -40,6 +40,7 @@ from launch import LaunchDescription from launch_ros.actions import Node + def generate_launch_description(): spacenav_node = Node( package="spacenav", @@ -69,5 +70,4 @@ def generate_launch_description(): output="both", ) - return LaunchDescription([spacenav_node, converter_node]) diff --git a/cartesian_controller_utilities/package.xml b/cartesian_controller_utilities/package.xml index 1e9c3cac..c27838b5 100644 --- a/cartesian_controller_utilities/package.xml +++ b/cartesian_controller_utilities/package.xml @@ -6,7 +6,7 @@ Make the spacemouse a teach device for contact-based teleoperation Stefan Scherzinger BSD-3-Clause - Stefan Scherzinger + Stefan Scherzinger ament_cmake diff --git a/cartesian_controller_utilities/scripts/buttons.py b/cartesian_controller_utilities/scripts/buttons.py index b965ccc4..8f84876d 100755 --- a/cartesian_controller_utilities/scripts/buttons.py +++ b/cartesian_controller_utilities/scripts/buttons.py @@ -45,35 +45,43 @@ import sys import time + class buttons(Node): - """ React to button events """ + """React to button events""" def __init__(self): - super().__init__('spacenav_buttons') + super().__init__("spacenav_buttons") - self.repeat_same_button = self.declare_parameter('repeat_same_button', False).value - self.button_sleep = self.declare_parameter('button_sleep', 0.1).value - self.button_cmds = self.declare_parameter('button_cmds', ['']).value - self.cmd_dirs = self.declare_parameter('cmd_dirs', ['']).value + self.repeat_same_button = self.declare_parameter( + "repeat_same_button", False + ).value + self.button_sleep = self.declare_parameter("button_sleep", 0.1).value + self.button_cmds = self.declare_parameter("button_cmds", [""]).value + self.cmd_dirs = self.declare_parameter("cmd_dirs", [""]).value self.last_button_cmds = None - self.joystick_topic = self.declare_parameter('joystick_topic', '').value - self.sub = self.create_subscription(Joy, self.joystick_topic, self.event_callback, 1) + self.joystick_topic = self.declare_parameter("joystick_topic", "").value + self.sub = self.create_subscription( + Joy, self.joystick_topic, self.event_callback, 1 + ) - def event_callback(self,data): + def event_callback(self, data): # Have some buttons been repeatedly pressed? - if self.last_button_cmds and any(np.bitwise_and(data.buttons,self.last_button_cmds)): + if self.last_button_cmds and any( + np.bitwise_and(data.buttons, self.last_button_cmds) + ): return for idx, val in enumerate(data.buttons): if val == 1: exec_dir = self.cmd_dirs[idx] - if not exec_dir: # Empty string + if not exec_dir: # Empty string exec_dir = None subprocess.Popen( self.button_cmds[idx], stdin=subprocess.PIPE, cwd=exec_dir, - shell=True) + shell=True, + ) # Prevent pressing the same buttons in a row if not self.repeat_same_button: self.last_button_cmds = data.buttons @@ -87,7 +95,8 @@ def main(args=None): rclpy.spin(node) rclpy.shutdown() -if __name__ == '__main__': + +if __name__ == "__main__": try: main() except KeyboardInterrupt: diff --git a/cartesian_controller_utilities/scripts/converter.py b/cartesian_controller_utilities/scripts/converter.py index 1621891a..b5b5ffba 100755 --- a/cartesian_controller_utilities/scripts/converter.py +++ b/cartesian_controller_utilities/scripts/converter.py @@ -45,15 +45,15 @@ class converter(Node): - """ Convert Twist messages to WrenchStamped """ + """Convert Twist messages to WrenchStamped""" def __init__(self): - super().__init__('converter') + super().__init__("converter") - self.twist_topic = self.declare_parameter('twist_topic', 'my_twist').value - self.wrench_topic = self.declare_parameter('wrench_topic', 'my_wrench').value - self.frame_id = self.declare_parameter('frame_id', 'world').value - period = 1.0 / self.declare_parameter('publishing_rate', 100).value + self.twist_topic = self.declare_parameter("twist_topic", "my_twist").value + self.wrench_topic = self.declare_parameter("wrench_topic", "my_wrench").value + self.frame_id = self.declare_parameter("frame_id", "world").value + period = 1.0 / self.declare_parameter("publishing_rate", 100).value self.timer = self.create_timer(period, self.publish) self.buffer = WrenchStamped() @@ -61,32 +61,32 @@ def __init__(self): self.pub = self.create_publisher(WrenchStamped, self.wrench_topic, 3) self.sub = self.create_subscription(Twist, self.twist_topic, self.twist_cb, 1) - - def twist_cb(self,data): - self.buffer.header.stamp = self.get_clock().now().to_msg() - self.buffer.header.frame_id = self.frame_id - self.buffer.wrench.force.x = data.linear.x - self.buffer.wrench.force.y = data.linear.y - self.buffer.wrench.force.z = data.linear.z - self.buffer.wrench.torque.x = data.angular.x - self.buffer.wrench.torque.y = data.angular.y - self.buffer.wrench.torque.z = data.angular.z - + def twist_cb(self, data): + self.buffer.header.stamp = self.get_clock().now().to_msg() + self.buffer.header.frame_id = self.frame_id + self.buffer.wrench.force.x = data.linear.x + self.buffer.wrench.force.y = data.linear.y + self.buffer.wrench.force.z = data.linear.z + self.buffer.wrench.torque.x = data.angular.x + self.buffer.wrench.torque.y = data.angular.y + self.buffer.wrench.torque.z = data.angular.z def publish(self): try: self.pub.publish(self.buffer) - except: + except Exception: # Swallow 'publish() to closed topic' error. # This rarely happens on killing this node. pass + def main(args=None): rclpy.init(args=args) node = converter() rclpy.spin(node) -if __name__ == '__main__': + +if __name__ == "__main__": try: main() except KeyboardInterrupt: diff --git a/cartesian_controller_utilities/scripts/pose.py b/cartesian_controller_utilities/scripts/pose.py index 5e2dd28b..dfb41cfd 100755 --- a/cartesian_controller_utilities/scripts/pose.py +++ b/cartesian_controller_utilities/scripts/pose.py @@ -51,7 +51,7 @@ class converter(Node): - """ Convert Twist messages to PoseStamped + """Convert Twist messages to PoseStamped Use this node to integrate twist messages into a moving target pose in Cartesian space. An initial TF lookup assures that the target pose always @@ -59,12 +59,12 @@ class converter(Node): """ def __init__(self): - super().__init__('converter') + super().__init__("converter") - self.twist_topic = self.declare_parameter('twist_topic', 'my_twist').value - self.pose_topic = self.declare_parameter('pose_topic', 'my_pose').value - self.frame_id = self.declare_parameter('frame_id', 'base_link').value - self.end_effector = self.declare_parameter('end_effector', 'tool0').value + self.twist_topic = self.declare_parameter("twist_topic", "my_twist").value + self.pose_topic = self.declare_parameter("pose_topic", "my_pose").value + self.frame_id = self.declare_parameter("frame_id", "base_link").value + self.end_effector = self.declare_parameter("end_effector", "tool0").value self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) @@ -76,23 +76,29 @@ def __init__(self): self.last = time.time() self.startup_done = False - period = 1.0 / self.declare_parameter('publishing_rate', 100).value + period = 1.0 / self.declare_parameter("publishing_rate", 100).value self.timer = self.create_timer(period, self.publish) self.thread = threading.Thread(target=self.startup, daemon=True) self.thread.start() def startup(self): - """ Make sure to start at the robot's current pose """ + """Make sure to start at the robot's current pose""" # Wait until we entered spinning in the main thread. time.sleep(1) try: start = self.tf_buffer.lookup_transform( - target_frame=self.frame_id, source_frame=self.end_effector, - time=rclpy.time.Time()) - - except (tf2_ros.InvalidArgumentException, tf2_ros.LookupException, - tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + target_frame=self.frame_id, + source_frame=self.end_effector, + time=rclpy.time.Time(), + ) + + except ( + tf2_ros.InvalidArgumentException, + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, + ) as e: print(f"Startup failed: {e}") os._exit(1) @@ -105,10 +111,8 @@ def startup(self): self.rot.w = start.transform.rotation.w self.startup_done = True - - def twist_cb(self, data): - """ Numerically integrate twist message into a pose + """Numerically integrate twist message into a pose Use global self.frame_id as reference for the navigation commands. """ @@ -126,7 +130,9 @@ def twist_cb(self, data): wy = data.angular.y wz = data.angular.z - _, q = quaternion.integrate_angular_velocity(lambda _: (wx, wy, wz), 0, dt, self.rot) + _, q = quaternion.integrate_angular_velocity( + lambda _: (wx, wy, wz), 0, dt, self.rot + ) self.rot = q[-1] # the last one is after dt passed @@ -146,17 +152,19 @@ def publish(self): msg.pose.orientation.w = self.rot.w self.pub.publish(msg) - except: + except Exception: # Swallow 'publish() to closed topic' error. # This rarely happens on killing this node. pass + def main(args=None): rclpy.init(args=args) node = converter() rclpy.spin(node) -if __name__ == '__main__': + +if __name__ == "__main__": try: main() except KeyboardInterrupt: diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h index a7a589f7..824d0066 100644 --- a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h +++ b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h @@ -40,14 +40,15 @@ #ifndef CARTESIAN_FORCE_CONTROLLER_H_INCLUDED #define CARTESIAN_FORCE_CONTROLLER_H_INCLUDED -#include "geometry_msgs/msg/wrench_stamped.hpp" #include #include + #include +#include "geometry_msgs/msg/wrench_stamped.hpp" + namespace cartesian_force_controller { - /** * @brief A ROS2-control controller for Cartesian force control * @@ -72,65 +73,67 @@ namespace cartesian_force_controller */ class CartesianForceController : public virtual cartesian_controller_base::CartesianControllerBase { - public: - CartesianForceController(); +public: + CartesianForceController(); -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - virtual LifecycleNodeInterface::CallbackReturn on_init() override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + virtual LifecycleNodeInterface::CallbackReturn on_init() override; #elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; + virtual controller_interface::return_type init(const std::string & controller_name) override; #endif - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + controller_interface::return_type update(const rclcpp::Time & time, + const rclcpp::Duration & period) override; #elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; + controller_interface::return_type update() override; #endif - using Base = cartesian_controller_base::CartesianControllerBase; + using Base = cartesian_controller_base::CartesianControllerBase; - protected: - /** +protected: + /** * @brief Compute the net force of target wrench and measured sensor wrench * * @return The remaining error wrench, given in robot base frame */ - ctrl::Vector6D computeForceError(); - std::string m_new_ft_sensor_ref; - void setFtSensorReferenceFrame(const std::string& new_ref); + ctrl::Vector6D computeForceError(); + std::string m_new_ft_sensor_ref; + void setFtSensorReferenceFrame(const std::string & new_ref); - private: - ctrl::Vector6D compensateGravity(); +private: + ctrl::Vector6D compensateGravity(); - void targetWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); - void ftSensorWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); + void targetWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); + void ftSensorWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); - rclcpp::Subscription::SharedPtr m_target_wrench_subscriber; - rclcpp::Subscription::SharedPtr m_ft_sensor_wrench_subscriber; - ctrl::Vector6D m_target_wrench; - ctrl::Vector6D m_ft_sensor_wrench; - std::string m_ft_sensor_ref_link; - KDL::Frame m_ft_sensor_transform; + rclcpp::Subscription::SharedPtr m_target_wrench_subscriber; + rclcpp::Subscription::SharedPtr m_ft_sensor_wrench_subscriber; + ctrl::Vector6D m_target_wrench; + ctrl::Vector6D m_ft_sensor_wrench; + std::string m_ft_sensor_ref_link; + KDL::Frame m_ft_sensor_transform; - /** + /** * Allow users to choose whether to specify their target wrenches in the * end-effector frame (= True) or the base frame (= False). The first one * is easier for explicit task programming, while the second one is more * intuitive for tele-manipulation. */ - bool m_hand_frame_control; - + bool m_hand_frame_control; }; -} +} // namespace cartesian_force_controller #endif diff --git a/cartesian_force_controller/package.xml b/cartesian_force_controller/package.xml index ab4e4939..c7ce634a 100644 --- a/cartesian_force_controller/package.xml +++ b/cartesian_force_controller/package.xml @@ -6,8 +6,8 @@ The cartesian_force_controller package scherzin BSD-3-Clause - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - Stefan Scherzinger + https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers + Stefan Scherzinger ament_cmake diff --git a/cartesian_force_controller/src/cartesian_force_controller.cpp b/cartesian_force_controller/src/cartesian_force_controller.cpp index 954ce82c..04c0db36 100644 --- a/cartesian_force_controller/src/cartesian_force_controller.cpp +++ b/cartesian_force_controller/src/cartesian_force_controller.cpp @@ -37,21 +37,24 @@ */ //----------------------------------------------------------------------------- -#include "cartesian_controller_base/Utility.h" -#include "controller_interface/controller_interface.hpp" #include + #include +#include "cartesian_controller_base/Utility.h" +#include "controller_interface/controller_interface.hpp" + namespace cartesian_force_controller { - CartesianForceController::CartesianForceController() : Base::CartesianControllerBase(), m_hand_frame_control(true) { } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_init() +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianForceController::on_init() { const auto ret = Base::on_init(); if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) @@ -62,10 +65,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes auto_declare("ft_sensor_ref_link", ""); auto_declare("hand_frame_control", true); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + ; } #elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianForceController::init(const std::string & controller_name) +controller_interface::return_type CartesianForceController::init( + const std::string & controller_name) { const auto ret = Base::init(controller_name); if (ret != controller_interface::return_type::OK) @@ -80,8 +85,8 @@ controller_interface::return_type CartesianForceController::init(const std::stri } #endif -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_configure( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianForceController::on_configure(const rclcpp_lifecycle::State & previous_state) { const auto ret = Base::on_configure(previous_state); if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) @@ -91,12 +96,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes // Make sure sensor link is part of the robot chain m_ft_sensor_ref_link = get_node()->get_parameter("ft_sensor_ref_link").as_string(); - if(!Base::robotChainContains(m_ft_sensor_ref_link)) + if (!Base::robotChainContains(m_ft_sensor_ref_link)) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - m_ft_sensor_ref_link << " is not part of the kinematic chain from " - << Base::m_robot_base_link << " to " - << Base::m_end_effector_link); + RCLCPP_ERROR_STREAM(get_node()->get_logger(), m_ft_sensor_ref_link + << " is not part of the kinematic chain from " + << Base::m_robot_base_link << " to " + << Base::m_end_effector_link); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -104,14 +109,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes setFtSensorReferenceFrame(Base::m_end_effector_link); m_target_wrench_subscriber = get_node()->create_subscription( - get_node()->get_name() + std::string("/target_wrench"), - 10, + get_node()->get_name() + std::string("/target_wrench"), 10, std::bind(&CartesianForceController::targetWrenchCallback, this, std::placeholders::_1)); m_ft_sensor_wrench_subscriber = get_node()->create_subscription( - get_node()->get_name() + std::string("/ft_sensor_wrench"), - 10, + get_node()->get_name() + std::string("/ft_sensor_wrench"), 10, std::bind(&CartesianForceController::ftSensorWrenchCallback, this, std::placeholders::_1)); m_target_wrench.setZero(); @@ -120,23 +123,24 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_activate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianForceController::on_activate(const rclcpp_lifecycle::State & previous_state) { Base::on_activate(previous_state); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianForceController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianForceController::on_deactivate(const rclcpp_lifecycle::State & previous_state) { Base::on_deactivate(previous_state); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -controller_interface::return_type CartesianForceController::update(const rclcpp::Time& time, - const rclcpp::Duration& period) +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +controller_interface::return_type CartesianForceController::update(const rclcpp::Time & time, + const rclcpp::Duration & period) #elif defined CARTESIAN_CONTROLLERS_FOXY controller_interface::return_type CartesianForceController::update() #endif @@ -153,7 +157,7 @@ controller_interface::return_type CartesianForceController::update() ctrl::Vector6D error = computeForceError(); // Turn Cartesian error into joint motion - Base::computeJointControlCmds(error,internal_period); + Base::computeJointControlCmds(error, internal_period); // Write final commands to the hardware interface Base::writeJointControlCmds(); @@ -166,24 +170,25 @@ ctrl::Vector6D CartesianForceController::computeForceError() ctrl::Vector6D target_wrench; m_hand_frame_control = get_node()->get_parameter("hand_frame_control").as_bool(); - if (m_hand_frame_control) // Assume end-effector frame by convention + if (m_hand_frame_control) // Assume end-effector frame by convention { - target_wrench = Base::displayInBaseLink(m_target_wrench,Base::m_end_effector_link); + target_wrench = Base::displayInBaseLink(m_target_wrench, Base::m_end_effector_link); } - else // Default to robot base frame + else // Default to robot base frame { target_wrench = m_target_wrench; } // Superimpose target wrench and sensor wrench in base frame -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref) + target_wrench; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + return Base::displayInBaseLink(m_ft_sensor_wrench, m_new_ft_sensor_ref) + target_wrench; #elif defined CARTESIAN_CONTROLLERS_FOXY return m_ft_sensor_wrench + target_wrench; #endif } -void CartesianForceController::setFtSensorReferenceFrame(const std::string& new_ref) +void CartesianForceController::setFtSensorReferenceFrame(const std::string & new_ref) { // Compute static transform from the force torque sensor to the new reference // frame of interest. @@ -194,30 +199,23 @@ void CartesianForceController::setFtSensorReferenceFrame(const std::string& new_ KDL::JntArray jnts(Base::m_ik_solver->getPositions()); KDL::Frame sensor_ref; - Base::m_forward_kinematics_solver->JntToCart( - jnts, - sensor_ref, - m_ft_sensor_ref_link); + Base::m_forward_kinematics_solver->JntToCart(jnts, sensor_ref, m_ft_sensor_ref_link); KDL::Frame new_sensor_ref; - Base::m_forward_kinematics_solver->JntToCart( - jnts, - new_sensor_ref, - m_new_ft_sensor_ref); + Base::m_forward_kinematics_solver->JntToCart(jnts, new_sensor_ref, m_new_ft_sensor_ref); m_ft_sensor_transform = new_sensor_ref.Inverse() * sensor_ref; } -void CartesianForceController::targetWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) +void CartesianForceController::targetWrenchCallback( + const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) { if (std::isnan(wrench->wrench.force.x) || std::isnan(wrench->wrench.force.y) || std::isnan(wrench->wrench.force.z) || std::isnan(wrench->wrench.torque.x) || std::isnan(wrench->wrench.torque.y) || std::isnan(wrench->wrench.torque.z)) { - auto& clock = *get_node()->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), - clock, - 3000, + auto & clock = *get_node()->get_clock(); + RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), clock, 3000, "NaN detected in target wrench. Ignoring input."); return; } @@ -230,22 +228,21 @@ void CartesianForceController::targetWrenchCallback(const geometry_msgs::msg::Wr m_target_wrench[5] = wrench->wrench.torque.z; } -void CartesianForceController::ftSensorWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) +void CartesianForceController::ftSensorWrenchCallback( + const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) { if (std::isnan(wrench->wrench.force.x) || std::isnan(wrench->wrench.force.y) || std::isnan(wrench->wrench.force.z) || std::isnan(wrench->wrench.torque.x) || std::isnan(wrench->wrench.torque.y) || std::isnan(wrench->wrench.torque.z)) { - auto& clock = *get_node()->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), - clock, - 3000, + auto & clock = *get_node()->get_clock(); + RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), clock, 3000, "NaN detected in force-torque sensor wrench. Ignoring input."); return; } - -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON KDL::Wrench tmp; tmp[0] = wrench->wrench.force.x; tmp[1] = wrench->wrench.force.y; @@ -275,9 +272,10 @@ void CartesianForceController::ftSensorWrenchCallback(const geometry_msgs::msg:: #endif } -} +} // namespace cartesian_force_controller // Pluginlib #include -PLUGINLIB_EXPORT_CLASS(cartesian_force_controller::CartesianForceController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(cartesian_force_controller::CartesianForceController, + controller_interface::ControllerInterface) diff --git a/cartesian_motion_controller/README.md b/cartesian_motion_controller/README.md index 557a4564..f8816b02 100644 --- a/cartesian_motion_controller/README.md +++ b/cartesian_motion_controller/README.md @@ -105,4 +105,3 @@ cartesian_motion_controller: # ... ``` - diff --git a/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h b/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h index 1c409e2d..92458b2c 100644 --- a/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h +++ b/cartesian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h @@ -40,14 +40,15 @@ #ifndef CARTESIAN_MOTION_CONTROLLER_H_INCLUDED #define CARTESIAN_MOTION_CONTROLLER_H_INCLUDED -#include "geometry_msgs/msg/pose_stamped.hpp" #include #include + #include +#include "geometry_msgs/msg/pose_stamped.hpp" + namespace cartesian_motion_controller { - /** * @brief A ROS2-control controller for Cartesian motion tracking * @@ -72,36 +73,38 @@ namespace cartesian_motion_controller */ class CartesianMotionController : public virtual cartesian_controller_base::CartesianControllerBase { - public: - CartesianMotionController(); - virtual ~CartesianMotionController() = default; +public: + CartesianMotionController(); + virtual ~CartesianMotionController() = default; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - virtual LifecycleNodeInterface::CallbackReturn on_init() override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + virtual LifecycleNodeInterface::CallbackReturn on_init() override; #elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; + virtual controller_interface::return_type init(const std::string & controller_name) override; #endif - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON + controller_interface::return_type update(const rclcpp::Time & time, + const rclcpp::Duration & period) override; #elif defined CARTESIAN_CONTROLLERS_FOXY - controller_interface::return_type update() override; + controller_interface::return_type update() override; #endif - using Base = cartesian_controller_base::CartesianControllerBase; - + using Base = cartesian_controller_base::CartesianControllerBase; - protected: - /** +protected: + /** * @brief Compute the offset between a target pose and the current end effector pose * * The pose offset is formulated with a translational component and a rotational @@ -111,16 +114,15 @@ class CartesianMotionController : public virtual cartesian_controller_base::Cart * * @return The error as a 6-dim vector (linear, angular) w.r.t to the robot base link */ - ctrl::Vector6D computeMotionError(); - KDL::Frame m_target_frame; - KDL::Frame m_current_frame; + ctrl::Vector6D computeMotionError(); + KDL::Frame m_target_frame; + KDL::Frame m_current_frame; - void targetFrameCallback(const geometry_msgs::msg::PoseStamped::SharedPtr target); + void targetFrameCallback(const geometry_msgs::msg::PoseStamped::SharedPtr target); - rclcpp::Subscription::SharedPtr m_target_frame_subscr; + rclcpp::Subscription::SharedPtr m_target_frame_subscr; }; -} - +} // namespace cartesian_motion_controller #endif diff --git a/cartesian_motion_controller/src/cartesian_motion_controller.cpp b/cartesian_motion_controller/src/cartesian_motion_controller.cpp index 359152e4..00443a11 100644 --- a/cartesian_motion_controller/src/cartesian_motion_controller.cpp +++ b/cartesian_motion_controller/src/cartesian_motion_controller.cpp @@ -37,24 +37,24 @@ */ //----------------------------------------------------------------------------- +#include + +#include +#include + #include "cartesian_controller_base/Utility.h" #include "controller_interface/controller_interface.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" -#include -#include -#include namespace cartesian_motion_controller { +CartesianMotionController::CartesianMotionController() : Base::CartesianControllerBase() {} -CartesianMotionController::CartesianMotionController() -: Base::CartesianControllerBase() -{ -} - -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_init() +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianMotionController::on_init() { const auto ret = Base::on_init(); if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) @@ -65,7 +65,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } #elif defined CARTESIAN_CONTROLLERS_FOXY -controller_interface::return_type CartesianMotionController::init(const std::string & controller_name) +controller_interface::return_type CartesianMotionController::init( + const std::string & controller_name) { const auto ret = Base::init(controller_name); if (ret != controller_interface::return_type::OK) @@ -77,8 +78,8 @@ controller_interface::return_type CartesianMotionController::init(const std::str } #endif -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_configure( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianMotionController::on_configure(const rclcpp_lifecycle::State & previous_state) { const auto ret = Base::on_configure(previous_state); if (ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) @@ -87,15 +88,14 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes } m_target_frame_subscr = get_node()->create_subscription( - get_node()->get_name() + std::string("/target_frame"), - 3, + get_node()->get_name() + std::string("/target_frame"), 3, std::bind(&CartesianMotionController::targetFrameCallback, this, std::placeholders::_1)); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_activate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianMotionController::on_activate(const rclcpp_lifecycle::State & previous_state) { Base::on_activate(previous_state); @@ -107,16 +107,17 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianMotionController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +CartesianMotionController::on_deactivate(const rclcpp_lifecycle::State & previous_state) { Base::on_deactivate(previous_state); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON -controller_interface::return_type CartesianMotionController::update(const rclcpp::Time& time, - const rclcpp::Duration& period) +#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \ + defined CARTESIAN_CONTROLLERS_IRON +controller_interface::return_type CartesianMotionController::update(const rclcpp::Time & time, + const rclcpp::Duration & period) #elif defined CARTESIAN_CONTROLLERS_FOXY controller_interface::return_type CartesianMotionController::update() #endif @@ -138,7 +139,7 @@ controller_interface::return_type CartesianMotionController::update() ctrl::Vector6D error = computeMotionError(); // Turn Cartesian error into joint motion - Base::computeJointControlCmds(error,internal_period); + Base::computeJointControlCmds(error, internal_period); } // Write final commands to the hardware interface @@ -147,8 +148,7 @@ controller_interface::return_type CartesianMotionController::update() return controller_interface::return_type::OK; } -ctrl::Vector6D CartesianMotionController:: -computeMotionError() +ctrl::Vector6D CartesianMotionController::computeMotionError() { // Compute motion error wrt robot_base_link m_current_frame = Base::m_ik_solver->getEndEffectorPose(); @@ -161,7 +161,7 @@ computeMotionError() // Use Rodrigues Vector for a compact representation of orientation errors // Only for angles within [0,Pi) KDL::Vector rot_axis = KDL::Vector::Zero(); - double angle = error_kdl.M.GetRotAngle(rot_axis); // rot_axis is normalized + double angle = error_kdl.M.GetRotAngle(rot_axis); // rot_axis is normalized double distance = error_kdl.p.Normalize(); // Clamp maximal tolerated error. @@ -171,8 +171,8 @@ computeMotionError() // wrench. const double max_angle = 1.0; const double max_distance = 1.0; - angle = std::clamp(angle,-max_angle,max_angle); - distance = std::clamp(distance,-max_distance,max_distance); + angle = std::clamp(angle, -max_angle, max_angle); + distance = std::clamp(distance, -max_distance, max_distance); // Scale errors to allowed magnitudes rot_axis = rot_axis * angle; @@ -190,47 +190,39 @@ computeMotionError() return error; } -void CartesianMotionController::targetFrameCallback(const geometry_msgs::msg::PoseStamped::SharedPtr target) +void CartesianMotionController::targetFrameCallback( + const geometry_msgs::msg::PoseStamped::SharedPtr target) { if (std::isnan(target->pose.position.x) || std::isnan(target->pose.position.y) || std::isnan(target->pose.position.z) || std::isnan(target->pose.orientation.x) || std::isnan(target->pose.orientation.y) || std::isnan(target->pose.orientation.z) || std::isnan(target->pose.orientation.w)) { - auto& clock = *get_node()->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), - clock, - 3000, + auto & clock = *get_node()->get_clock(); + RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), clock, 3000, "NaN detected in target pose. Ignoring input."); return; } if (target->header.frame_id != Base::m_robot_base_link) { - auto& clock = *get_node()->get_clock(); - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), - clock, 3000, - "Got target pose in wrong reference frame. Expected: %s but got %s", - Base::m_robot_base_link.c_str(), - target->header.frame_id.c_str()); + auto & clock = *get_node()->get_clock(); + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), clock, 3000, + "Got target pose in wrong reference frame. Expected: %s but got %s", + Base::m_robot_base_link.c_str(), target->header.frame_id.c_str()); return; } m_target_frame = KDL::Frame( - KDL::Rotation::Quaternion( - target->pose.orientation.x, - target->pose.orientation.y, - target->pose.orientation.z, - target->pose.orientation.w), - KDL::Vector( - target->pose.position.x, - target->pose.position.y, - target->pose.position.z)); + KDL::Rotation::Quaternion(target->pose.orientation.x, target->pose.orientation.y, + target->pose.orientation.z, target->pose.orientation.w), + KDL::Vector(target->pose.position.x, target->pose.position.y, target->pose.position.z)); } -} // namespace +} // namespace cartesian_motion_controller // Pluginlib #include -PLUGINLIB_EXPORT_CLASS(cartesian_motion_controller::CartesianMotionController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(cartesian_motion_controller::CartesianMotionController, + controller_interface::ControllerInterface) diff --git a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h b/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h index 906f5809..7176a544 100644 --- a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h +++ b/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h @@ -45,9 +45,9 @@ #include // ROS control -#include #include #include +#include #include // KDL @@ -58,39 +58,38 @@ namespace joint_to_cartesian_controller { - /** * @brief A controller adapter in form of a ROS-control hardware interface */ class JointControllerAdapter : public hardware_interface::RobotHW { - public: - - JointControllerAdapter(); - ~JointControllerAdapter(); +public: + JointControllerAdapter(); + ~JointControllerAdapter(); - bool init(const std::vector& handles, ros::NodeHandle& nh); + bool init(const std::vector & handles, + ros::NodeHandle & nh); - void write(KDL::JntArray& positions); + void write(KDL::JntArray & positions); - private: - //! Number of actuated joints - size_t m_number_joints; +private: + //! Number of actuated joints + size_t m_number_joints; - //! Actuated joints in order from base to tip - std::vector m_joint_names; + //! Actuated joints in order from base to tip + std::vector m_joint_names; - hardware_interface::JointStateInterface m_state_interface; - hardware_interface::PositionJointInterface m_pos_interface; + hardware_interface::JointStateInterface m_state_interface; + hardware_interface::PositionJointInterface m_pos_interface; - joint_limits_interface::PositionJointSoftLimitsInterface m_limits_interface; + joint_limits_interface::PositionJointSoftLimitsInterface m_limits_interface; - std::vector m_joint_handles; - std::vector m_limits_handles; + std::vector m_joint_handles; + std::vector m_limits_handles; - std::vector m_cmd; + std::vector m_cmd; }; -} // end namespace +} // namespace joint_to_cartesian_controller #endif diff --git a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h b/joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h index 2196b367..48db2285 100644 --- a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h +++ b/joint_to_cartesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h @@ -48,8 +48,8 @@ // ros_controls #include -#include #include +#include // KDL #include @@ -57,7 +57,6 @@ namespace joint_to_cartesian_controller { - /** * @brief A controller to turn joint trajectories into a moving Cartesian target pose * @@ -76,41 +75,37 @@ namespace joint_to_cartesian_controller * target following loses explicit control over the joints and collision checking. */ class JointToCartesianController - : public controller_interface::Controller +: public controller_interface::Controller { - public: - JointToCartesianController(); - - bool init(hardware_interface::JointStateInterface* hw, ros::NodeHandle& nh); +public: + JointToCartesianController(); - void starting(const ros::Time& time); + bool init(hardware_interface::JointStateInterface * hw, ros::NodeHandle & nh); - void stopping(const ros::Time& time); + void starting(const ros::Time & time); - void update(const ros::Time& time, const ros::Duration& period); + void stopping(const ros::Time & time); - private: - std::string m_end_effector_link; - std::string m_robot_base_link; - std::string m_target_frame_topic; - KDL::JntArray m_positions; - KDL::JntArray m_velocities; - std::vector m_joint_names; - ros::Publisher m_pose_publisher; + void update(const ros::Time & time, const ros::Duration & period); - JointControllerAdapter m_controller_adapter; +private: + std::string m_end_effector_link; + std::string m_robot_base_link; + std::string m_target_frame_topic; + KDL::JntArray m_positions; + KDL::JntArray m_velocities; + std::vector m_joint_names; + ros::Publisher m_pose_publisher; - std::vector< - hardware_interface::JointStateHandle> m_joint_handles; + JointControllerAdapter m_controller_adapter; - std::shared_ptr< - KDL::ChainFkSolverPos_recursive> m_fk_solver; + std::vector m_joint_handles; - std::shared_ptr< - controller_manager::ControllerManager> m_controller_manager; + std::shared_ptr m_fk_solver; + std::shared_ptr m_controller_manager; }; -} +} // namespace joint_to_cartesian_controller #endif diff --git a/joint_to_cartesian_controller/package.xml b/joint_to_cartesian_controller/package.xml index db6e578c..070bd5d1 100644 --- a/joint_to_cartesian_controller/package.xml +++ b/joint_to_cartesian_controller/package.xml @@ -17,13 +17,13 @@ - https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers + https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers - Stefan Scherzinger + Stefan Scherzinger diff --git a/joint_to_cartesian_controller/src/JointControllerAdapter.cpp b/joint_to_cartesian_controller/src/JointControllerAdapter.cpp index d99971c5..05ed6b5e 100644 --- a/joint_to_cartesian_controller/src/JointControllerAdapter.cpp +++ b/joint_to_cartesian_controller/src/JointControllerAdapter.cpp @@ -48,12 +48,10 @@ namespace joint_to_cartesian_controller { +JointControllerAdapter::JointControllerAdapter() {} -JointControllerAdapter::JointControllerAdapter() -{ -} - -bool JointControllerAdapter::init(const std::vector& state_handles, ros::NodeHandle& nh) +bool JointControllerAdapter::init( + const std::vector & state_handles, ros::NodeHandle & nh) { for (size_t i = 0; i < state_handles.size(); ++i) { @@ -79,9 +77,7 @@ bool JointControllerAdapter::init(const std::vector(positions.data.size()) != m_cmd.size()) { @@ -136,4 +125,4 @@ void JointControllerAdapter::write(KDL::JntArray& positions) } } -} // end namespace +} // namespace joint_to_cartesian_controller diff --git a/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp b/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp index ec93c263..a7f05d87 100644 --- a/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp +++ b/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp @@ -41,8 +41,8 @@ #include // Project -#include #include +#include // KDL #include @@ -56,7 +56,7 @@ namespace cartesian_controllers { - /** +/** * @brief Connect joint-based controllers and transform their commands to Cartesian target poses * * This controller handles an internal controller manager, which can load @@ -66,58 +66,53 @@ namespace cartesian_controllers * provide an easy interface to the rqt_joint_trajectory_controller plugin and * MoveIt!. */ - typedef joint_to_cartesian_controller::JointToCartesianController JointControllerAdapter; -} - -PLUGINLIB_EXPORT_CLASS(cartesian_controllers::JointControllerAdapter, controller_interface::ControllerBase) - - - - +typedef joint_to_cartesian_controller::JointToCartesianController JointControllerAdapter; +} // namespace cartesian_controllers +PLUGINLIB_EXPORT_CLASS(cartesian_controllers::JointControllerAdapter, + controller_interface::ControllerBase) namespace joint_to_cartesian_controller { +JointToCartesianController::JointToCartesianController() {} -JointToCartesianController::JointToCartesianController() -{ -} - -bool JointToCartesianController::init(hardware_interface::JointStateInterface* hw, ros::NodeHandle& nh) +bool JointToCartesianController::init(hardware_interface::JointStateInterface * hw, + ros::NodeHandle & nh) { std::string robot_description; urdf::Model robot_model; - KDL::Tree robot_tree; - KDL::Chain robot_chain; + KDL::Tree robot_tree; + KDL::Chain robot_chain; // Get controller specific configuration - if (!nh.getParam("/robot_description",robot_description)) + if (!nh.getParam("/robot_description", robot_description)) { ROS_ERROR("Failed to load '/robot_description' from parameter server"); return false; } - if (!nh.getParam("robot_base_link",m_robot_base_link)) + if (!nh.getParam("robot_base_link", m_robot_base_link)) { - ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/robot_base_link" << " from parameter server"); + ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/robot_base_link" + << " from parameter server"); return false; } - if (!nh.getParam("end_effector_link",m_end_effector_link)) + if (!nh.getParam("end_effector_link", m_end_effector_link)) { - ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/end_effector_link" << " from parameter server"); + ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/end_effector_link" + << " from parameter server"); return false; } - if (!nh.getParam("target_frame_topic",m_target_frame_topic)) + if (!nh.getParam("target_frame_topic", m_target_frame_topic)) { m_target_frame_topic = "target_frame"; ROS_WARN_STREAM("Failed to load " - << nh.getNamespace() + "/target_frame_topic" - << " from parameter server. " - << "Will default to: " - << nh.getNamespace() + m_target_frame_topic); + << nh.getNamespace() + "/target_frame_topic" + << " from parameter server. " + << "Will default to: " << nh.getNamespace() + m_target_frame_topic); } // Publishers - m_pose_publisher = nh.advertise(m_target_frame_topic,10); + m_pose_publisher = nh.advertise(m_target_frame_topic, 10); // Build a kinematic chain of the robot if (!robot_model.initString(robot_description)) @@ -125,16 +120,18 @@ bool JointToCartesianController::init(hardware_interface::JointStateInterface* h ROS_ERROR("Failed to parse urdf model from 'robot_description'"); return false; } - if (!kdl_parser::treeFromUrdfModel(robot_model,robot_tree)) + if (!kdl_parser::treeFromUrdfModel(robot_model, robot_tree)) { - const std::string error = "" + const std::string error = + "" "Failed to parse KDL tree from urdf model"; ROS_ERROR_STREAM(error); throw std::runtime_error(error); } - if (!robot_tree.getChain(m_robot_base_link,m_end_effector_link,robot_chain)) + if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, robot_chain)) { - const std::string error = "" + const std::string error = + "" "Failed to parse robot chain from urdf model. " "Are you sure that both your 'robot_base_link' and 'end_effector_link' exist?"; ROS_ERROR_STREAM(error); @@ -142,10 +139,12 @@ bool JointToCartesianController::init(hardware_interface::JointStateInterface* h } // Get names of controllable joints from the parameter server - if (!nh.getParam("joints",m_joint_names)) + if (!nh.getParam("joints", m_joint_names)) { - const std::string error = "" - "Failed to load " + nh.getNamespace() + "/joints" + " from parameter server"; + const std::string error = + "" + "Failed to load " + + nh.getNamespace() + "/joints" + " from parameter server"; ROS_ERROR_STREAM(error); throw std::runtime_error(error); } @@ -161,7 +160,7 @@ bool JointToCartesianController::init(hardware_interface::JointStateInterface* h m_velocities.data = ctrl::VectorND::Zero(m_joint_handles.size()); // Initialize controller adapter and according manager - m_controller_adapter.init(m_joint_handles,nh); + m_controller_adapter.init(m_joint_handles, nh); m_controller_manager.reset(new controller_manager::ControllerManager(&m_controller_adapter, nh)); // Initialize forward kinematics solver @@ -170,7 +169,7 @@ bool JointToCartesianController::init(hardware_interface::JointStateInterface* h return true; } -void JointToCartesianController::starting(const ros::Time& time) +void JointToCartesianController::starting(const ros::Time & time) { // Get current joint positions from hardware for (size_t i = 0; i < m_joint_handles.size(); ++i) @@ -179,25 +178,23 @@ void JointToCartesianController::starting(const ros::Time& time) } } -void JointToCartesianController::stopping(const ros::Time& time) -{ -} +void JointToCartesianController::stopping(const ros::Time & time) {} -void JointToCartesianController::update(const ros::Time& time, const ros::Duration& period) +void JointToCartesianController::update(const ros::Time & time, const ros::Duration & period) { // Note: The connected joint-based controller gets the feedback directly from // the joint state handles of this joint_to_cartesian_controller. So, // there's no need for a read() function. // Update connected joint controller - m_controller_manager->update(time,period); + m_controller_manager->update(time, period); // Get commanded positions m_controller_adapter.write(m_positions); // Solve forward kinematics KDL::Frame frame; - m_fk_solver->JntToCart(m_positions,frame); + m_fk_solver->JntToCart(m_positions, frame); // Publish end-effector pose geometry_msgs::PoseStamped target_pose = geometry_msgs::PoseStamped(); @@ -206,12 +203,9 @@ void JointToCartesianController::update(const ros::Time& time, const ros::Durati target_pose.pose.position.x = frame.p.x(); target_pose.pose.position.y = frame.p.y(); target_pose.pose.position.z = frame.p.z(); - frame.M.GetQuaternion( - target_pose.pose.orientation.x, - target_pose.pose.orientation.y, - target_pose.pose.orientation.z, - target_pose.pose.orientation.w); + frame.M.GetQuaternion(target_pose.pose.orientation.x, target_pose.pose.orientation.y, + target_pose.pose.orientation.z, target_pose.pose.orientation.w); m_pose_publisher.publish(target_pose); } -} +} // namespace joint_to_cartesian_controller diff --git a/resources/doc/Solver_details.md b/resources/doc/Solver_details.md index 344a59a1..dda3d465 100644 --- a/resources/doc/Solver_details.md +++ b/resources/doc/Solver_details.md @@ -91,4 +91,3 @@ Building in Release mode can give you a 10-times speed-up, and makes sure that the implementation of the control loop is not the performance bottle neck. If you use a higher number of iterations, then this in fact becomes a requirement. - From 15b82f9afd405469e1900b67e361513e82520563 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Tue, 9 Jan 2024 18:49:19 +0100 Subject: [PATCH 3/4] Fix ROS2 version detection for IK solvers --- cartesian_controller_base/CMakeLists.txt | 1 + .../include/cartesian_controller_base/IKSolver.h | 1 + 2 files changed, 2 insertions(+) diff --git a/cartesian_controller_base/CMakeLists.txt b/cartesian_controller_base/CMakeLists.txt index 21f49074..bf6b0186 100644 --- a/cartesian_controller_base/CMakeLists.txt +++ b/cartesian_controller_base/CMakeLists.txt @@ -90,6 +90,7 @@ target_include_directories(ik_solvers PUBLIC $ $ + ${CMAKE_BINARY_DIR} # ROS2VersionConfig.h ) # Prevent pluginlib from using boost diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h index 4415114a..79f0bc04 100644 --- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h +++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h @@ -57,6 +57,7 @@ #include #include +#include "ROS2VersionConfig.h" #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" From aa35214c3ab7b911dbd7fc308503a612d11ebcf3 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Tue, 9 Jan 2024 19:36:46 +0100 Subject: [PATCH 4/4] Remove duplicate semicolon --- cartesian_force_controller/src/cartesian_force_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/cartesian_force_controller/src/cartesian_force_controller.cpp b/cartesian_force_controller/src/cartesian_force_controller.cpp index 04c0db36..e6bc90e4 100644 --- a/cartesian_force_controller/src/cartesian_force_controller.cpp +++ b/cartesian_force_controller/src/cartesian_force_controller.cpp @@ -66,7 +66,6 @@ CartesianForceController::on_init() auto_declare("hand_frame_control", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - ; } #elif defined CARTESIAN_CONTROLLERS_FOXY controller_interface::return_type CartesianForceController::init(