hardware_interface_adapter design in joint_trajectory_controller

if we want to connect hardware_interface and joint_trajectory_controller, we can customize hardware_interface_adapter template class.

template <class HardwareInterface, class State>
class HardwareInterfaceAdapter
{
public:
  bool init(std::vector<typename HardwareInterface::ResourceHandleType>& /*joint_handles*/, ros::NodeHandle& /*controller_nh*/)
  {
    return false;
  }

  void starting(const ros::Time& /*time*/) {}
  void stopping(const ros::Time& /*time*/) {}

  void updateCommand(const ros::Time&     /*time*/,
                     const ros::Duration& /*period*/,
                     const State&         /*desired_state*/,
                     const State&         /*state_error*/) {}
};

In this class, the last input of updateCommand function is state_error. I think it should be just current_state. state_error argument is limited for pid controller. for example, if we're going to use gravity compensation + pid controller, we need current_state and state_error. state_error can be calculated from desired_state and current_state. In order to generalize hardware_interface_adapter for general controller, I think we should change the last argument from state_error to current_state.

[Modify] Actually, the above issue was not a big problem, because I can get current_state from joint handles. But, more serious problem is ,in hardware_interface_adapter.h, ClosedLoopHardwareInterfaceAdapter and class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State> : public ClosedLoopHardwareInterfaceAdapter are limited for pid controller.

I want to implement other controllers such as gravity compensation controller, computed-torque controller, passivity-based controller which is necessary to control serial-chain arm through effort hardwareinterface. I tried to do this not by fixing hardware_interface_adapter.h directly, but writing custom-code outside joint-trajectory codes, but since effort hardware interface adapter inherited from ClosedLoopHardwareInterfaceAdapter is limited for pid controller, I failed.

Is there simple way that I can write custom controller code upon this joint-trajectory controller module? It was possible to write custom controller code referencing effort_controllers. (https://github.com/ros-controls/ros_control/wiki/controller_interface) But, with joint_trajectory_controller, I cannot find a way to write custom controller easily.

I tried to do the very same and just wanted to document the solution here for reference...

The way that the joint trajectory controller works in ROS 1 is outlined here:

The controller is templated to work with multiple hardware interface types. Currently joints with position, velocity and effort interfaces are supported. For position-controlled joints, desired positions are simply forwarded to the joints; while for velocity (effort) joints, the position+velocity trajectory following error is mapped to velocity (effort) commands through a PID loop. Example controller configurations can be found here.

For example the effort_controllers::JointTrajectoryController is a short-hand notation for the fully specialized C++ template joint_trajectory_controller::JointTrajectoryController< trajectory_interface::QuinticSplineSegment<double>, hardware_interface::EffortJointInterface> (see here). At the very end of the update loop the set-point is mapped through the HwIfaceAdapter<HardwareInterface, Segment::State> to a point that matches the HardwareInterface. The partial specialization for effort can be found here and is basically just a ClosedLoopHardwareInterfaceAdapter<State> which performs a simple PID loop for mapping the position set-point to effort here.

Anyone wishing to implement a custom JointTrajectoryController can therefore re-use most of the ros_control infrastructure to do so. All they have to do is implement their own HardwareInterfaceAdapter. The main problem is integrating it into the rest of the code base as the following line hard codes the mapping for the known interfaces:

https://github.com/ros-controls/ros_controllers/blob/c2348e85abf35cf33cf654a84d61db206abe9a73/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h#L178

There are the following three options of implementation that come to my mind:

The probably best option is to fully specialize the class template template <class HardwareInterface, class State> HardwareInterfaceAdapter as e.g. HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, joint_trajectory_controller::JointTrajectorySegment<trajectory_interface::QuinticSplineSegment<double> >::State> (see e.g. here). This is more specific than the partial specialization template <typename State> HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State> and therefore is used by the compiler when instantiating the controller. In my case I defined my custom hardware interface adapter as:

// This is needed as we only want to specialize the effort implementation
template <typename HardwareInterface, typename State>
class CustomHardwareInterfaceAdapter;

template <typename State>
class CustomHardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State> {
public:
  CustomHardwareInterfaceAdapter();
  CustomHardwareInterfaceAdapter(CustomHardwareInterfaceAdapter const&) = default;
  CustomHardwareInterfaceAdapter& operator= (CustomHardwareInterfaceAdapter const&) = default;
  CustomHardwareInterfaceAdapter(CustomHardwareInterfaceAdapter&&) = default;
  CustomHardwareInterfaceAdapter& operator= (CustomHardwareInterfaceAdapter&&) = default;

  [[nodiscard]]
  bool init(std::vector<hardware_interface::JointHandle>& joint_handles,
            ros::NodeHandle& controller_nh);

  void starting(ros::Time const& /*time*/);

  void stopping(ros::Time const& /*time*/);

  void updateCommand(ros::Time const& /*time*/, ros::Duration const& period,
                   State const& desired_state, State const& /*state_error*/);

protected:
  std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
  // Other class members
};

and then before instantiating the controller I created the fully specialized class template inheriting from my implementation:

using TrajectoryState = joint_trajectory_controller::JointTrajectorySegment<
trajectory_interface::QuinticSplineSegment<double>
>::State;
template <>
class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface,TrajectoryState>: public
  custom_controllers::CostumHardwareInterfaceAdapter<
    hardware_interface::EffortJointInterface,TrajectoryState
  > {
public:
  HardwareInterfaceAdapter() = default;
  HardwareInterfaceAdapter(HardwareInterfaceAdapter const&) = default;
  HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter const&) = default;
  HardwareInterfaceAdapter(HardwareInterfaceAdapter&&) = default;
  HardwareInterfaceAdapter& operator= (HardwareInterfaceAdapter&&) = default;
};

What should work as well (see https://github.com/ros-controls/ros_controllers/issues/125#issuecomment-263287821, this commit as well as elfin_robot) is to implement an interface custom_controllers::EffortJointInterface that basically inherits from hardware_interface::EffortJointInterface but being derived from it, one can provide a specialization for template <typename State> HardwareInterfaceAdapter<custom_controllers::EffortJointInterface, State>. From what I understood one would have then to use the custom_controllers::EffortJointInterface in the corresponding hardware interface as well which might be impractical as it closely couples controller and hardware interface.
The easiest and most flexible way that I see involves modifying the original ros_control repository (meaning that your code won't compile when installing ros_control from Debian packages), adding an additional template parameter Adapter with the default value HardwareInterfaceAdapter to the JointTrajectoryController: template <class SegmentImpl, class HardwareInterface, template <class HW, class S> class Adapter = HardwareInterfaceAdapter> class JointTrajectoryController.

ros-controls / ros_controllers

hardware_interface_adapter design in joint_trajectory_controller #396