ROS control-----transmission interface

最新推荐文章于 2023-06-30 09:51:09 发布

倔强不倒翁

最新推荐文章于 2023-06-30 09:51:09 发布

阅读量2k

点赞数 4

分类专栏： ROS 文章标签： ros_control transimission_inface

本文链接：https://blog.csdn.net/x_r_su/article/details/53261843

版权

ROS 专栏收录该内容

68 篇文章 24 订阅

订阅专栏

transmission_interface中有表示机构传输的数据结构，也有填充actuator和joint空间中position, velocity and effort变量的函数接口。

1 重要数据结构

1.1 transmission interface

1.2 transmission handle

1.3 transmission

该结构主要实现joint和actuator之间effort, velocity and position的双向映射。

（1）)SimpleTransmission

通常是通过带轮或者齿轮将执行器和关节连接，如下图所示

（2）DifferentialTransmission

通常通过如下差速机构将2个执行器和2个关节相连，如下图所示：

（3） FourBarLinkageTransmission

这种传输2个执行器和2个关节通过某种特定机构连接（第1个关节仅依赖第1个执行器，第2个关节仅仅依赖第2个执行器）。

2 代码分析

2.1 简单示例

#include <vector>

#include <transmission_interface/simple_transmission.h>
#include <transmission_interface/differential_transmission.h>
#include <transmission_interface/transmission_interface.h>

using std::vector;
using namespace transmission_interface;

class MyRobot
{
public:
  MyRobot()
   : sim_trans(-10.0,  // 10x reducer. Negative sign: actuator and joint spin in opposite directions
                 1.0), // joint position offset

     dif_trans(vector<double>(2, 5.0), // 5x reducer on each actuator
               vector<double>(2, 1.0)) // No reducer in joint output
  {
  
    /// Wrapping the raw data is the most verbose part of the setup process... /

    // Wrap simple transmission raw data - current state
    a_state_data[0].position.push_back(&a_curr_pos[0]);
    a_state_data[0].velocity.push_back(&a_curr_vel[0]);
    a_state_data[0].effort.push_back(&a_curr_eff[0]);

    j_state_data[0].position.push_back(&j_curr_pos[0]);
    j_state_data[0].velocity.push_back(&j_curr_vel[0]);
    j_state_data[0].effort.push_back(&j_curr_eff[0]);

    // Wrap simple transmission raw data - position command
    a_cmd_data[0].position.push_back(&a_cmd_pos[0]); // Velocity and effort vectors are unused

    j_cmd_data[0].position.push_back(&j_cmd_pos[0]); // Velocity and effort vectors are unused

 
   // Wrap differential transmission raw data - current state
    a_state_data[1].position.push_back(&a_curr_pos[1]); a_state_data[1].position.push_back(&a_curr_pos[2]);
    a_state_data[1].velocity.push_back(&a_curr_vel[1]); a_state_data[1].velocity.push_back(&a_curr_vel[2]);
    a_state_data[1].effort.push_back(&a_curr_eff[1]);   a_state_data[1].effort.push_back(&a_curr_eff[2]);

    j_state_data[1].position.push_back(&j_curr_pos[1]); j_state_data[1].position.push_back(&j_curr_pos[2]);
    j_state_data[1].velocity.push_back(&j_curr_vel[1]); j_state_data[1].velocity.push_back(&j_curr_vel[2]);
    j_state_data[1].effort.push_back(&j_curr_eff[1]);   j_state_data[1].effort.push_back(&j_curr_eff[2]);

    // Wrap differential transmission raw data - position command
    a_cmd_data[1].position.push_back(&a_cmd_pos[1]); a_cmd_data[1].position.push_back(&a_cmd_pos[2]);

    j_cmd_data[1].position.push_back(&j_cmd_pos[1]); j_cmd_data[1].position.push_back(&j_cmd_pos[2]);

    // ...once the raw data has been wrapped, the rest is straightforward //

    // Register transmissions to each transmission interface
    act_to_jnt_state.registerHandle(ActuatorToJointStateHandle("sim_trans",
                                                               &sim_trans,
                                                               a_state_data[0],
                                                               j_state_data[0]));

    act_to_jnt_state.registerHandle(ActuatorToJointStateHandle("dif_trans",
                                                              &dif_trans,
                                                              a_state_data[1],
                                                              j_state_data[1]));

    jnt_to_act_pos.registerHandle(JointToActuatorPositionHandle("sim_trans",
                                                                &sim_trans,
                                                                a_cmd_data[0],
                                                                j_cmd_data[0]));

    jnt_to_act_pos.registerHandle(JointToActuatorPositionHandle("dif_trans",
                                                                &dif_trans,
                                                                a_cmd_data[1],
                                                                j_cmd_data[1]));

    // Names must be unique within a single transmission interface, but a same name can be used in
    // multiple interfaces, as shown above
  }

  void read()
  {
    // Read actuator state from hardware
    // ...

    // Propagate current actuator state to joints
    act_to_jnt_state.propagate();
  }

  void write()
  {
    // Porpagate joint commands to actuators
    jnt_to_act_pos.propagate();

    // Send actuator command to hardware
    // ...
  }

private:

  // Transmission interfaces
  ActuatorToJointStateInterface    act_to_jnt_state; // For propagating current actuator state to joint space
  JointToActuatorPositionInterface jnt_to_act_pos;   // For propagating joint position commands to actuator space

  // Transmissions
  SimpleTransmission       sim_trans;
  DifferentialTransmission dif_trans;

  // Actuator and joint space variables: wrappers around raw data
  ActuatorData a_state_data[2]; // Size 2: One per transmission
  ActuatorData a_cmd_data[2];

  JointData j_state_data[2];
  JointData j_cmd_data[2];

  // Actuator and joint space variables - raw data:
  // The first actuator/joint are coupled through a reducer.
  // The last two actuators/joints are coupled through a differential.
  double a_curr_pos[3]; // Size 3: One per actuator
  double a_curr_vel[3];
  double a_curr_eff[3];
  double a_cmd_pos[3];

  double j_curr_pos[3]; // Size 3: One per joint
  double j_curr_vel[3];
  double j_curr_eff[3];
  double j_cmd_pos[3];
};

以上示例比较简单，数据方面：

1）是传输模块两侧要转化的joint raw data和actuator raw data，以及把它们封装成适合模块用的传输数据ActuatorData&&JointData；

2）是传输接口ActuatorToJointStateInterface和JointToActuatorPositionInterface；

3）是传输类型SimpleTransmission和DifferentialTransmission；

处理流程方面：

1）调用MyRobot构造函数时候，完成raw data数据格式化，传输接口注册handle（就是注册resource），传输转化比的初始化；

2）在read/write接口做进行数据的tramsimission转化；

2.2 更复杂示例

请参考：https://github.com/ros-controls/ros_control/blob/kinetic-devel/transmission_interface/test/transmission_interface_loader_test.cpp

中的TEST_F(TransmissionInterfaceLoaderTest, SuccessfulLoad)，这个流程大概如下：

1）传输信息放到URDF文件

主要有：传输（name，type）；joint（name，offset，hardwareinterface）；actuator（name，reduction）

2）解析URDF文件，获取那些文件中信息

3）加载传输信息，完成执行器与关键的映射

这一步以下几点加以说明：

a）以插件方式将传输类型插件（transmission_interface::SimpleTransmissionLoader或transmission_interface::DifferentialTransmissionLoader）加载实现创建传输；

b）以插件方式完成传输接口handle初始化，并把handle注册到传输接口。插件包括（transmission_interface::PositionJointInterfaceProvider，transmission_interface::VelocityJointInterfaceProvider，transmission_interface::EffortJointInterfaceProvider）；

把handle注册到传输的函数是RequisiteProvider::loadTransmissionMaps。

c） TransmissionInterfaceLoader类构造函数完成了2个加载器的初始化，用以加载传输类型插件和传输接口handle注册；

transmission_class_loader_.reset(new TransmissionClassLoader("transmission_interface",
"transmission_interface::TransmissionLoader"));

req_provider_loader_.reset(new RequisiteProviderClassLoader("transmission_interface",

"transmission_interface::RequisiteProvider"));

d）TransmissionInterfaceLoader类成员函数load，实例化插件并使用插件中接口；

e）使用加载器方式需注意

4）根据需要调用传输接口进行数据转化，

//Propagate state forward（fake）
  act_pos.assign(3,  50.0);
  act_vel.assign(3, -50.0);
  act_eff.assign(3,   1.0);

act_to_jnt_state->propagate();

// Propagate position commands forward

pos_jnt_handle_red.setCommand(1.5)

pos_jnt_handle_diff1.setCommand(1.5);

pos_jnt_handle_diff2.setCommand(0.5);

jnt_to_act_pos_cmd->propagate();

5）示例

class FooRobot : public hardware_interface::RobotHW
 {
 public:
   void read(ros::Time time, ros::Duration period);
   void write(ros::Time time, ros::Duration period);

   bool init()
   {
     // Populate robot_hw with actuator interfaces (e.g., EffortActuatorInterface)
     // This is hardware-specific, and not detailed here
     // ...

     // Initialize transmission loader
     try
     {
       transmission_loader_.reset(new TransmissionInterfaceLoader(this, &robot_transmissions));
     }
     catch(const std::invalid_argument& ex)
     {
       ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what());
       return false;
     }
     catch(const pluginlib::LibraryLoadException& ex)
     {
       ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what());
       return false;
     }
     catch(...)
     {
       ROS_ERROR_STREAM("Failed to create transmission interface loader. ");
       return false;
     }

     std::string robot_description;
     // ...load URDF from parameter server or file

     // Perform actual transmission loading
     if (!transmission_loader_->load(robot_description)) {return false;}

    // We can now query for any of the created interfaces, for example:
    // robot_transmissions_.get<ActuatorToJointStateInterface>();
    // this->get<JointStateInterface>();

    return true;
  }

 private:
   RobotTransmissions robot_transmissions_;
   boost::scoped_ptr<TransmissionInterfaceLoader> transmission_loader_;

 };

3 参考文献

以上部分代码分析来自ROS wiki