Ahcorde/add/joint limit interface

joint_limits_interface/CMakeLists.txt

@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.5.0)
+project(joint_limits_interface)
+
+find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(urdf REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
+
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp)
+  target_include_directories(joint_limits_interface_test PUBLIC include)
+  ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp)
+
+  add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp)
+  target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS})
+  target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES})
+  ament_target_dependencies(joint_limits_rosparam_test rclcpp)
+  add_launch_test(test/joint_limits_rosparam.launch.py)
+  install(
+    TARGETS
+    joint_limits_rosparam_test
+    DESTINATION lib/${PROJECT_NAME}
+  )
+  install(
+    FILES
+    test/joint_limits_rosparam.yaml
+    DESTINATION share/${PROJECT_NAME}/test
+  )
+
+  ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp)
+  target_include_directories(joint_limits_urdf_test PUBLIC include)
+  ament_target_dependencies(joint_limits_urdf_test rclcpp)
+endif()
+
+# Install headers
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_dependencies(rclcpp urdf)
+
+ament_package()

joint_limits_interface/README.md

@@ -0,0 +1,30 @@
+## Joint Limits Interface ##
+
+### Overview ###
+
+**joint_limits_interface** contains data structures for representing joint limits, methods for populating them from common
+formats such as URDF and the ROS parameter server, and methods for enforcing limits on different kinds of hardware interfaces.
+
+The **joint_limits_interface** is *not* used by controllers themselves (it does not implement a `HardwareInterface`) but
+instead operates after the controllers have updated, in the `write()` method (or equivalent) of the robot abstraction.
+Enforcing limits will *overwrite* the commands set by the controllers, it does not operate on a separate raw data buffer.
+
+There are two main elements involved in setting up a joint limits interface:
+
+ - **Joint limits representation**
+   - **JointLimits** Position, velocity, acceleration, jerk and effort.
+   - **SoftJointLimits** Soft position limits, `k_p`, `k_v` (as described [here](http://www.ros.org/wiki/pr2_controller_manager/safety_limits)).
+   - **Loading from URDF** There are convenience methods for loading joint limits information
+     (position, velocity and effort), as well as soft joint limits information from the URDF.
+   - **Loading from ROS params** There are convenience methods for loading joint limits from the ROS parameter server
+     (position, velocity, acceleration, jerk and effort). Parameter specification is the same used in MoveIt,
+     with the addition that we also parse jerk and effort limits.
+
+ - **Joint limits interface**
+
+  - For **effort-controlled** joints, the soft-limits implementation from the PR2 has been ported.
+  - For **position-controlled** joints, a modified version of the PR2 soft limits has been implemented.
+  - For **velocity-controlled** joints, simple saturation based on acceleration and velocity limits has been implemented.
+
+### Examples ###
+Please refer to the  [joint_limits_interface](https://github.com/ros-controls/ros_control/wiki/joint_limits_interface) wiki page.

joint_limits_interface/include/joint_limits_interface/joint_limits.hpp

@@ -0,0 +1,90 @@
+// Copyright 2013, PAL Robotics S.L.
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of the copyright holders nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/// \author Adolfo Rodriguez Tsouroukdissian
+
+#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_
+#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_
+
+namespace joint_limits_interface
+{
+
+struct JointLimits
+{
+  JointLimits()
+  : min_position(0.0),
+    max_position(0.0),
+    max_velocity(0.0),
+    max_acceleration(0.0),
+    max_jerk(0.0),
+    max_effort(0.0),
+    has_position_limits(false),
+    has_velocity_limits(false),
+    has_acceleration_limits(false),
+    has_jerk_limits(false),
+    has_effort_limits(false),
+    angle_wraparound(false)
+  {}
+
+  double min_position;
+  double max_position;
+  double max_velocity;
+  double max_acceleration;
+  double max_jerk;
+  double max_effort;
+
+  bool has_position_limits;
+  bool has_velocity_limits;
+  bool has_acceleration_limits;
+  bool has_jerk_limits;
+  bool has_effort_limits;
+  bool angle_wraparound;
+};
+
+struct SoftJointLimits
+{
+  SoftJointLimits()
+  : min_position(0.0),
+    max_position(0.0),
+    k_position(0.0),
+    k_velocity(0.0)
+  {}
+
+  double min_position;
+  double max_position;
+  double k_position;
+  double k_velocity;
+};
+
+}  // namespace joint_limits_interface
+
+#endif  // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_