From 54f1c3cf5e14473af0b8a9bbaa150d05994b013b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 4 Oct 2021 16:43:01 +0200 Subject: [PATCH] Ros2 control extensions rolling joint limits plugins (#5) * Added initial structures for joint-limit plugins. * Move Ruckig limiter to package joint_limits_enforcement_plugins and make it working. Co-authored-by: AndyZe --- joint_limits/CMakeLists.txt | 97 ++++++++++ .../joint_limits/joint_limiter_interface.hpp | 93 +++++++++ .../include/joint_limits/joint_limits.hpp | 58 ++++++ .../joint_limits/joint_limits_rosparam.hpp | 2 +- .../joint_limits/simple_joint_limiter.hpp | 41 ++++ .../include/joint_limits/visibility_control.h | 49 +++++ joint_limits/joint_limiters.xml | 9 + joint_limits/package.xml | 26 +++ joint_limits/src/joint_limiter_interface.cpp | 72 +++++++ joint_limits/src/simple_joint_limiter.cpp | 170 ++++++++++++++++ .../test/test_simple_joint_limiter.cpp | 42 ++++ .../CMakeLists.txt | 85 ++++++++ .../visibility_control.h | 49 +++++ .../ruckig_joint_limiter.hpp | 64 ++++++ .../joint_limits_enforcement_plugins.xml | 9 + joint_limits_enforcement_plugins/package.xml | 29 +++ .../src/ruckig_joint_limiter.cpp | 182 ++++++++++++++++++ .../test/test_ruckig_joint_limiter.cpp | 44 +++++ ros2_control/package.xml | 2 + 19 files changed, 1122 insertions(+), 1 deletion(-) create mode 100644 joint_limits/CMakeLists.txt create mode 100644 joint_limits/include/joint_limits/joint_limiter_interface.hpp rename {hardware_interface => joint_limits}/include/joint_limits/joint_limits.hpp (54%) rename {hardware_interface => joint_limits}/include/joint_limits/joint_limits_rosparam.hpp (99%) create mode 100644 joint_limits/include/joint_limits/simple_joint_limiter.hpp create mode 100644 joint_limits/include/joint_limits/visibility_control.h create mode 100644 joint_limits/joint_limiters.xml create mode 100644 joint_limits/package.xml create mode 100644 joint_limits/src/joint_limiter_interface.cpp create mode 100644 joint_limits/src/simple_joint_limiter.cpp create mode 100644 joint_limits/test/test_simple_joint_limiter.cpp create mode 100644 joint_limits_enforcement_plugins/CMakeLists.txt create mode 100644 joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h create mode 100644 joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp create mode 100644 joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml create mode 100644 joint_limits_enforcement_plugins/package.xml create mode 100644 joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp create mode 100644 joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt new file mode 100644 index 0000000000..b50834b8e0 --- /dev/null +++ b/joint_limits/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.5) +project(joint_limits) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) + +add_library(joint_limiter_interface SHARED src/joint_limiter_interface.cpp) +target_include_directories( + joint_limiter_interface + PRIVATE + include) +ament_target_dependencies( + joint_limiter_interface + rclcpp +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_limiter_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + +add_library( + simple_joint_limiter + SHARED + src/simple_joint_limiter.cpp +) +target_include_directories( + simple_joint_limiter + PRIVATE + include +) +target_link_libraries( + simple_joint_limiter + joint_limiter_interface +) +ament_target_dependencies( + simple_joint_limiter + rclcpp + rcutils +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(simple_joint_limiter PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + joint_limiter_interface + simple_joint_limiter + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + + #ament_add_gmock(joint_limits_test test/joint_limits_test.cpp) + #target_include_directories(joint_limits_test PUBLIC include) + #target_link_libraries(joint_limits_test joint_limits) + + ament_add_gmock(test_simple_joint_limiter test/test_simple_joint_limiter.cpp) + target_include_directories(test_simple_joint_limiter PRIVATE include) + target_link_libraries(test_simple_joint_limiter joint_limiter_interface) + ament_target_dependencies( + test_simple_joint_limiter + pluginlib + rclcpp + ) +endif() + +ament_export_dependencies( + rclcpp + rcutils +) + +ament_export_include_directories( + include +) + +ament_export_libraries( + joint_limiter_interface + simple_joint_limiter +) + +ament_package() diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..0aaf3ee57e --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include + +#include "joint_limits/joint_limits.hpp" +#include "joint_limits/visibility_control.h" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace joint_limits +{ +template +class JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC JointLimiterInterface() = default; + + JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default; + + /// Initialization of every JointLimiter. + /** + * Initialization of JointLimiter for defined joints with their names. + * Robot description topic provides a topic name where URDF of the robot can be found. + * This is needed to use joint limits from URDF (not implemented yet!). + * Override this method only if Initialization and reading joint limits should be adapted. + * Otherwise, initialize your custom limiter in `on_limit` method. + * + * \param[in] joint_names names of joints where limits should be applied. + * \param[in] node shared pointer to the node where joint limit parameters defined. + * \param[in] robot_description_topic string of a topic where robot description is accessible. + * + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & robot_description_topic = "/robot_description"); + + JOINT_LIMITS_PUBLIC virtual bool configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) + { + // TODO(destogl): add checks for position + return on_configure(current_joint_states); + } + + JOINT_LIMITS_PUBLIC virtual bool enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) + { + // TODO(destogl): add checks if sizes of vectors and number of limits correspond. + return on_enforce(current_joint_states, desired_joint_states, dt); + } + + // TODO(destogl): Make those protected? + // Methods that each limiter implementation has to implement + JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; } + + JOINT_LIMITS_PUBLIC virtual bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) + { + return true; + } + + JOINT_LIMITS_PUBLIC virtual bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) = 0; + +protected: + size_t number_of_joints_; + std::vector joint_limits_; + rclcpp::Node::SharedPtr node_; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp similarity index 54% rename from hardware_interface/include/joint_limits/joint_limits.hpp rename to joint_limits/include/joint_limits/joint_limits.hpp index 537d6305fb..0ccb3e52a4 100644 --- a/hardware_interface/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -18,6 +18,8 @@ #define JOINT_LIMITS__JOINT_LIMITS_HPP_ #include +#include +#include namespace joint_limits { @@ -52,6 +54,62 @@ struct JointLimits bool has_jerk_limits; bool has_effort_limits; bool angle_wraparound; + + std::string to_string() + { + std::stringstream ss_output; + + if (has_position_limits) + { + ss_output << " position limits: " + << "[" << min_position << ", " << max_position << "]\n"; + } + if (has_velocity_limits) + { + ss_output << " velocity limit: " + << "[" << max_velocity << "]\n"; + } + if (has_acceleration_limits) + { + ss_output << " acceleration limit: " + << "[" << max_acceleration << "]\n"; + } + if (has_jerk_limits) + { + ss_output << " jerk limit: " + << "[" << max_acceleration << "]\n"; + } + if (has_effort_limits) + { + ss_output << " effort limit: " + << "[" << max_acceleration << "]\n"; + } + if (angle_wraparound) + { + ss_output << " angle wraparound is active."; + } + + return ss_output.str(); + } + + std::string debug_to_string() + { + std::stringstream ss_output; + + ss_output << " has position limits: " << (has_position_limits ? "true" : "false") << "[" + << min_position << ", " << max_position << "]\n"; + ss_output << " has velocity limits: " << (has_velocity_limits ? "true" : "false") << "[" + << max_velocity << "]\n"; + ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false") + << " [" << max_acceleration << "]\n"; + ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << "[" << max_jerk + << "]\n"; + ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << "[" + << max_effort << "]\n"; + ss_output << " angle wraparound: " << (angle_wraparound ? "true" : "false"); + + return ss_output.str(); + } }; struct SoftJointLimits diff --git a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp similarity index 99% rename from hardware_interface/include/joint_limits/joint_limits_rosparam.hpp rename to joint_limits/include/joint_limits/joint_limits_rosparam.hpp index eeb96d48a2..05c7f3d67c 100644 --- a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -259,7 +259,7 @@ inline bool get_joint_limits( * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. */ -inline bool get_soft_joint_limits( +inline bool get_joint_limits( const std::string & joint_name, const rclcpp::Node::SharedPtr & node, SoftJointLimits & soft_limits) { diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp new file mode 100644 index 0000000000..2011e0d48e --- /dev/null +++ b/joint_limits/include/joint_limits/simple_joint_limiter.hpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ + +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" + +namespace joint_limits +{ +template +class SimpleJointLimiter : public JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC SimpleJointLimiter(); + + JOINT_LIMITS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h new file mode 100644 index 0000000000..0dcc0193a1 --- /dev/null +++ b/joint_limits/include/joint_limits/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_BUILDING_DLL +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT +#else +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT +#endif +#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#else +#define JOINT_LIMITS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#endif +#define JOINT_LIMITS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml new file mode 100644 index 0000000000..929e7f538b --- /dev/null +++ b/joint_limits/joint_limiters.xml @@ -0,0 +1,9 @@ + + + + Simple joint limiter using clamping approach. + + + diff --git a/joint_limits/package.xml b/joint_limits/package.xml new file mode 100644 index 0000000000..b66a16e928 --- /dev/null +++ b/joint_limits/package.xml @@ -0,0 +1,26 @@ + + joint_limits + 0.0.0 + Package for with interfaces for handling of joint limits for use in controllers or in hardware. It also implements a simple, default joint limit strategy by clamping the values. + + Bence Magyar + Denis Štogl + + Apache License 2.0 + + https://github.com/ros-controls/ros2_control/wiki + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + ament_cmake + + rclcpp + pluginlib + rcutils + + ament_cmake_gmock + + + ament_cmake + + diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp new file mode 100644 index 0000000000..b0d109b0cd --- /dev/null +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include "joint_limits/joint_limiter_interface.hpp" + +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" + +// TODO(anyone): Add handing of SoftLimits +namespace joint_limits +{ +template <> +bool JointLimiterInterface::init( + const std::vector joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & /*robot_description_topic*/) +{ + number_of_joints_ = joint_names.size(); + joint_limits_.resize(number_of_joints_); + node_ = node; + + bool result = true; + + // TODO(destogl): get limits from URDF + + // Initialize and get joint limits from parameter server + for (auto i = 0ul; i < number_of_joints_; ++i) + { + if (!declare_parameters(joint_names[i], node)) + { + RCLCPP_ERROR( + node->get_logger(), "JointLimiter: Joint '%s': parameter declaration has failed", + joint_names[i].c_str()); + result = false; + break; + } + if (!joint_limits::get_joint_limits(joint_names[i], node, joint_limits_[i])) + { + RCLCPP_ERROR( + node->get_logger(), "JointLimiter: Joint '%s': getting parameters has failed", + joint_names[i].c_str()); + result = false; + break; + } + RCLCPP_INFO( + node->get_logger(), "Joint '%s':\n%s", joint_names[i].c_str(), + joint_limits_[i].to_string().c_str()); + } + + if (result) + { + result = on_init(); + } + + return result; +} + +} // namespace joint_limits diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp new file mode 100644 index 0000000000..5106615ea8 --- /dev/null +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Nathan Brooks, Denis Stogl + +#include "joint_limits/simple_joint_limiter.hpp" + +#include + +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops + +namespace joint_limits +{ +template <> +SimpleJointLimiter::SimpleJointLimiter() +: joint_limits::JointLimiterInterface() +{ +} + +template <> +bool SimpleJointLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +{ + auto num_joints = joint_limits_.size(); + auto dt_seconds = dt.seconds(); + + if (current_joint_states.velocities.empty()) + { + // First update() after activating does not have velocity available, assume 0 + current_joint_states.velocities.resize(num_joints, 0.0); + } + + // Clamp velocities to limits + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_velocity_limits) + { + if (std::abs(desired_joint_states.velocities[index]) > joint_limits_[index].max_velocity) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed velocity limits, limiting"); + desired_joint_states.velocities[index] = + copysign(joint_limits_[index].max_velocity, desired_joint_states.velocities[index]); + double accel = + (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / + dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * accel * dt_seconds * dt_seconds; + } + } + } + + // Clamp acclerations to limits + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + double accel = + (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / + dt_seconds; + if (std::abs(accel) > joint_limits_[index].max_acceleration) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed acceleration limits, limiting"); + desired_joint_states.velocities[index] = + current_joint_states.velocities[index] + + copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds * dt_seconds; + } + } + } + + // Check that stopping distance is within joint limits + // - In joint mode, slow down only joints whose stopping distance isn't inside joint limits, + // at maximum decel + // - In Cartesian mode, slow down all joints at maximum decel if any don't have stopping distance + // within joint limits + bool position_limit_triggered = false; + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + // delta_x = (v2*v2 - v1*v1) / (2*a) + // stopping_distance = (- v1*v1) / (2*max_acceleration) + // Here we assume we will not trigger velocity limits while maximally decelerating. + // This is a valid assumption if we are not currently at a velocity limit since we are just + // coming to a rest. + double stopping_distance = std::abs( + (-desired_joint_states.velocities[index] * desired_joint_states.velocities[index]) / + (2 * joint_limits_[index].max_acceleration)); + // Check that joint limits are beyond stopping_distance and desired_velocity is towards + // that limit + // TODO(anyone): Should we consider sign on acceleration here? + if ( + (desired_joint_states.velocities[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + stopping_distance)) || + (desired_joint_states.velocities[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + stopping_distance))) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed position limits, limiting"); + position_limit_triggered = true; + + // We will limit all joints + break; + } + } + } + + if (position_limit_triggered) + { + // In Cartesian admittance mode, stop all joints if one would exceed limit + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + double accel_to_stop = -current_joint_states.velocities[index] / dt_seconds; + double limited_accel = copysign( + std::min(std::abs(accel_to_stop), joint_limits_[index].max_acceleration), accel_to_stop); + + desired_joint_states.velocities[index] = + current_joint_states.velocities[index] + limited_accel * dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * limited_accel * dt_seconds * dt_seconds; + } + } + } + return true; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + joint_limits::SimpleJointLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp new file mode 100644 index 0000000000..e025ac0b9f --- /dev/null +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/executor.hpp" + +TEST(TestLoadSimpleJointLimiter, load_limiter) +{ + rclcpp::init(0, nullptr); + + using JointLimiter = joint_limits::JointLimiterInterface; + pluginlib::ClassLoader joint_limiter_loader( + "joint_limits", "joint_limits::JointLimiterInterface"); + + std::unique_ptr joint_limiter; + std::string joint_limiter_type = "joint_limits/SimpleJointLimiter"; + + ASSERT_NO_THROW( + joint_limiter = std::unique_ptr( + joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); + ASSERT_NE(joint_limiter, nullptr); +} diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt new file mode 100644 index 0000000000..302c3562f3 --- /dev/null +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.5) +project(joint_limits_enforcement_plugins) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(joint_limits REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) +find_package(ruckig REQUIRED) + +add_library( + ruckig_joint_limiter + SHARED + src/ruckig_joint_limiter.cpp +) +target_include_directories( + ruckig_joint_limiter + PRIVATE + include +) +target_link_libraries( + ruckig_joint_limiter + ruckig::ruckig +) +ament_target_dependencies( + ruckig_joint_limiter + joint_limits + rclcpp + rcutils + ruckig +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ruckig_joint_limiter PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limits_enforcement_plugins.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + ruckig_joint_limiter + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(joint_limits REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + + ament_add_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp) + target_include_directories(test_ruckig_joint_limiter PRIVATE include) + ament_target_dependencies( + test_ruckig_joint_limiter + joint_limits + pluginlib + rclcpp + ) +endif() + +ament_export_dependencies( + joint_limits + rclcpp + rcutils + ruckig +) + +ament_export_include_directories( + include +) + +ament_export_libraries( + ruckig_joint_limiter +) + +ament_package() diff --git a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h new file mode 100644 index 0000000000..abd0019cf6 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp new file mode 100644 index 0000000000..fd577c32b3 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp @@ -0,0 +1,64 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Andy Zelenak, Denis Stogl + +#ifndef RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ +#define RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ + +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_limits_enforcement_plugins/visibility_control.h" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +namespace // utility namespace +{ +constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s +constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 +constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3 +} // namespace + +template +class RuckigJointLimiter : public joint_limits::JointLimiterInterface +{ +public: + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC RuckigJointLimiter(); + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_init() override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; + +private: + bool received_initial_state_ = false; + // Ruckig algorithm + std::shared_ptr> ruckig_; + std::shared_ptr> ruckig_input_; + std::shared_ptr> ruckig_output_; +}; + +} // namespace ruckig_joint_limiter + +#endif // RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ diff --git a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml new file mode 100644 index 0000000000..40b2a0de5e --- /dev/null +++ b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml @@ -0,0 +1,9 @@ + + + + Jerk-limited smoothing with the Ruckig library. + + + diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml new file mode 100644 index 0000000000..50fecd417d --- /dev/null +++ b/joint_limits_enforcement_plugins/package.xml @@ -0,0 +1,29 @@ + + joint_limits_enforcement_plugins + 0.0.0 + Package for handling of joint limits using external libraries for use in controllers or in hardware. + + Bence Magyar + Denis Štogl + Andy Zelenak + + Apache License 2.0 + + https://github.com/ros-controls/ros2_control/wiki + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + ament_cmake + + joint_limits + rclcpp + pluginlib + rcutils + ruckig + + ament_cmake_gmock + + + ament_cmake + + diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..ad264aed67 --- /dev/null +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -0,0 +1,182 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Andy Zelenak, Denis Stogl + +#include "ruckig_joint_limiter/ruckig_joint_limiter.hpp" + +#include +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" +#include "rcutils/logging_macros.h" +#include "ruckig/brake.hpp" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +template <> +RuckigJointLimiter::RuckigJointLimiter() +: joint_limits::JointLimiterInterface() +{ +} + +template <> +bool RuckigJointLimiter::on_init(/*const rclcpp::Duration & dt*/) +{ + // TODO(destogl): This should be used from parameter + const rclcpp::Duration dt = rclcpp::Duration::from_seconds(0.005); + + // Initialize Ruckig + ruckig_ = std::make_shared>(number_of_joints_, dt.seconds()); + ruckig_input_ = std::make_shared>(number_of_joints_); + ruckig_output_ = std::make_shared>(number_of_joints_); + + // Velocity mode works best for smoothing one waypoint at a time + ruckig_input_->control_interface = ruckig::ControlInterface::Velocity; + ruckig_input_->synchronization = ruckig::Synchronization::Time; + + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + if (joint_limits_[joint].has_jerk_limits) + { + ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; + } + else + { + ruckig_input_->max_jerk.at(joint) = DEFAULT_MAX_JERK; + } + if (joint_limits_[joint].has_acceleration_limits) + { + ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration; + } + else + { + ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION; + } + if (joint_limits_[joint].has_velocity_limits) + { + ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; + } + else + { + ruckig_input_->max_velocity.at(joint) = DEFAULT_MAX_VELOCITY; + } + } + + return true; +} + +template <> +bool RuckigJointLimiter::on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) +{ + // Initialize Ruckig with current_joint_states + ruckig_input_->current_position = current_joint_states.positions; + + if (current_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->current_velocity = current_joint_states.velocities; + } + else + { + ruckig_input_->current_velocity = std::vector(number_of_joints_, 0.0); + } + if (current_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->current_acceleration = current_joint_states.accelerations; + } + else + { + ruckig_input_->current_acceleration = std::vector(number_of_joints_, 0.0); + } + + // Initialize output data + ruckig_output_->new_position = ruckig_input_->current_position; + ruckig_output_->new_velocity = ruckig_input_->current_velocity; + ruckig_output_->new_acceleration = ruckig_input_->current_acceleration; + + return true; +} + +template <> +bool RuckigJointLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & /*dt*/) +{ + // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig + // output back in as input for the next iteration. This assumes the robot tracks the command well. + ruckig_input_->current_position = ruckig_output_->new_position; + ruckig_input_->current_velocity = ruckig_output_->new_velocity; + ruckig_input_->current_acceleration = ruckig_output_->new_acceleration; + + // Target state is the next waypoint + ruckig_input_->target_position = desired_joint_states.positions; + // TODO(destogl): in current use-case we use only velocity + if (desired_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->target_velocity = desired_joint_states.velocities; + } + else + { + ruckig_input_->target_velocity = std::vector(number_of_joints_, 0.0); + } + if (desired_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->target_acceleration = desired_joint_states.accelerations; + } + else + { + ruckig_input_->target_acceleration = std::vector(number_of_joints_, 0.0); + } + + ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); + RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); + + desired_joint_states.positions = ruckig_output_->new_position; + desired_joint_states.velocities = ruckig_output_->new_velocity; + desired_joint_states.accelerations = ruckig_output_->new_acceleration; + + // Feed output from the previous timestep back as input + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", + "Desired position: %e \nDesired velocity: %e\n Desired acceleration: %e.", + ruckig_input_->target_position.at(joint), ruckig_input_->target_velocity.at(joint), + ruckig_input_->target_acceleration.at(joint)); + } + + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", "New position: %e \nNew velocity: %e\nNew acceleration: %e.", + ruckig_output_->new_position.at(joint), ruckig_output_->new_velocity.at(joint), + ruckig_output_->new_acceleration.at(joint)); + } + + return result == ruckig::Result::Finished; +} + +} // namespace ruckig_joint_limiter + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ruckig_joint_limiter::RuckigJointLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..b1b19d0587 --- /dev/null +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/executor.hpp" + +TEST(TestLoadSimpleJointLimiter, load_limiter) +{ + rclcpp::init(0, nullptr); + + using JointLimiter = joint_limits::JointLimiterInterface; + pluginlib::ClassLoader joint_limiter_loader( + "joint_limits", "joint_limits::JointLimiterInterface"); + + std::unique_ptr joint_limiter; + std::string joint_limiter_type = "ruckig_joint_limiter/RuckigJointLimiter"; + + joint_limiter = + std::unique_ptr(joint_limiter_loader.createUnmanagedInstance(joint_limiter_type)); + ASSERT_NO_THROW( + joint_limiter = std::unique_ptr( + joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); + ASSERT_NE(joint_limiter, nullptr); +} diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 302c990a3f..ab3df121f5 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -15,6 +15,8 @@ controller_manager controller_manager_msgs hardware_interface + joint_limits + ros2controlcli ros2_control_test_assets ros2controlcli transmission_interface