Feature/Add Joint Impedance Controller

flexiv_bringup/config/rizon_controllers.yaml

@@ -33,3 +33,23 @@ position_joint_trajectory_controller:
   stop_trajectory_duration: 0.5
   state_publish_rate: 100.0
   action_monitor_rate: 20.0
+
+joint_impedance_controller:
+  type: flexiv_controllers/JointImpedanceController
+  joints: *robot_joints
+  k_p:
+    - 3000.0
+    - 3000.0
+    - 800.0
+    - 800.0
+    - 200.0
+    - 200.0
+    - 200.0
+  k_d:
+    - 80.0
+    - 80.0
+    - 40.0
+    - 40.0
+    - 8.0
+    - 8.0
+    - 8.0

flexiv_bringup/package.xml

@@ -11,6 +11,7 @@
 
   <exec_depend>flexiv_description</exec_depend>
   <exec_depend>flexiv_hardware</exec_depend>
+  <exec_depend>flexiv_controllers</exec_depend>
   <exec_depend>xacro</exec_depend>
 
 </package>

flexiv_controllers/CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.5)
+project(flexiv_controllers)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(catkin REQUIRED COMPONENTS
+  controller_interface
+  controller_manager
+  flexiv_msgs
+  hardware_interface
+  joint_limits_interface
+  pluginlib
+  realtime_tools
+  roscpp
+  urdf
+)
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  CATKIN_DEPENDS
+    controller_interface
+    controller_manager
+    flexiv_msgs
+    hardware_interface
+    joint_limits_interface
+    pluginlib
+    realtime_tools
+    roscpp
+    urdf
+  LIBRARIES
+    ${PROJECT_NAME}
+)
+
+# Build
+add_library(${PROJECT_NAME}
+  src/joint_impedance_controller.cpp
+)
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${catkin_INCLUDE_DIRS}
+)
+
+target_link_libraries(${PROJECT_NAME} PRIVATE ${catkin_LIBRARIES})
+
+# INSTALL
+# Install targets
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Install headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+# Install plugins
+install(FILES flexiv_controllers_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

flexiv_controllers/flexiv_controllers_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libflexiv_controllers">
+  <class name="flexiv_controllers/JointImpedanceController" type="flexiv_controllers::JointImpedanceController" base_class_type="controller_interface::ControllerBase">
+      <description>
+        The joint impedance controller commands a group of joints in effort interface.
+      </description>
+  </class>
+</library>

flexiv_controllers/include/flexiv_controllers/joint_impedance_controller.h

@@ -0,0 +1,76 @@
+/**
+ * @file joint_impedance_controller.hpp
+ * Joint impedance control as ROS controller. Adapted from
+ * ros_controllers/effort_controllers
+ * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#ifndef FLEXIV_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_
+#define FLEXIV_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+// ROS
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <ros/node_handle.h>
+#include <flexiv_msgs/JointPosVel.h>
+#include <ros/time.h>
+#include <urdf/model.h>
+
+// Flexiv
+
+namespace flexiv_controllers {
+
+class JointImpedanceController : public controller_interface::Controller<
+                                     hardware_interface::EffortJointInterface>
+{
+public:
+    struct Commands
+    {
+        std::array<double, 7> positions_; // Last commanded position
+        std::array<double, 7> velocities_; // Last commanded velocity
+        bool has_velocity_; // false if no velocity command has been specified
+    };
+
+    JointImpedanceController() = default;
+    ~JointImpedanceController();
+
+    bool init(
+        hardware_interface::EffortJointInterface* hw, ros::NodeHandle& nh);
+
+    void update(const ros::Time& time, const ros::Duration& period);
+
+    void starting(const ros::Time& time);
+
+    void setCommand(std::array<double, 7> pos_command);
+
+    void setCommand(
+        std::array<double, 7> pos_command, std::array<double, 7> vel_command);
+
+private:
+    std::vector<hardware_interface::JointHandle> joints_;
+    std::vector<urdf::JointConstSharedPtr> joints_urdf_;
+    std::vector<double> k_p_;
+    std::vector<double> k_d_;
+    static const size_t num_joints_ = 7;
+
+    // Commands
+    realtime_tools::RealtimeBuffer<Commands> command_;
+    Commands command_struct_;
+    ros::Subscriber sub_command_;
+
+    void setCommandCB(const flexiv_msgs::JointPosVelConstPtr& msg);
+
+    void enforceJointLimits(int joint_index, double& pos_command);
+};
+
+} // namespace flexiv_controllers
+
+#endif // FLEXIV_CONTROLLERS__JOINT_IMPEDANCE_CONTROLLER_HPP_

flexiv_controllers/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>flexiv_controllers</name>
+  <version>0.7.0</version>
+  <description>Flexiv custom ROS controllers</description>
+  <maintainer email="[email protected]">Mun Seng Phoon</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Mun Seng Phoon</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>controller_interface</build_depend>
+  <build_depend>controller_manager</build_depend>
+  <build_depend>hardware_interface</build_depend>
+  <build_depend>joint_limits_interface</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>realtime_tools</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>urdf</build_depend>
+  <build_depend>flexiv_msgs</build_depend>
+
+  <exec_depend>controller_interface</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>hardware_interface</exec_depend>
+  <exec_depend>joint_limits_interface</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>realtime_tools</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>flexiv_msgs</exec_depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/flexiv_controllers_plugin.xml"/>
+  </export>
+</package>