diff --git a/.github/workflows/build_code.yml b/.github/workflows/build_code.yml index 76a71c3..ac55877 100644 --- a/.github/workflows/build_code.yml +++ b/.github/workflows/build_code.yml @@ -1,6 +1,6 @@ name: CI -on: [push] +on: [push, pull_request] jobs: build-code: @@ -29,4 +29,10 @@ jobs: catkin init catkin config --extend /opt/ros/$ROS_DISTRO catkin build - echo "Compile complete." \ No newline at end of file + echo "Compile complete." + - name: Run tests + shell: bash + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + cd catkin_ws + catkin test \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 25eb88e..47ce961 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) +cmake_policy(SET CMP0048 NEW) # Version not in project() command project(cartesian_impedance_controller) find_package(Boost 1.49 REQUIRED) @@ -98,3 +99,21 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) + +## Testing +if (CATKIN_ENABLE_TESTING) + # Base library tests + find_package(rostest REQUIRED) + add_rostest_gtest(base_tests test/base.test test/base_tests.cpp) + target_link_libraries(base_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + + # ROS basic tests + add_rostest_gtest(ros_tests test/ros.test test/ros_tests.cpp) + target_link_libraries(ros_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) + + # ROS functionality tests + add_rostest_gtest(ros_func_tests test/ros_func.test test/ros_func_tests.cpp) + target_link_libraries(ros_func_tests ${PROJECT_NAME} ${Eigen3_LIBRARIES} ${catkin_LIBRARIES}) +endif() + + diff --git a/README.md b/README.md index 069f33b..62f2c77 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,8 @@ ## Description This project is an implementation of Cartesian impedance control for robotic manipulators. It is a type of control strategy that sets a dynamic relationship between contact forces and the position of a robot arm, making it suitable for collaborative robots. It is particularily useful when the interesting dimensions in the workspace are in the Cartesian space. -The controller is developed using the seven degree-of-freedom (DoF) robot arm `LBR iiwa` by `KUKA AG` and has also been tested `Franka Emika Robot (Panda)` both in reality and simulation. +The controller is developed using the seven degree-of-freedom (DoF) robot arm `LBR iiwa` by `KUKA AG` and has also been tested with the `Franka Emika Robot (Panda)` both in reality and simulation. +This controller is used and tested with ROS 1 `melodic` and `noetic`. The implementation consists of a 1. base library that has few dependencies and can e.g. be directly integrated into software such as the DART simulator and a @@ -26,21 +27,20 @@ http://www.youtube.com/watch?v=Q4aPm4O_9fY - Separate base library that can be integrated in non-ROS environments - Interface to ROS messages and dynamic_reconfigure for easy runtime configuration - ![](./res/flowchart.png) ## Torques The torque signal commanded to the joints of the robot is composed by the superposition of three joint-torque signals: - The torque calculated for Cartesian impedance control with respect to a Cartesian pose reference in the frame of the EE of the robot (`tau_task`). -- The torque calculated for joint impedance control with respect to a desired configuration and projected in the null-space of the robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector (`tau_ns`). +- The torque calculated for joint impedance control with respect to a desired configuration and projected in the nullspace of the robot's Jacobian, so it should not affect the Cartesian motion of the robot's end-effector (`tau_ns`). - The torque necessary to achieve the desired external force command (`cartesian_wrench`), in the frame of the EE of the robot (`tau_ext`). ## Limitations - Joint friction is not accounted for - Stiffness and damping values along the Cartesian dimensions are uncoupled -- No built-in gravity compensation for tools or workpieces (can be achieved by commanded a wrench) +- No built-in gravity compensation for tools or workpieces (can be achieved by commanding a wrench) ## Prerequisites ### Required @@ -54,22 +54,23 @@ We use `RBDyn` to calculate forward kinematics and the Jacobian. - [mc_rbdyn_urdf](https://github.com/jrl-umi3218/mc_rbdyn_urdf) - [SpaceVecAlg](https://github.com/jrl-umi3218/SpaceVecAlg) -The installation steps are automated in `scripts/install_dependencies.sh`: +The installation steps for the installation of the non-ROS dependencies are automated in `scripts/install_dependencies.sh`. ## Controller Usage in ROS Assuming that there is an [initialized catkin workspace](https://catkin-tools.readthedocs.io/en/latest/quick_start.html#initializing-a-new-workspace) you can clone this repository, install the dependencies and compile the controller. -After cloning this repository in your catkin workspace, execute these commands: +Here are the steps: ```bash cd catkin_ws -src/cartesian_impedance_controller/scripts/install_dependencies.sh +git clone https://github.com/matthias-mayr/Cartesian-Impedance-Controller src/Cartesian-Impedance-Controller +src/Cartesian-Impedance-Controller/scripts/install_dependencies.sh rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y -catkin build # or catkin_make +catkin_make # or 'catkin build' source devel/setup.bash ``` -This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController`. +This allows you to add a controller configuration for the controller type `cartesian_impedance_controller/CartesianImpedanceController` in your `ros_control` configuration. ### Configuration file When using the controller it is a good practice to describe the parameters in a `YAML` file and load it. Usually this is already done by your robot setup - e.g. for [iiwa_ros](https://github.com/epfl-lasa/iiwa_ros/) that is [here](https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_control/config/iiwa_control.yaml). @@ -180,7 +181,7 @@ wrench: #### Cartesian Damping factors -The damping factors can be configured with a `geometry_msgs/WrenchStamped` msg similar to the stiffnesses to the topic `${controller_ns}/set_damping_factors`. Damping factors are in the interval [0.01,1]. +The damping factors can be configured with a `geometry_msgs/WrenchStamped` msg similar to the stiffnesses to the topic `${controller_ns}/set_damping_factors`. Damping factors are in the interval [0.01,2]. These damping factors are additionally applied to the damping rule which is `2*sqrt(stiffness)`. #### Stiffnesses, damping and nullspace at once When also setting the nullspace stiffnes, a custom messages of the type `cartesian_impedance_controller/ControllerConfig` is to be sent to `set_config`: @@ -189,11 +190,11 @@ When also setting the nullspace stiffnes, a custom messages of the type `cartesi rostopic pub --once /${controller_ns}/set_config cartesian_impedance_controller/ControllerConfig "cartesian_stiffness: force: {x: 300.0, y: 300.0, z: 300.0} torque: {x: 30.0, y: 30.0, z: 30.0} -cartesian_damping: - force: {x: 0.0, y: 0.0, z: 0.0} - torque: {x: 0.0, y: 0.0, z: 0.0} +cartesian_damping_factors: + force: {x: 1.0, y: 1.0, z: 1.0} + torque: {x: 1.0, y: 1.0, z: 1.0} nullspace_stiffness: 10.0 -nullspace_damping: 0.1 +nullspace_damping_factor: 0.1 q_d_nullspace: [0, 0, 0, 0, 0, 0, 0]" ``` @@ -269,7 +270,23 @@ https://matthias-mayr.github.io/Cartesian-Impedance-Controller/ ## Repository and Contributions The main public code repository is at: https://github.com/matthias-mayr/Cartesian-Impedance-Controller -Issues and pull requests are welcome. Feel free to contact the main author at `firstname.lastname@cs.lth.se` if you are using this implementation. +Issues, questions and pull requests are welcome. Feel free to contact the main author at `firstname.lastname@cs.lth.se` if you are using this implementation. + +## Citing this Work +A brief paper about the features and the control theory is under submission at [JOSS](https://joss.theoj.org/). For now there is a [preprint on arXiv](https://arxiv.org/abs/2212.11215).
+If you are using it or interacting with it, we would be happy if you could cite it. +```bibtex +@misc{https://doi.org/10.48550/arxiv.2212.11215, + doi = {10.48550/ARXIV.2212.11215}, + url = {https://arxiv.org/abs/2212.11215}, + author = {Mayr, Matthias and Salt-Ducaju, Julian M.}, + keywords = {Robotics (cs.RO), FOS: Computer and information sciences, FOS: Computer and information sciences}, + title = {A C++ Implementation of a Cartesian Impedance Controller for Robotic Manipulators}, + publisher = {arXiv}, + year = {2022}, + copyright = {Creative Commons Attribution Share Alike 4.0 International} +} +``` ## Troubleshooting ### Compilation - A required package was not found @@ -289,3 +306,16 @@ cd catkin_ws src/cartesian_impedance_controller/scripts/install_dependencies.sh rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y ``` + +### RBDyn Library not found + +When starting the controller, this message appears: +``` + [ControllerManager::loadController]: Could not load class 'cartesian_impedance_controller/CartesianImpedanceController': Failed to load library /home/matthias/catkin_ws/devel/lib//libcartesian_impedance_controller_ros.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Pocoexception = libRBDyn.so.1: cannot open shared object file: No such file or directory) /gazebo: [ControllerManager::loadController]: Could not load controller 'CartesianImpedance_trajectory_controller' because controller type 'cartesian_impedance_controller/CartesianImpedanceController' does exist. + ``` + + This happens when a shared library can not be found. They are installed with `scripts/install_dependencies.sh`. The dynamic linker has a cache and we now manually update it by calling `ldconfig` after the installation. + + ### Controller crashes + + Most likely this happens because some parts of the stack like `iiwa_ros` or `RBDyn` were built with `SIMD`/`march=native` being turned on and other parts are not. Everything needs to be built with or without this option in order to have working alignment. This package builds without, because it is otherwise cumbersome for people to ensure that this happens across the whole stack. diff --git a/cfg/stiffness.cfg b/cfg/stiffness.cfg old mode 100755 new mode 100644 diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller.h b/include/cartesian_impedance_controller/cartesian_impedance_controller.h index 383e728..969333b 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller.h +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller.h @@ -73,7 +73,7 @@ namespace cartesian_impedance_controller * \param[in] r_z Rotational damping z * \param[in] n Nullspace damping */ - void setDamping(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, double d_n); + void setDampingFactors(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, double d_n); /*! \brief Sets the desired end-effector pose * @@ -206,12 +206,17 @@ namespace cartesian_impedance_controller double nullspace_damping_{0.0}; //!< Current nullspace damping double nullspace_damping_target_{0.0}; //!< Nullspace damping target + Eigen::Matrix cartesian_stiffness_target_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness target + Eigen::Matrix cartesian_damping_target_{Eigen::Matrix::Identity()}; //!< Cartesian damping target + Eigen::Matrix damping_factors_{Eigen::Matrix::Ones()}; //!< Damping factors + Eigen::VectorXd q_; //!< Joint positions Eigen::VectorXd dq_; //!< Joint velocities Eigen::MatrixXd jacobian_; //!< Jacobian. Row format: 3 translations, 3 rotation // End Effector + Eigen::Matrix error_; //!< Calculate pose error Eigen::Vector3d position_{Eigen::Vector3d::Zero()}; //!< Current end-effector position Eigen::Vector3d position_d_{Eigen::Vector3d::Zero()}; //!< Current end-effector reference position Eigen::Vector3d position_d_target_{Eigen::Vector3d::Zero()}; //!< End-effector target position @@ -220,6 +225,10 @@ namespace cartesian_impedance_controller Eigen::Quaterniond orientation_d_{Eigen::Quaterniond::Identity()}; //!< Current end-effector target orientation Eigen::Quaterniond orientation_d_target_{Eigen::Quaterniond::Identity()}; //!< End-effector orientation target + // External applied forces + Eigen::Matrix cartesian_wrench_{Eigen::Matrix::Zero()}; //!< Current Cartesian wrench + Eigen::Matrix cartesian_wrench_target_{Eigen::Matrix::Zero()}; //!< Cartesian wrench target + Eigen::VectorXd tau_c_; //!< Last commanded torques double update_frequency_{1000}; //!< Update frequency in Hz @@ -273,16 +282,6 @@ namespace cartesian_impedance_controller /*! \brief Adds a percental filtering effect to the applied Cartesian wrench */ void updateFilteredWrench(); - - Eigen::Matrix error_; //!< Calculate pose error - - Eigen::Matrix cartesian_stiffness_target_{Eigen::Matrix::Identity()}; //!< Cartesian stiffness target - Eigen::Matrix cartesian_damping_target_{Eigen::Matrix::Identity()}; //!< Cartesian damping target - Eigen::Matrix damping_factors_{Eigen::Matrix::Ones()}; //!< Damping factors - - // External applied forces - Eigen::Matrix cartesian_wrench_{Eigen::Matrix::Zero()}; //!< Current Cartesian wrench - Eigen::Matrix cartesian_wrench_target_{Eigen::Matrix::Zero()}; //!< Cartesian wrench target }; -} // namespace cartesian_impedance_controller \ No newline at end of file +} // namespace cartesian_impedance_controller diff --git a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h index 932933a..99b119b 100644 --- a/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h +++ b/include/cartesian_impedance_controller/cartesian_impedance_controller_ros.h @@ -151,7 +151,7 @@ namespace cartesian_impedance_controller * \param[in] cart_damping Cartesian damping [0,1] * \param[in] nullspace Nullspace damping [0,1] */ - void setDamping(const geometry_msgs::Wrench &cart_damping, double nullspace); + void setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace); /*! \brief Sets Cartesian and nullspace stiffness * @@ -164,11 +164,11 @@ namespace cartesian_impedance_controller /*! \brief Message callback for Cartesian damping. * - * Calls setDamping function. - * @sa setDamping. + * Calls setDampingFactors function. + * @sa setDampingFactors. * \param[in] msg Received message */ - void cartesianDampingCb(const geometry_msgs::WrenchConstPtr &msg); + void cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg); /*! \brief Message callback for Cartesian stiffness. * @@ -181,7 +181,7 @@ namespace cartesian_impedance_controller /*! \brief Message callback for the whole controller configuration. * * Sets stiffness, damping and nullspace. - * @sa setDamping, setStiffness + * @sa setDampingFactors, setStiffness * \param[in] msg Received message */ void controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg); @@ -289,8 +289,8 @@ namespace cartesian_impedance_controller ros::Subscriber sub_cart_stiffness_; //!< Cartesian stiffness subscriber ros::Subscriber sub_cart_wrench_; //!< Cartesian wrench subscriber - ros::Subscriber sub_damping_; //!< Damping subscriber - ros::Subscriber sub_impedance_config_; //!< Controller configuration subscriber + ros::Subscriber sub_damping_factors_; //!< Damping subscriber + ros::Subscriber sub_controller_config_; //!< Controller configuration subscriber ros::Subscriber sub_reference_pose_; //!< Cartesian reference pose subscriber tf::TransformListener tf_listener_; //!< tf transformation listener @@ -303,8 +303,8 @@ namespace cartesian_impedance_controller const double rot_stf_max_{100}; //!< Maximum rotational stiffness const double ns_min_{0}; //!< Minimum nullspace stiffness const double ns_max_{100}; //!< Maximum nullspace stiffness - const double dmp_min_{0.001}; //!< Minimum damping - const double dmp_max_{1.0}; //!< Maximum damping + const double dmp_factor_min_{0.001}; //!< Minimum damping factor + const double dmp_factor_max_{2.0}; //!< Maximum damping factor // The Jacobian of RBDyn comes with orientation in the first three lines. Needs to be interchanged. const Eigen::VectorXi perm_indices_ = diff --git a/msg/ControllerConfig.msg b/msg/ControllerConfig.msg index 4c7873d..5f81f2b 100644 --- a/msg/ControllerConfig.msg +++ b/msg/ControllerConfig.msg @@ -2,17 +2,18 @@ # Values: Translation: [0.0, 2000.0] in [N/m]; Rotation [0.0; 500.0] in [Nm/rad] geometry_msgs/Wrench cartesian_stiffness -# Damping values for translation and rotation of the axes x,y,z -# Values: [0.0, 1.0] -geometry_msgs/Wrench cartesian_damping +# Damping factor for translation and rotation of the axes x,y,z +# The rule is always 2*sqrt(stiffness) +# Values: [0.001, 2.0] +geometry_msgs/Wrench cartesian_damping_factors # Stiffness value for nullspace # Values: >= 0.0 in [Nm/rad] float64 nullspace_stiffness # Damping parameter for nullspace [Nm*s/rad]. -# Value: [0.3, 1.0]; A good damping value is 0.7. -float64 nullspace_damping +# Value: [0.001, 2.0] +float64 nullspace_damping_factor # The desired nullspace configuration # Values: According to the limits of the robot diff --git a/package.xml b/package.xml index 2b225e2..da42dcc 100644 --- a/package.xml +++ b/package.xml @@ -1,14 +1,15 @@ - + cartesian_impedance_controller - 1.0.0 + 1.1.0 A Cartesian Impedance controller implementation Matthias Mayr - BSD + BSD-3-Clause Matthias Mayr Oussama Chouman + Julian Salt Ducaju catkin @@ -32,27 +33,37 @@ tf_conversions trajectory_msgs - actionlib - actionlib_msgs - control_msgs - controller_interface - controller_manager - dynamic_reconfigure - eigen_conversions - geometry_msgs - hardware_interface - message_runtime - pluginlib - realtime_tools - roscpp - sensor_msgs - std_msgs - tf - tf_conversions - trajectory_msgs + actionlib + actionlib_msgs + control_msgs + controller_interface + controller_manager + dynamic_reconfigure + eigen_conversions + geometry_msgs + hardware_interface + message_runtime + pluginlib + realtime_tools + roscpp + sensor_msgs + std_msgs + tf + tf_conversions + trajectory_msgs + yaml-cpp - + + + gtest + eigen + roscpp + rostest + geometry_msgs + std_msgs + dynamic_reconfigure + yaml-cpp diff --git a/res/Abstract.txt b/res/Abstract.txt new file mode 100644 index 0000000..2557919 --- /dev/null +++ b/res/Abstract.txt @@ -0,0 +1 @@ +Cartesian impedance control is a type of motion control strategy for robots that improves safety in partially unknown environments by achieving a compliant behavior of the robot with respect to its external forces. This compliant robot behavior has the added benefit of allowing physical human guidance of the robot. In this paper, we propose a C++ implementation of compliance control valid for any torque-commanded robotic manipulator. The proposed controller implements Cartesian impedance control to track a desired end-effector pose. Additionally, joint impedance is projected in the nullspace of the Cartesian robot motion to track a desired robot joint configuration without perturbing the Cartesian motion of the robot. The proposed implementation also allows the robot to apply desired forces and torques to its environment. Several safety features such as filtering, rate limiting, and saturation are included in the proposed implementation. The core functionalities are in a re-usable base library and a Robot Operating System (ROS) ros_control integration is provided on top of that. The implementation was tested with the KUKA LBR iiwa robot and the Franka Emika Robot (Panda) both in simulation and with the physical robots. \ No newline at end of file diff --git a/res/config/example_controller.yaml b/res/config/example_controller.yaml new file mode 100644 index 0000000..9e0c8d5 --- /dev/null +++ b/res/config/example_controller.yaml @@ -0,0 +1,46 @@ +# An example configuration that works with the example +# +# It is also used in the integration tests + +CartesianImpedance_trajectory_controller: + type: cartesian_impedance_controller/CartesianImpedanceController + joints: # Joints to control + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 + end_effector: tool0 # Link to control arm in + update_frequency: 500 # Controller update frequency in Hz + dynamic_reconfigure: true # Starts dynamic reconfigure server + handle_trajectories: true # Accept traj., e.g. from MoveIt + robot_description: /robot_description # In case of a varying name + wrench_ee_frame: tool0 # Default frame for wrench commands + delta_tau_max: 1.0 # Max. commanded torque diff between steps in Nm + filtering: # Update existing values (0.0 1.0] per s + nullspace_config: 0.1 # Nullspace configuration filtering + pose: 0.1 # Reference pose filtering + stiffness: 0.1 # Cartesian and nullspace stiffness + wrench: 0.1 # Commanded torque + verbosity: + verbose_print: false # Enables additional prints + state_msgs: true # Messages of controller state + tf_frames: true # Extra tf frames + +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 500 + +# Settings for ros_control hardware interface +hardware_interface: + control_freq: 500 # in Hz + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 diff --git a/res/launch/examples.launch b/res/launch/examples.launch new file mode 100644 index 0000000..7f85fbb --- /dev/null +++ b/res/launch/examples.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/res/orcidlink.sty b/res/orcidlink.sty new file mode 100644 index 0000000..cfa2f7f --- /dev/null +++ b/res/orcidlink.sty @@ -0,0 +1,63 @@ +%% +%% This is file `orcidlink.sty', +%% generated with the docstrip utility. +%% +%% The original source files were: +%% +%% orcidlink.dtx (with options: `package') +%% +%% This is a generated file. +%% +%% Copyright (C) 2020 by Leo C. Stein +%% -------------------------------------------------------------------------- +%% This work may be distributed and/or modified under the +%% conditions of the LaTeX Project Public License, either version 1.3 +%% of this license or (at your option) any later version. +%% The latest version of this license is in +%% http://www.latex-project.org/lppl.txt +%% and version 1.3 or later is part of all distributions of LaTeX +%% version 2005/12/01 or later. +%% +\NeedsTeXFormat{LaTeX2e}[1994/06/01] +\ProvidesPackage{orcidlink} + [2021/06/11 v1.0.4 Linked ORCiD logo macro package] + +%% All I did was package up Milo's code on TeX.SE, +%% see https://tex.stackexchange.com/a/445583/34063 +\RequirePackage{hyperref} +\RequirePackage{tikz} + +\ProcessOptions\relax + +\usetikzlibrary{svg.path} + +\definecolor{orcidlogocol}{HTML}{A6CE39} +\tikzset{ + orcidlogo/.pic={ + \fill[orcidlogocol] svg{M256,128c0,70.7-57.3,128-128,128C57.3,256,0,198.7,0,128C0,57.3,57.3,0,128,0C198.7,0,256,57.3,256,128z}; + \fill[white] svg{M86.3,186.2H70.9V79.1h15.4v48.4V186.2z} + svg{M108.9,79.1h41.6c39.6,0,57,28.3,57,53.6c0,27.5-21.5,53.6-56.8,53.6h-41.8V79.1z M124.3,172.4h24.5c34.9,0,42.9-26.5,42.9-39.7c0-21.5-13.7-39.7-43.7-39.7h-23.7V172.4z} + svg{M88.7,56.8c0,5.5-4.5,10.1-10.1,10.1c-5.6,0-10.1-4.6-10.1-10.1c0-5.6,4.5-10.1,10.1-10.1C84.2,46.7,88.7,51.3,88.7,56.8z}; + } +} + +%% Reciprocal of the height of the svg whose source is above. The +%% original generates a 256pt high graphic; this macro holds 1/256. +\newcommand{\@OrigHeightRecip}{0.00390625} + +%% We will compute the current X height to make the logo the right height +\newlength{\@curXheight} + +\DeclareRobustCommand\orcidlink[1]{% +\texorpdfstring{% +\setlength{\@curXheight}{\fontcharht\font`X}% +\href{https://orcid.org/#1}{\XeTeXLinkBox{\mbox{% +\begin{tikzpicture}[yscale=-\@OrigHeightRecip*\@curXheight, +xscale=\@OrigHeightRecip*\@curXheight,transform shape] +\pic{orcidlogo}; +\end{tikzpicture}% +}}}}{}} + +\endinput +%% +%% End of file `orcidlink.sty'. diff --git a/res/urdf/robot.urdf.xacro b/res/urdf/robot.urdf.xacro new file mode 100644 index 0000000..1a33f43 --- /dev/null +++ b/res/urdf/robot.urdf.xacro @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scripts/compile_paper.sh b/scripts/compile_paper.sh index 27fde1c..c619a7c 100755 --- a/scripts/compile_paper.sh +++ b/scripts/compile_paper.sh @@ -10,4 +10,6 @@ docker run --rm \ --volume $PWD/res:/data \ --user $(id -u):$(id -g) \ --env JOURNAL=joss \ - openjournals/inara \ No newline at end of file + openjournals/inara \ + -o preprint,pdf,crossref \ + paper.md \ No newline at end of file diff --git a/scripts/install_dependencies.sh b/scripts/install_dependencies.sh index 5e54b08..6f22830 100755 --- a/scripts/install_dependencies.sh +++ b/scripts/install_dependencies.sh @@ -1,5 +1,5 @@ #!/bin/bash - +sudo apt install python3-rosdep if [ ! -d "src" ] then print "This script should be run in a catkin workspace. Exiting." @@ -40,4 +40,7 @@ cmake -DCMAKE_BUILD_TYPE=Release -DPYTHON_BINDING=OFF .. make -j sudo make install -cd ../.. \ No newline at end of file +cd ../.. + +# Rebuild the library cache +sudo ldconfig diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index 4edd9b8..14615b0 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/src/cartesian_impedance_controller.cpp @@ -103,6 +103,10 @@ namespace cartesian_impedance_controller void CartesianImpedanceController::setNumberOfJoints(size_t n_joints) { + if (n_joints < 0) + { + throw std::invalid_argument("Number of joints must be positive"); + } this->n_joints_ = n_joints; this->q_ = Eigen::VectorXd::Zero(this->n_joints_); this->dq_ = Eigen::VectorXd::Zero(this->n_joints_); @@ -116,12 +120,25 @@ namespace cartesian_impedance_controller { for (int i = 0; i < 6; i++) { - assert(stiffness(i) >= 0.0 && "Stiffness values need to be positive."); // Set diagonal values of stiffness matrix - this->cartesian_stiffness_target_(i, i) = stiffness(i); + if (stiffness(i) < 0.0) + { + assert(stiffness(i) >= 0 && "Stiffness values need to be positive."); + this->cartesian_stiffness_target_(i, i) = 0.0; + } + else + { + this->cartesian_stiffness_target_(i, i) = stiffness(i); + } + } + if (stiffness(6) < 0.0) { + assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); + this->nullspace_stiffness_target_ = 0.0; + } + else + { + this->nullspace_stiffness_target_ = stiffness(6); } - assert(stiffness(6) >= 0.0 && "Stiffness values need to be positive."); - this->nullspace_stiffness_target_ = stiffness(6); if (auto_damping) { this->applyDamping(); @@ -143,7 +160,7 @@ namespace cartesian_impedance_controller this->setStiffness(stiffness_vector, auto_damping); } - void CartesianImpedanceController::setDamping(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, + void CartesianImpedanceController::setDampingFactors(double d_x, double d_y, double d_z, double d_a, double d_b, double d_c, double d_n) { Eigen::Matrix damping_new; @@ -152,8 +169,9 @@ namespace cartesian_impedance_controller { if (damping_new(i) < 0) { + assert(damping_new(i) >= 0 && "Damping factor must not be negative."); damping_new(i) = this->damping_factors_(i); - } + } } this->damping_factors_ = damping_new; this->applyDamping(); @@ -167,6 +185,7 @@ namespace cartesian_impedance_controller this->cartesian_damping_target_(i, i) = this->damping_factors_(i) * this->dampingRule(this->cartesian_stiffness_target_(i, i)); } + assert(this->damping_factors_(6) >= 0.0 && "Damping values need to be positive."); this->nullspace_damping_target_ = this->damping_factors_(6) * this->dampingRule(this->nullspace_stiffness_target_); } @@ -236,8 +255,8 @@ namespace cartesian_impedance_controller updateFilteredWrench(); // Compute error term - error_.head(3) << this->position_ - this->position_d_; - error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); + this->error_.head(3) << this->position_ - this->position_d_; + this->error_.tail(3) << calculateOrientationError(this->orientation_d_, this->orientation_); // Kinematic pseuoinverse Eigen::MatrixXd jacobian_transpose_pinv; diff --git a/src/cartesian_impedance_controller_ros.cpp b/src/cartesian_impedance_controller_ros.cpp index f08846e..6354ff0 100644 --- a/src/cartesian_impedance_controller_ros.cpp +++ b/src/cartesian_impedance_controller_ros.cpp @@ -54,7 +54,7 @@ namespace cartesian_impedance_controller std::vector joint_names; if (!nh.getParam("joints", joint_names)) { - ROS_ERROR("Invalid or no joint_names parameters provided, aborting controller init!"); + ROS_ERROR("Invalid or no 'joints' parameter provided, aborting controller init!"); return false; } for (size_t i = 0; i < joint_names.size(); ++i) @@ -70,6 +70,7 @@ namespace cartesian_impedance_controller } } ROS_INFO_STREAM("Number of joints specified in parameters: " << joint_names.size()); + this->setNumberOfJoints(joint_names.size()); return true; } @@ -80,9 +81,9 @@ namespace cartesian_impedance_controller &CartesianImpedanceControllerRos::cartesianStiffnessCb, this); this->sub_cart_wrench_ = nh->subscribe("set_cartesian_wrench", 1, &CartesianImpedanceControllerRos::wrenchCommandCb, this); - this->sub_damping_ = nh->subscribe("set_damping_factors", 1, - &CartesianImpedanceControllerRos::cartesianDampingCb, this); - this->sub_impedance_config_ = + this->sub_damping_factors_ = nh->subscribe("set_damping_factors", 1, + &CartesianImpedanceControllerRos::cartesianDampingFactorCb, this); + this->sub_controller_config_ = nh->subscribe("set_config", 1, &CartesianImpedanceControllerRos::controllerConfigCb, this); this->sub_reference_pose_ = nh->subscribe("reference_pose", 1, &CartesianImpedanceControllerRos::referencePoseCb, this); @@ -297,7 +298,7 @@ namespace cartesian_impedance_controller void CartesianImpedanceControllerRos::controllerConfigCb(const cartesian_impedance_controller::ControllerConfigConstPtr &msg) { this->setStiffness(msg->cartesian_stiffness, msg->nullspace_stiffness, false); - this->setDamping(msg->cartesian_damping, msg->nullspace_damping); + this->setDampingFactors(msg->cartesian_damping_factors, msg->nullspace_damping_factor); if (msg->q_d_nullspace.size() == this->n_joints_) { @@ -314,9 +315,9 @@ namespace cartesian_impedance_controller } } - void CartesianImpedanceControllerRos::cartesianDampingCb(const geometry_msgs::WrenchConstPtr &msg) + void CartesianImpedanceControllerRos::cartesianDampingFactorCb(const geometry_msgs::WrenchConstPtr &msg) { - this->setDamping(*msg, this->nullspace_damping_); + this->setDampingFactors(*msg, this->damping_factors_[6]); } void CartesianImpedanceControllerRos::referencePoseCb(const geometry_msgs::PoseStampedConstPtr &msg) @@ -344,15 +345,15 @@ namespace cartesian_impedance_controller this->setStiffness(msg->wrench, this->nullspace_stiffness_target_); } - void CartesianImpedanceControllerRos::setDamping(const geometry_msgs::Wrench &cart_damping, double nullspace) + void CartesianImpedanceControllerRos::setDampingFactors(const geometry_msgs::Wrench &cart_damping, double nullspace) { - CartesianImpedanceController::setDamping(saturateValue(cart_damping.force.x, dmp_min_, dmp_max_), - saturateValue(cart_damping.force.y, dmp_min_, dmp_max_), - saturateValue(cart_damping.force.z, dmp_min_, dmp_max_), - saturateValue(cart_damping.torque.x, dmp_min_, dmp_max_), - saturateValue(cart_damping.torque.y, dmp_min_, dmp_max_), - saturateValue(cart_damping.torque.z, dmp_min_, dmp_max_), - saturateValue(nullspace, dmp_min_, dmp_max_)); + CartesianImpedanceController::setDampingFactors(saturateValue(cart_damping.force.x, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.force.y, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.force.z, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.torque.x, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.torque.y, dmp_factor_min_, dmp_factor_max_), + saturateValue(cart_damping.torque.z, dmp_factor_min_, dmp_factor_max_), + saturateValue(nullspace, dmp_factor_min_, dmp_factor_max_)); } void CartesianImpedanceControllerRos::setStiffness(const geometry_msgs::Wrench &cart_stiffness, double nullspace, bool auto_damping) @@ -507,11 +508,8 @@ namespace cartesian_impedance_controller { if (config.update_damping_factors) { - CartesianImpedanceController::setDamping( - saturateValue(config.translation_x, dmp_min_, dmp_max_), saturateValue(config.translation_y, dmp_min_, dmp_max_), - saturateValue(config.translation_z, dmp_min_, dmp_max_), saturateValue(config.rotation_x, dmp_min_, dmp_max_), - saturateValue(config.rotation_y, dmp_min_, dmp_max_), saturateValue(config.rotation_z, dmp_min_, dmp_max_), - config.nullspace_damping); + CartesianImpedanceController::setDampingFactors( + config.translation_x, config.translation_y, config.translation_z, config.rotation_x, config.rotation_y, config.rotation_z, config.nullspace_damping); } } diff --git a/test/base.test b/test/base.test new file mode 100644 index 0000000..954f11c --- /dev/null +++ b/test/base.test @@ -0,0 +1,3 @@ + + + diff --git a/test/base_tests.cpp b/test/base_tests.cpp new file mode 100644 index 0000000..46865cb --- /dev/null +++ b/test/base_tests.cpp @@ -0,0 +1,124 @@ +#include "cartesian_impedance_controller/cartesian_impedance_controller.h" +#include +#include + + +using namespace cartesian_impedance_controller; + + + +CartesianImpedanceController cont; // Create an instance of the controller + + +TEST(ControllerTests, initializationTests){ + const Eigen::Vector3d position_d_target(1.0, 2.0, 3.0); + const Eigen::Quaterniond orientation_d_target(1.0, 0.0, 0.0, 0.0); + + EXPECT_NO_THROW(cont.initDesiredPose(position_d_target, orientation_d_target)); +} + +TEST(ControllerTests, nullspaceConfigTests){ + Eigen::VectorXd q_d_nullspace_target(7); + q_d_nullspace_target << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; + const int n = 7; + cont.setNumberOfJoints(n); + EXPECT_NO_THROW(cont.initNullspaceConfig(q_d_nullspace_target)); //Should not fail since we have the same number of joints as q_d_nullspace_target elements + + cont.setNumberOfJoints(n+1); + EXPECT_DEBUG_DEATH(cont.initNullspaceConfig(q_d_nullspace_target), "Nullspace target needs to same size as n_joints_"); //Should fail since we have differences between the number of joints and the number of q_d_nullspace_target elements +} + +TEST(ControllerTests, setjointnumberTests){ + const int n = 7; + EXPECT_NO_THROW(cont.setNumberOfJoints(n)); //This value of n shouldn't throw an exception. + EXPECT_ANY_THROW(cont.setNumberOfJoints(-n)); //For a negative number of joints, we expect to catch the error. +} + +TEST(ControllerTests, dampingTests){ + const double d = 3.2; + EXPECT_NO_THROW(cont.setDampingFactors(d,d,d,d,d,d,d)); //This should work. + EXPECT_NO_THROW(cont.setDampingFactors(-d,d,d,d,d,d,d)); //Negative damping factors are ignored outside debug builds + + EXPECT_DEBUG_DEATH(cont.setDampingFactors(-d,d,d,d,d,d,d), "Negative damping factors are tested by asserts"); //This should not work since damping should be positive +} + +TEST(ControllerTests, stiffnessTests) { + Eigen::Matrix stiffness_good, stiffness_bad; + stiffness_good << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + stiffness_bad << -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + double t_x = 1.0, t_y = 1.0, t_z = 1.0, r_x = 1.0, r_y = 1.0, r_z = 1.0, n = 1.0; + + // Without auto-damping + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, false)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, false), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, false)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, false)); + // With auto-damping + EXPECT_NO_THROW(cont.setStiffness(stiffness_good, true)); + EXPECT_DEBUG_DEATH(cont.setStiffness(stiffness_bad, true), "Stiffness values need to be positive."); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, true)); + EXPECT_NO_THROW(cont.setStiffness(t_x, t_y, t_z, r_x, r_y, r_z, n, true)); +} + + +TEST(ControllerTests, filteringTests){ + EXPECT_NO_THROW(cont.setFiltering(100, 0.5, 0.6, 0.7, 0.8)); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 0.7, 0.8), "Update frequency needs to be greater or equal to zero"); + EXPECT_DEBUG_DEATH(cont.setFiltering(-100, 0.5, 0.6, 1.7, 0.8), "Filter params need to be between 0 and 1."); +} + +TEST(ControllerTests, saturationTests){ + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0)); + EXPECT_NO_THROW(cont.setMaxTorqueDelta(2.0,100.0)); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(-2.0,100.0), "Allowed torque change must be positive"); + EXPECT_DEBUG_DEATH(cont.setMaxTorqueDelta(2.0,-100.0), "Update frequency needs to be greater or equal to zero"); +} + +TEST(ControllerTests, wrenchTest){ + Eigen::Matrix wrench; + wrench << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + EXPECT_NO_THROW(cont.applyWrench(wrench)); +} + +TEST(ControllerTests, getterTests){ + Eigen::VectorXd q(7); + q << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::VectorXd dq(7); + dq << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Vector3d position(1.0, 2.0, 3.0); + Eigen::Quaterniond orientation(1.0, 0.0, 0.0, 0.0); + Eigen::Vector3d position_d(2.0, 2.0, 3.0); + Eigen::Quaterniond orientation_d(1.0, 0.0, 0.0, 0.0); + Eigen::Matrix cartesian_stiffness; + double nullspace_stiffness = 1.0; + Eigen::VectorXd q_d_nullspace(7); + q_d_nullspace << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; + Eigen::Matrix cartesian_damping; + + EXPECT_NO_THROW(cont.getState(&q, &dq, &position, &orientation, &position_d, &orientation_d, + &cartesian_stiffness, &nullspace_stiffness, &q_d_nullspace, &cartesian_damping)); + EXPECT_NO_THROW(cont.getAppliedWrench()); + EXPECT_NO_THROW(cont.getLastCommands()); + EXPECT_NO_THROW(cont.getPoseError()); + + cont.setNumberOfJoints(7); + Eigen::MatrixXd jacobian(6,7); + jacobian.setZero(); + EXPECT_NO_THROW(cont.calculateCommandedTorques(q, dq, position, orientation, jacobian)); + + Eigen::Matrix error_getter, error_state; + error_getter << cont.getPoseError(); + error_state.head(3) << position - position_d; + + EXPECT_DOUBLE_EQ(error_getter(0),error_state(0)); + EXPECT_DOUBLE_EQ(error_getter(1),error_state(1)); + EXPECT_DOUBLE_EQ(error_getter(2),error_state(2)); +} + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/test/ros.test b/test/ros.test new file mode 100644 index 0000000..34de6e1 --- /dev/null +++ b/test/ros.test @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/test/ros_func.test b/test/ros_func.test new file mode 100644 index 0000000..2143ede --- /dev/null +++ b/test/ros_func.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/test/ros_func_tests.cpp b/test/ros_func_tests.cpp new file mode 100644 index 0000000..e223ff4 --- /dev/null +++ b/test/ros_func_tests.cpp @@ -0,0 +1,90 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +const std::string ctrl_name = "/CartesianImpedance_trajectory_controller"; + +//ROSFunctionalityTests tested the controller functionality + +TEST(ROSFunctionalityTests, forceTests) // Tested that a desired external force is being applied. +{ + ros::NodeHandle n; + ros::Duration timeout(5); + ros::Publisher wrench_publisher = n.advertise(ctrl_name +"/set_cartesian_wrench", 500); + + const std::string controller_state {ctrl_name + "/controller_state"}; + auto msg_controller_up = ros::topic::waitForMessage(controller_state, n, timeout); + ASSERT_TRUE(msg_controller_up != nullptr) << "Controller is not up."; + EXPECT_DOUBLE_EQ(std::abs(msg_controller_up->commanded_wrench.force.x), 0.0); + + geometry_msgs::WrenchStamped wrench_msg; + wrench_msg.header.frame_id = "world"; + wrench_msg.header.stamp = ros::Time::now(); + wrench_msg.wrench.force.x = 5.0; + wrench_msg.wrench.force.y = 0.0; + wrench_msg.wrench.force.z = 0.0; + wrench_msg.wrench.torque.x = 0.0; + wrench_msg.wrench.torque.y = 0.0; + wrench_msg.wrench.torque.z = 0.0; + wrench_publisher.publish(wrench_msg); + ros::Duration(1).sleep(); + + for (int i = 0; i < 5; i++) { + auto msg_state = ros::topic::waitForMessage(controller_state, n, timeout); + + ASSERT_TRUE(msg_state != nullptr) << "Did not receive message on iteration " << i + 1; + EXPECT_GE(std::abs(msg_state->commanded_wrench.force.x), 1.0); + } + + // Cancel the wrench + wrench_msg.header.stamp = ros::Time::now(); + wrench_msg.wrench.force.x = 0.0; + wrench_publisher.publish(wrench_msg); + ros::Duration(1).sleep(); +} + +TEST(ROSFunctionalityTests, motionTests) // Tested that a new desired reference pose is sought +{ + ros::NodeHandle n; + ros::Duration timeout(5); + ros::Publisher pose_publisher = n.advertise(ctrl_name +"/reference_pose", 500); + ros::Duration(0.5).sleep(); // C++ publisher need some time to start up + + const std::string controller_state {ctrl_name + "/controller_state"}; + auto msg_controller_up = ros::topic::waitForMessage(controller_state, n, timeout); + ASSERT_TRUE(msg_controller_up != nullptr) << "Controller is not up."; + EXPECT_NEAR(std::abs(msg_controller_up->current_pose.position.y), 0.3, 0.05); + + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = "world"; + pose_msg.header.stamp = ros::Time::now(); + pose_msg.pose.position.x = 0.4; + pose_msg.pose.position.y = 0.0; + pose_msg.pose.position.z = 0.14; + pose_msg.pose.orientation.x = 0.0; + pose_msg.pose.orientation.y = 1.0; + pose_msg.pose.orientation.z = 0.0; + pose_msg.pose.orientation.w = 0.0; + pose_publisher.publish(pose_msg); + ros::Duration(5).sleep(); + + for (int i = 0; i < 5; i++) { + auto msg_state = ros::topic::waitForMessage(controller_state, n, timeout); + ASSERT_TRUE(msg_state != nullptr) << "Did not receive message on iteration " << i + 1; + EXPECT_NEAR(std::abs(msg_state->reference_pose.position.y), 0.0, 0.11); + EXPECT_NEAR(std::abs(msg_state->current_pose.position.y), 0.0, 0.15); + ros::Duration(0.1).sleep(); + } +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_ros"); + return RUN_ALL_TESTS(); +} + diff --git a/test/ros_tests.cpp b/test/ros_tests.cpp new file mode 100644 index 0000000..7aa09af --- /dev/null +++ b/test/ros_tests.cpp @@ -0,0 +1,135 @@ +#include +#include +#include +#include +#include +#include +#include + +const std::string ctrl_name = "/CartesianImpedance_trajectory_controller"; + +//ROSBaseTests tested that key topics in the library are being written. + +TEST(ROSBaseTests, commandedtorqueTests) +{ + ros::NodeHandle n; + ros::Duration timeout(5); + const std::string torque_topic {ctrl_name + "/commanded_torques"}; + for (int i = 0; i < 5; i++) { + auto msg_torque = ros::topic::waitForMessage(torque_topic, n, timeout); + + ASSERT_TRUE(msg_torque != nullptr) << "Did not receive message on iteration " << i + 1; + EXPECT_LE(std::abs(msg_torque->data[0]), 1000.0); + } + +} + +//ROSReconfigureTests tested that we are able to modify external wrench, virtual stiffness, and damping online. +TEST(ROSReconfigureTests, wrenchTest) { + ros::NodeHandle n; + ros::Duration timeout(10); + for (int i = 0; i < 5; i++) { + auto msg_wup = ros::topic::waitForMessage( + ctrl_name + "/cartesian_wrench_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_wup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_wup->doubles) { + if(double_param.name == "f_x") { + EXPECT_EQ(double_param.value, 5.0); + break; + } + } + } +} + +TEST(ROSReconfigureTests, stiffnessTest) { + ros::NodeHandle n; + ros::Duration timeout(5); + for (int i = 0; i < 5; i++) { + auto msg_stup = ros::topic::waitForMessage( + ctrl_name + "/stiffness_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_stup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_stup->doubles) { + if(double_param.name == "translation_x") { + EXPECT_EQ(double_param.value, 100.0); + break; + } + } + } +} + +TEST(ROSReconfigureTests, dampingTest) { + ros::NodeHandle n; + ros::Duration timeout(5); + for (int i = 0; i < 5; i++) { + auto msg_dup = ros::topic::waitForMessage( + ctrl_name + "/damping_factors_reconfigure/parameter_updates", n, timeout); + + ASSERT_TRUE(msg_dup != nullptr) << "Did not receive message on iteration " << i + 1; + + for(const dynamic_reconfigure::DoubleParameter& double_param : msg_dup->doubles) { + if(double_param.name == "translation_x") { + EXPECT_EQ(double_param.value, 0.75); + break; + } + } + } +} + +//ROSParameterTests make sure that all the necessary ROS parameters have been created. +TEST(ROSParameterTests, paramTest) { + const std::vector params_to_check = { + "cartesian_wrench_reconfigure/apply_wrench", + "cartesian_wrench_reconfigure/f_x", + "cartesian_wrench_reconfigure/f_y", + "cartesian_wrench_reconfigure/f_z", + "cartesian_wrench_reconfigure/tau_x", + "cartesian_wrench_reconfigure/tau_y", + "cartesian_wrench_reconfigure/tau_z", + "damping_factors_reconfigure/nullspace_damping", + "damping_factors_reconfigure/rotation_x", + "damping_factors_reconfigure/rotation_y", + "damping_factors_reconfigure/rotation_z", + "damping_factors_reconfigure/translation_x", + "damping_factors_reconfigure/translation_y", + "damping_factors_reconfigure/translation_z", + "damping_factors_reconfigure/update_damping_factors", + "delta_tau_max", + "dynamic_reconfigure", + "end_effector", + "filtering/nullspace_config", + "filtering/pose", + "filtering/stiffness", + "filtering/wrench", + "handle_trajectories", + "joints", + "root_frame", + "stiffness_reconfigure/nullspace_stiffness", + "stiffness_reconfigure/rotation_x", + "stiffness_reconfigure/rotation_y", + "stiffness_reconfigure/rotation_z", + "stiffness_reconfigure/translation_x", + "stiffness_reconfigure/translation_y", + "stiffness_reconfigure/translation_z", + "stiffness_reconfigure/update_stiffness", + "type", + "update_frequency", + "verbosity/state_msgs", + "verbosity/tf_frames", + "verbosity/verbose_print", + "wrench_ee_frame" + }; + + for (const auto ¶m : params_to_check) { + EXPECT_TRUE(ros::param::has("/" + ctrl_name + "/" + param)) << "Parameter [" << param << "] does NOT exist."; + } +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_ros"); + return RUN_ALL_TESTS(); +}