Integrating transmissions in ros_control using URDF

edit

Let me share my experience about using the TransmissionInterfaceLoader. First, let me talk about the handles. For each joint or actuator, there are two handles directly related to them. One is the state handle which stores the states (position, vel, effort), and the type is JointStateHandle or ActuatorStateHandle. The other is the command handle, which has getCommand() and setCommand() methods, and the type is JointHandle or ActuatorHandle.

For the TransmissionInterfaceLoader, it reads the URDF file and automatically creates the Actuator command (only) interface, the Joint state/command interface, and the state/command transmissions, and all the related handles of course.

Usage: As users, we only need to manually create the ActuatorStateHandle for each actuator and the ActuatorStateInterface. And populate it using registerInterface() function to the RobotHW. Then the TransmissionInterfaceLoader will load other interfaces for us.

Note: We can manually populate the Actuator command interfaces and Joint command interfaces, although not required. But DO NOT populate a JointStateInterface. It will cause error (at least in Kinetic). Link here.

The reason for manually populating the actuator state interfaces is (in my understanding): in the RobotHW, we need to implement the method to get actuator states from the hardware. With the manually created actuator state handles, we can manually update the data linked to the state handles. And that is the only way to set the states of a state handle.

About your questions. I cannot answer your first one due to lack of information. About your second question, we only need to populate the actuator state interface using the actuator names. The reason for this is explained above. The other interfaces optional (except JointStateInterface will cause error).

I'm attaching my miniature code here for someone who wants to use the TransmissionInterfaceLoader. It is incomplete, and some variable declarations may be missing.

Header:

class PhenobotHW : public hardware_interface::RobotHW
{
public:
    /**
    * Create hardware instance, and read parameters
    */
    PhenobotHW(ros::NodeHandle nh, ros::NodeHandle private_nh, double target_control_freq);

    // Connect hardware, initialize hardware interface functionalities. 
    bool initializeHardware();

    void updateJointsFromHardware();
    void writeCommandsToHardware();
private:
    // Register Actuator interfaces with the RobotHW interface manager, for transmissions
    void registerActuatorInterfaces();

    // Load transmission information from URDF
    bool loadTransmissions();
   // ROS Control Actuator interfaces
    hardware_interface::ActuatorStateInterface act_state_interface_;
    hardware_interface::VelocityActuatorInterface velocity_act_interface_;  // Optional
    // Transmission interfaces
    transmission_interface::RobotTransmissions transmissions_;
    transmission_interface::ActuatorToJointStateInterface* p_act_to_jnt_stat_;     // For Joint states
    transmission_interface::JointToActuatorVelocityInterface* p_jnt_to_act_vel_;   // For velocity commands

    /**
    * Joint structure that is hooked to ros_control's InterfaceManager, to allow control via the controllers
    */
    struct Joint
    {
      double position;
      double velocity;
      double effort;
      double command;

      Joint() :
        position(0), velocity(0), effort(0), command(0)
      { }
    }; 
    Joint actuators_[3]; 
}

Source:

PhenobotHW::PhenobotHW(ros::NodeHandle nh, ros::NodeHandle private_nh, double target_control_freq)
    :
    nh_(nh),
    private_nh_(private_nh)
{}

bool PhenobotHW::initializeHardware()
{
  // Register interfaces with the RobotHW interface manager, allowing ros_control operation
  registerActuatorInterfaces();
  // Load transmission information from URDF
  loadTransmissions();
  return true;
}

 /**
  * Register Joint interfaces with the RobotHW interface manager, for transmissions
  */
  void PhenobotHW::registerActuatorInterfaces()
  {
    using namespace hardware_interface;
    // Head wheel actuator
    ActuatorStateHandle act_state_handle_head("head_wheel_motor",
                    &actuators_[0].position, &actuators_[0].velocity, &actuators_[0].effort);
    act_state_interface_.registerHandle(act_state_handle_head);

    // Command handles are optional
    ActuatorHandle act_handle_head(
                    act_state_handle_head, &actuators_[0].command ...