Add Saturation Joint Limiter that uses clamping method

controller_interface/CMakeLists.txt

@@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 )
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
  find_package(${Dependency} REQUIRED)
 endforeach()

controller_interface/package.xml

@@ -10,6 +10,7 @@
 
  <buildtool_depend>ament_cmake</buildtool_depend>
 
+ <build_depend>backward_ros</build_depend>
  <build_depend>hardware_interface</build_depend>
  <build_depend>rclcpp_lifecycle</build_depend>
  <build_depend>sensor_msgs</build_depend>

hardware_interface/CMakeLists.txt

@@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 )
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
  find_package(${Dependency} REQUIRED)
 endforeach()

hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp

@@ -15,6 +15,8 @@
 #ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
 #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
 
+#include <vector>
+
 namespace hardware_interface
 {
 /// Constant defining position interface
@@ -25,6 +27,11 @@ constexpr char HW_IF_VELOCITY[] = "velocity";
 constexpr char HW_IF_ACCELERATION[] = "acceleration";
 /// Constant defining effort interface
 constexpr char HW_IF_EFFORT[] = "effort";
+
+// TODO(destogl): use "inline static const"-type when switched to C++17
+/// Definition of standard names for movement interfaces
+const std::vector<const char *> MOVEMENT_INTERFACES = {
+ HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, HW_IF_EFFORT};
 } // namespace hardware_interface
 
 #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_

hardware_interface/package.xml

@@ -9,6 +9,7 @@
 
  <buildtool_depend>ament_cmake</buildtool_depend>
 
+ <depend>backward_ros</depend>
  <depend>control_msgs</depend>
  <depend>lifecycle_msgs</depend>
  <depend>pluginlib</depend>

joint_limits/CMakeLists.txt

@@ -6,11 +6,14 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+ pluginlib
  rclcpp
  rclcpp_lifecycle
+ trajectory_msgs
 )
 
 find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
  find_package(${Dependency} REQUIRED)
 endforeach()
@@ -23,10 +26,41 @@ target_include_directories(joint_limits INTERFACE
 )
 ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+add_library(joint_limiter_interface SHARED
+ src/joint_limiter_interface.cpp
+)
+target_compile_features(joint_limiter_interface PUBLIC cxx_std_17)
+target_include_directories(joint_limiter_interface PUBLIC
+ $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include/joint_limits>
+)
+ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# 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 "JOINT_LIMITS_BUILDING_DLL")
+
+
+add_library(simple_joint_limiter SHARED
+ src/simple_joint_limiter.cpp
+)
+target_compile_features(simple_joint_limiter PUBLIC cxx_std_17)
+target_include_directories(simple_joint_limiter PUBLIC
+ $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include/joint_limits>
+)
+ament_target_dependencies(simple_joint_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# 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 "JOINT_LIMITS_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml)
 
 if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
+ find_package(ament_cmake_gmock REQUIRED)
  find_package(launch_testing_ament_cmake REQUIRED)
+ find_package(pluginlib REQUIRED)
+ find_package(rclcpp REQUIRED)
 
  ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp)
  target_link_libraries(joint_limits_rosparam_test joint_limits)
@@ -40,13 +74,25 @@ if(BUILD_TESTING)
  FILES test/joint_limits_rosparam.yaml
  DESTINATION share/joint_limits/test
  )
+
+ 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()
 
 install(
  DIRECTORY include/
  DESTINATION include/joint_limits
 )
-install(TARGETS joint_limits
+install(TARGETS
+ joint_limits
+ joint_limiter_interface
+ simple_joint_limiter
  EXPORT export_joint_limits
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib

joint_limits/include/joint_limits/joint_limiter_interface.hpp

@@ -0,0 +1,94 @@
+// 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 <string>
+#include <vector>
+
+#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 <typename LimitsType>
+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<std::string> 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<std::string> joint_names_;
+ std::vector<LimitsType> joint_limits_;
+ rclcpp::Node::SharedPtr node_;
+};
+
+} // namespace joint_limits
+
+#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_

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 <string>
+
+#include "joint_limits/joint_limiter_interface.hpp"
+#include "joint_limits/joint_limits.hpp"
+
+namespace joint_limits
+{
+template <typename LimitsType>
+class SimpleJointLimiter : public JointLimiterInterface<JointLimits>
+{
+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_

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_

joint_limits/joint_limiters.xml

@@ -0,0 +1,9 @@
+<library path="simple_joint_limiter">
+ <class name="joint_limits/SimpleJointLimiter"
+ type="joint_limits::SimpleJointLimiter&lt;joint_limits::JointLimits&gt;"
+ base_class_type="joint_limits::JointLimiterInterface&lt;joint_limits::JointLimits&gt;">
+ <description>
+ Simple joint limiter using clamping approach.
+ </description>
+ </class>
+</library>

joint_limits/package.xml

@@ -1,10 +1,10 @@
 <package format="3">
  <name>joint_limits</name>
  <version>3.9.1</version>
- <description>Interfaces for handling of joint limits for controllers or hardware.</description>
+ <description>Package for with interfaces for handling of joint limits for use in controllers or in hardware. The package also implements a simple, default joint limit strategy by clamping the values.</description>
 
  <maintainer email="[email protected]">Bence Magyar</maintainer>
- <maintainer email="denis@stogl.de">Denis Štogl</maintainer>
+ <maintainer email="denis.stogl@stoglrobotics.de">Denis Štogl</maintainer>
 
  <license>Apache License 2.0</license>
 
@@ -14,11 +14,15 @@
 
  <buildtool_depend>ament_cmake</buildtool_depend>
 
+ <depend>backward_ros</depend>
+ <depend>pluginlib</depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_lifecycle</depend>
+ <depend>trajectory_msgs</depend>
 
- <test_depend>launch_testing_ament_cmake</test_depend>
+ <test_depend>ament_cmake_gmock</test_depend>
  <test_depend>ament_cmake_gtest</test_depend>
+ <test_depend>launch_testing_ament_cmake</test_depend>
 
  <export>
  <build_type>ament_cmake</build_type>