[WIP] Velocity Based Cartesian Controller for Arm

.github/workflows/ci.yaml

@@ -15,6 +15,7 @@ env:
     uwrt_mars_rover_drivetrain
     uwrt_mars_rover_drivetrain_description
     uwrt_mars_rover_drivetrain_hw
+    uwrt_arm_cartesian_controller
   ROS_DISTRO: galactic
   IMAGE: "rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-desktop-latest"
 

uwrt_arm_cartesian_controller/CMakeLists.txt

@@ -0,0 +1,133 @@
+cmake_minimum_required(VERSION 3.5)
+project(uwrt_arm_cartesian_controller)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(orocos_kdl REQUIRED)
+find_package(realtime_tools REQUIRED)
+
+
+add_library(uwrt_arm_cartesian_controller
+  SHARED
+  src/uwrt_arm_cartesian_controller.cpp
+)
+target_include_directories(uwrt_arm_cartesian_controller PRIVATE include)
+ament_target_dependencies(uwrt_arm_cartesian_controller
+  builtin_interfaces
+  controller_interface
+  hardware_interface
+  pluginlib
+  rclcpp_lifecycle
+  rcutils
+  geometry_msgs
+  orocos_kdl
+  realtime_tools
+)
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(uwrt_arm_cartesian_controller PRIVATE "UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_LIBRARY")
+pluginlib_export_plugin_description_file(controller_interface uwrt_arm_cartesian_controller_plugin.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS
+  uwrt_arm_cartesian_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+if(FALSE) # younes todo add unit testing like in the forwarding controller example BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(
+    test_load_uwrt_arm_cartesian_controller
+    test/test_load_uwrt_arm_cartesian_controller.cpp
+  )
+  target_include_directories(test_load_uwrt_arm_cartesian_controller PRIVATE include)
+  ament_target_dependencies(test_load_uwrt_arm_cartesian_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  ament_add_gmock(
+    test_uwrt_arm_cartesian_controller
+    test/test_uwrt_arm_cartesian_controller.cpp
+  )
+  target_include_directories(test_uwrt_arm_cartesian_controller PRIVATE include)
+  target_link_libraries(test_uwrt_arm_cartesian_controller
+    uwrt_arm_cartesian_controller
+  )
+endif()
+
+if (BUILD_TESTING)
+   # Force generation of compile_commands.json for clang-tidy
+   set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE INTERNAL "")
+
+   # clang-format
+   find_package(ament_cmake_clang_format REQUIRED)
+   ament_clang_format(
+   CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.clang-format
+   )
+
+   # clang-tidy
+   find_package(ament_cmake_clang_tidy REQUIRED)
+   ament_clang_tidy(
+   ${CMAKE_BINARY_DIR}
+   CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.clang-tidy
+   )
+
+   # cppcheck
+   find_package(ament_cmake_cppcheck REQUIRED)
+   ament_cppcheck()
+
+   # flake8
+   find_package(ament_cmake_flake8 REQUIRED)
+   ament_flake8(
+   CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.flake8
+   )
+
+   # xmllint
+   find_package(ament_cmake_xmllint REQUIRED)
+   ament_xmllint()
+endif ()
+
+ament_export_dependencies(
+  controller_interface
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+  geometry_msgs
+  orocos_kdl
+  realtime_tools
+)
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  uwrt_arm_cartesian_controller
+)
+ament_package()

uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/uwrt_arm_cartesian_controller.hpp

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <memory>
+
+#include "controller_interface/controller_interface.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "kdl/chainiksolver.hpp"
+#include "kdl/chainiksolvervel_pinv.hpp"
+#include "kdl/tree.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "uwrt_arm_cartesian_controller/visibility_control.hpp"
+
+namespace uwrt_arm_cartesian_controller
+{
+using TwistStamped = geometry_msgs::msg::TwistStamped;
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class UWRTCartesianController : public controller_interface::ControllerInterface
+{
+public:
+  UWRTCartesianController();
+
+  ~UWRTCartesianController();
+
+  UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+  CallbackReturn on_init() override;
+
+  UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  rclcpp::Subscription<TwistStamped>::SharedPtr m_cart_command_sub;
+
+  std::vector<std::string> m_joint_names;
+  std::string m_cart_command_topic, m_robot_description, m_root_name, m_tip_name;
+
+private:
+  bool parse_params();
+  TwistStamped get_null_twist() const;
+
+  KDL::Tree m_kdl_tree;
+  KDL::Chain m_kdl_chain;
+  std::unique_ptr<KDL::ChainIkSolverVel> m_ik_solver;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<TwistStamped>> m_rt_buffer;
+
+  static constexpr auto COMMAND_INTERFACE = "velocity";
+  static constexpr auto STATE_INTERFACE = "position";
+};
+
+}  // namespace uwrt_arm_cartesian_controller

uwrt_arm_cartesian_controller/include/uwrt_arm_cartesian_controller/visibility_control.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+// 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 UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT __attribute__((dllexport))
+#define UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT __attribute__((dllimport))
+#else
+#define UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT __declspec(dllexport)
+#define UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT __declspec(dllimport)
+#endif
+#ifdef UWRT_ARM_CARTESIAN_CONTROLLER_BUILDING_LIBRARY
+#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT
+#else
+#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT
+#endif
+#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC_TYPE UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+#define UWRT_ARM_CARTESIAN_CONTROLLER_LOCAL
+#else
+#define UWRT_ARM_CARTESIAN_CONTROLLER_EXPORT __attribute__((visibility("default")))
+#define UWRT_ARM_CARTESIAN_CONTROLLER_IMPORT
+#if __GNUC__ >= 4
+#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC __attribute__((visibility("default")))
+#define UWRT_ARM_CARTESIAN_CONTROLLER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC
+#define UWRT_ARM_CARTESIAN_CONTROLLER_LOCAL
+#endif
+#define UWRT_ARM_CARTESIAN_CONTROLLER_PUBLIC_TYPE
+#endif

uwrt_arm_cartesian_controller/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>uwrt_arm_cartesian_controller</name>
+  <version>1.0.0</version>
+  <description>The uwrt_arm_cartesian_controller converts velocity commands expressed in cartesian coordinates to joint velocities</description>
+  <maintainer email="[email protected]">Younes Reda</maintainer>
+
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>orocos_kdl</depend>
+  <depend>geometry_msgs</depend>
+
+  <build_depend>pluginlib</build_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_clang_format</test_depend>
+  <test_depend>ament_cmake_clang_tidy</test_depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_flake8</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>