1. 说在前面

在我上一篇博客上, 简单介绍了关于URDF和XACRO的东西. 利用现有的模型, 是能够很容易建立出一个现成的模型. 这里, 直接使用上一篇博客中提到的UR5 + Barrett(为了方便, 后面我都称之为ur5bh). 这篇博客的目的本来是想利用Gazebo + ROS Control + MoveIt! 让模型动起来, 但是在编辑的过程中, 为了让所有事情变得更清晰, 最终导致博客太长了, 预计分作几篇博客来完成这件事.

回顾:
第一篇 – 机器人描述: http://blog.csdn.net/sunbibei/article/details/52297524

本篇作为第二篇, 主要介绍在机器人描述文件中, 关于transmission相关的一些内容, 以及怎么将一个模型导入到Gazebo中.

该系列, 主要是以具体任务为向导, 将我所了解到的一些东西分享出来, 尽可能的让所有事情知其所以然. 如果有误或不清晰的地方, 还请指出来, 谢谢…

1.1. ROS Control

无论因为什么而做机器人相关的工作, 控制是绝对避开不了的一个话题. 怎么让机器人动起来, 或者怎么控制机器人上挂载的传感器等等问题, 如何避免不同控制器对硬件资源的冲突. 这些都是很繁琐的工作, 并且必然存在大量的重复工作. ROS Control为这些问题提出了一种解决方案, 当然, 其功能会更强大. ROS Control, 脱胎于pr2_mechanism, 在此基础上, 将其抽象, 使之成为一个框架, 而不限于pr2. 整体架构如下图所示. (pr2_mechanism也是一个很好的软件系统. 如果有时间, 后面也整理出来分享给大家)

1.2. 机器人 – ur5bh

既然是给定的任务, 那么必然需要一个载体. 后面都将统一使用下图模型.

该模型的描述文件, 在第一篇博客中也提到了, 其实很简单. 就是如下所示的内容:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur5bh" >

    <!-- common stuff -->
    <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />

    <!-- ur5 -->
    <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />

    <!-- barrett hand -->
    <xacro:include filename="$(find bhand_description)/urdf/bh282.urdf.xacro" />

    <!-- arm -->
    <xacro:ur5_robot prefix="" joint_limited="true"/>

    <link name="world" />

    <joint name="world_joint" type="fixed">
        <parent link="world" />
        <child link = "base_link" />
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
    </joint>

    <!-- end effector -->
    <xacro:bhand_macro parent="ee_link" name="bh">
        <origin xyz="0.012 0.0 0.0" rpy="${pi/2.0} ${pi/2.0} ${pi/2.0}"/>
    </xacro:bhand_macro>

</robot>

当然, 前述文件需要编译成功的话, 是需要依赖于你电脑上是有安装两个ROS Package – ur_dscription and bhand_description.

1.3. Step by Step

为了统一环境和路径这种, 后面, 我会记录我运行的每一步. 首先我在~/.bashrc中注释掉了自己的所有工作空间. 依次执行下列命令:

$ mkdir -p ~/csdn_ws/src && cd ~/csdn_ws/src
$ catkin_init_workspace
# 下载bhand包, 得到bhand_description
$ git clone -b hydro-devel https://github.com/RobotnikAutomation/barrett_hand.git
$ cd .. && catkin_make
$ echo "source ~/csdn_ws/devel/setup.bash" >> ~/.bashrc
$ source ~/.bashrc
$ cd ~/csdn_ws/src/ && catkin_create_pkg ur5bh_description ur_description bhand_description
$ cd ~/csdn_ws && catkin_make
$ mkdir src/ur5bh_description/urdf && cd src/ur5bh_description/urdf
$ gedit ur5bh.urdf.xacro

上述命令很简单, 就不多作解释了吧? 最后打开编辑器之后, 将上述内容xml内容复制到该文本中, 保存. 另外, 如果在$ cd ~/csdn_ws && catkin_make这一步出现找不到ur_description包的错误, 在命令行里面通过apt-get来安装, 命令如下:

# 若提示ur_description包找不到的错误, 则执行下述命令
$ sudo apt-get install ros-indigo-ur-description
# 所有都OK的话, 可以用如下命令来查看模型效果
$ roslaunch urdf_tutorial xacrodisplay.launch model:=ur5bh.urdf.xacro

如果没有问题, 最后使用urdf_tutorial包可以可视化模型, 就是前图所示的状态.

2. 解析URDF

想要搞清楚后面的事情, 读懂URDF文件是很重要的一件事情. 现在我们建立好了ur5bh, 可以通过如下命令, 将XACRO文件转换成URDF.

# 默认情况下rosrun xacro xacro.py ur5bh.urdf.xacro将会直接将所有转换后的内容输出到命令行
$ rosrun xacro xacro.py ur5bh.urdf.xacro >> ur5bh.urdf

在该目录下, 将会在当前目录下生成一个ur5bh.urdf文件, 是解析之后得到的所有内容. 在使用过程中, 最终上传到ROS 参数服务器中的”robot_description”的内容一模一样. 我摘录其中值得说道的内容如下:

<gazebo>
    <plugin filename="libgazebo_ros_control.so" name="ros_control">
        <!--robotNamespace>/</robotNamespace-->
        <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
    </plugin>
</gazebo>

... ...

<transmission name="shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_pan_joint">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_pan_motor">
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>
... ...
<transmission name="bh_j32_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="bh_j32_joint"/>
    <actuator name="bh_j32">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
        <motorTorqueConstant>1</motorTorqueConstant>
    </actuator>
</transmission>

其中第一条Tag gazebo的内容, 在文件的第一行, 作用是导入gazebo_ros_control, 如果该文件被Gazebo加载的话, Gazebo将会为该模型添加默认的Gazebo_ros_control库. 该库究竟有什么作用, 后面我会细说. 这儿我先详细解析一下, transmission是一个什么鬼. 其实从下面这张图中, 可以窥视一些端倪.

在前述ROS Control的数据流图中可以看得到, transmission是硬件抽象层中的一部分. 从图中可以看到, transmission的任务很清晰, 主要是两个方向的任务:
- Actuator <–> Joint: 定义Joint的Pos, Vel, Tor与Motor的Pos, Vel, Tor.
- State: 将编码器(例如角度, 力矩传感器等)的数据, 传递给对应的Joint, 变成最终的RobotState.

2.1. `TransmissionInfo`

此处所指的是第一种transmission. 每一个transmission主要由两部分构成, 从上面xml内容中也可以看出来, 分别是joint和actuator. 转换成代码, 如下所示(transmission_interface/transmission_info.h):

/**
 * \brief Contains semantic info about a given joint loaded from XML (URDF)
 */
struct JointInfo
{
    std::string name_;
    std::vector<std::string> hardware_interfaces_;
    std::string role_;
    std::string xml_element_;
};

/**
 * \brief Contains semantic info about a given actuator loaded from XML (URDF)
 */
struct ActuatorInfo
{
    std::string name_;
    std::vector<std::string> hardware_interfaces_;
    std::string xml_element_;
};

/**
 * \brief Contains semantic info about a given transmission loaded from XML (URDF)
 */
struct TransmissionInfo
{
    std::string name_;
    std::string type_;
    std::vector<JointInfo> joints_;
    std::vector<ActuatorInfo> actuators_;
};

2.2. `TransmissionLoader`

那这些信息又是怎么加载的呢? 以及, 每一个Tag里面的子Tag又是些什么鬼啊? 答案都可以在TransmissionLoader这个类里面找到答案. 这是一个抽象类, 实现的每一个Transmission的子类, 都要配套实现一个TransmissionLoader. 该基类的定义如下(只是摘录了部分):

class TransmissionLoader
{
public:
    virtual ~TransmissionLoader() {}

    typedef boost::shared_ptr<Transmission> TransmissionPtr;
    virtual TransmissionPtr load(const TransmissionInfo& transmission_info) = 0;
protected:
    enum ParseStatus
    {
      SUCCESS,
      NO_DATA,
      BAD_TYPE
    };

    ... ...

    static ParseStatus getActuatorReduction(
            const TiXmlElement& parent_el,
            const std::string&  actuator_name,
            const std::string&  transmission_name,
            bool                required,
            double&             reduction);

    static ParseStatus getJointReduction(
            const TiXmlElement& parent_el,
            const std::string&  joint_name,
            const std::string&  transmission_name,
            bool                required,
            double&             reduction);

    static ParseStatus getJointOffset(
            const TiXmlElement& parent_el,
            const std::string&  joint_name,
            const std::string&  transmission_name,
            bool                required,
            double&             offset);

    static ParseStatus getActuatorRole(
            const TiXmlElement& parent_el,
            const std::string&  actuator_name,
            const std::string&  transmission_name,
            bool                required,
            std::string&        role);

    static ParseStatus getJointRole(
            const TiXmlElement& parent_el,
            const std::string&  joint_name,
            const std::string&  transmission_name,
            bool                required,
            std::string&        role);
}

中间省略了几个静态函数, 主要用以判断数据格式是否正确等. 上面是TransmissionLoader基类的定义, 其中包含一个纯虚函数. 其他都是一些工具函数. 其中关于几个子标签的数据读取, 其中mechanicalReduction在getActuatorReduction()中读取, 源码如下, 类似的, Joint也是可以有mechanicalReduction标签的. 除了这个标签外, Actuator和Joint还可以有role标签, joint还有一个独有的标签offset.

TransmissionLoader::ParseStatus
TransmissionLoader::getActuatorReduction(
        const TiXmlElement& parent_el,
        const std::string&  actuator_name,
        const std::string&  transmission_name,
        bool                required,
        double&             reduction ) {
    // Get XML element
    const TiXmlElement* reduction_el =
        parent_el.FirstChildElement("mechanicalReduction");
    if(!reduction_el) {
    if (required) {
    ROS_ERROR_STREAM_NAMED("parser",
        "Actuator '" << actuator_name << "' of transmission '"
        << transmission_name <<
        "' does not specify the required <mechanicalReduction> element.");
    } else {
        ROS_DEBUG_STREAM_NAMED("parser",
            "Actuator '" << actuator_name << "' of transmission '"
            << transmission_name <<
            "' does not specify the optional <mechanicalReduction> element.");
        }
        return NO_DATA;
    }

    // Cast to number
    try {
        reduction = boost::lexical_cast<double>(reduction_el->GetText());
    } catch (const boost::bad_lexical_cast&) {
        ROS_ERROR_STREAM_NAMED("parser",
            "Actuator '" << actuator_name << "' of transmission '"
            << transmission_name <<
            "' specifies the <mechanicalReduction> element, but is not a number.");
        return BAD_TYPE;
    }
    return SUCCESS;
}

2.3. `SimpleTransmission`

transmission的类型是transmission_interface/SimpleTransmission, 实质是指transmission_interface::SimpleTransmission类, 该类完成简单的传递, 定义如下:

class SimpleTransmission : public Transmission
{
public:
  SimpleTransmission(const double reduction,
                     const double joint_offset = 0.0);

  void actuatorToJointEffort(
              const ActuatorData& act_data,
                    JointData&    jnt_data);

  void actuatorToJointVelocity(
              const ActuatorData& act_data,
                    JointData&    jnt_data);

  void actuatorToJointPosition(
              const ActuatorData& act_data,
                    JointData&    jnt_data);

  void jointToActuatorEffort(
              const JointData&    jnt_data,
                    ActuatorData& act_data);

  void jointToActuatorVelocity(
              const JointData&    jnt_data,
                    ActuatorData& act_data);

  void jointToActuatorPosition(
              const JointData&    jnt_data,
                    ActuatorData& act_data);

  // 从代码中看到, 强制要求只能有一个Actuator和一个Joint
  // 后面在TransmissionLoader中也会对其进行判错.
  std::size_t numActuators() const {return 1;}
  std::size_t numJoints()    const {return 1;}

  double getActuatorReduction() const {return reduction_;}
  double getJointOffset()       const {return jnt_offset_;}

private:
  double reduction_;
  double jnt_offset_;
};

SimpleTransmission是建模Actuator和Joint通过减速器(reductor)或放大器(amplifier), 结构图如下:

上图是SimpleTransimission的典型建模场景. 各个关系如下所示:

Type	Effort	Velocity	Position
Actuator to Joint	$\tau_j = n \tau_a$	$\dot{x}_j = \frac{\dot{x}_a}{n}$	$x_j = \frac{x_a}{n} + x_{off}$
Joint to actuator	$\tau_a = \frac{\tau_j}{n}$	$\dot{x}_a = n \dot{x}_j$	$x_a = n (x_j - x_{off})$

其中:

公式中 $x$ , $\dot{x}$ and $\tau$ 分别代表 position, velocity and effort.
下标 $_a$ and $_j$ 用来标记actuator 和 joint.
$x_{off}$ 标记电机与joint zeros之间的偏移量(用joint position值表示).
n是传递比例(the transmission ratio), 能够通过传动带的输入输出半径之比或齿轮的齿数来计算.
- 若 $n > 1$ , 则这个系统是速度减速器/力矩放大器; 若 $n \in (0, 1)$ , 正好相反, 是速度放大器, 力矩减速器.
- 若 $n < 0$ , 则代表反向.

到此, 上面SimpleTransmission的功能和定义就清晰可见了. 其中两个私有成员变量, 就是分别对应 $n$ 和 $x_{off}$ . SimpleTransmission类对应的Loader的实现如下, 下面是完整的CPP文件内容:

// ROS头文件
#include <ros/console.h>

// Pluginlib, 详情参看之前一篇博客 http://blog.csdn.net/sunbibei/article/details/52958724
#include <pluginlib/class_list_macros.h>

// ros_control中的几个头文件, 第一个暂时不管, 另外两个在前面都已提到了.
// 第一个头文件仅仅提供模板函数demangledTypeName()
#include <hardware_interface/internal/demangle_symbol.h>
#include <transmission_interface/simple_transmission.h>
#include <transmission_interface/simple_transmission_loader.h>
namespace transmission_interface // 命名空间
{
// SimpleTransmissionLoader共有继承于TransmissionLoader, 所以必须实现纯虚函数load
SimpleTransmissionLoader::TransmissionPtr
SimpleTransmissionLoader::load(const TransmissionInfo& transmission_info)
{
  // Transmission should contain only one actuator/joint
  // 下面两个函数都是基类TransmissionLoader中的静态函数, 工具函数
  // 在基类中, 有类型定义: typedef boost::shared_ptr<Transmission> TransmissionPtr;
  if (!checkActuatorDimension(transmission_info, 1)) {return TransmissionPtr();}
  if (!checkJointDimension(transmission_info,    1)) {return TransmissionPtr();}

  // Parse actuator and joint xml elements
  TiXmlElement actuator_el =
      loadXmlElement(transmission_info.actuators_.front().xml_element_);
  TiXmlElement joint_el =
      loadXmlElement(transmission_info.joints_.front().xml_element_);

  // 这儿就是获得Actuator中的子标签 mechanicalReduction
  double reduction = 0.0;
  const ParseStatus reduction_status = getActuatorReduction(actuator_el,
                    transmission_info.actuators_.front().name_,
                    transmission_info.name_,
                    true, // Required
                    reduction);
  if (reduction_status != SUCCESS) {return TransmissionPtr();}

  // 这儿是解析获得joint中的子标签offset的值
  // required 参数为false. 将不会输出错误信息
  // 该参数true和false的区别仅仅在于, 不存在该标签时, 是否输出错误
  // 返回值都是NO_DATA, 并不修改joint_offset的值
  double joint_offset = 0.0;
  const ParseStatus joint_offset_status = getJointOffset(joint_el,
                  transmission_info.joints_.front().name_,
                  transmission_info.name_,
                  false, // Optional
                  joint_offset);
  if (joint_offset_status == BAD_TYPE) {return TransmissionPtr();}

  // 在这儿实例化SimpleTransmission.
  try
  {
    TransmissionPtr transmission(new SimpleTransmission(reduction, joint_offset));
    return transmission;
  }
  catch(const TransmissionInterfaceException& ex)
  {
    using hardware_interface::internal::demangledTypeName;
    ROS_ERROR_STREAM_NAMED("parser", "Failed to construct transmission '" <<        
            transmission_info.name_ << "' of type '" <<
            demangledTypeName<SimpleTransmission>()<< "'. " << ex.what());
    return TransmissionPtr();
  }
}

} // namespace

// 这个是pluginlib中定义的一个宏, 具体作用见另一篇博客: 
// http://blog.csdn.net/sunbibei/article/details/52958724
PLUGINLIB_EXPORT_CLASS(transmission_interface::SimpleTransmissionLoader,
                           transmission_interface::TransmissionLoader)

2.4. `TransmissionParser`

说完SimpleTransmission, 上面标签中有一个奇怪的现象, 不知道各位朋友是否注意到.

子标签hardwareInterface在没有看到是在什么地方解析的啊, 怎么有时候是Joint有该标签, 有时候是Actuator有该标签
在Barrett的手指关节Transmission内的Actuator标签下, 有一个子标签是motorTorqueConstant, 在默认的SimpleTramsmisson中, 并不会解析这个参数啊.
在TransmissionLoader中, 传入的参数是TransmissionInfo的常引用, 那么这个TransmissionInfo又是谁给Loader的? 这里面每一个结构体都有一个string xml_element_, 这里面又是什么鬼?

带着这个疑问, 接着解析下去. 实际上, 所有的TransmissionInfo都是由一个工具类TransmissionParser得到的. 该类解析整个robot_description的URDF文件, 从中间得到所有关于Transmission的内容, 转换成TransmissionInfo. 该类定义如下, 内部仅有三个静态成员函数, 没有任何成员变量.

class TransmissionParser
{
public:
  // 从URDF中解析所有的transmission元素
  static bool parse(const std::string& urdf_string, std::vector<TransmissionInfo>& transmissions);

private:
  // 从transmission块中解析Joint相关的内容
  static bool parseJoints(TiXmlElement *trans_it, std::vector<JointInfo>& joints);
  // 从transmission块中解析Actuator相关的内容
  static bool parseActuators(TiXmlElement *trans_it, std::vector<ActuatorInfo>& actuators);

}; // class

其中, 共有成员函数就一个, 其实现如下:

bool TransmissionParser::parse(
    const std::string& urdf,
    std::vector<TransmissionInfo>& transmissions) {
  // 从string中初始化TiXmlDocument doc
  TiXmlDocument doc;
  if (!doc.Parse(urdf.c_str()) && doc.Error()) {
    ROS_ERROR("Can't parse transmissions. Invalid robot description.");
    return false;
  }
  // 迭代解析所有的transmission块
  TiXmlElement *root = doc.RootElement();
  TiXmlElement *trans_it = NULL;
  for (trans_it = root->FirstChildElement("transmission"); trans_it;
       trans_it = trans_it->NextSiblingElement("transmission")) {
    transmission_interface::TransmissionInfo transmission; // 初始化
    // Transmission name, 例如"bh_j22_transmission"
    if(trans_it->Attribute("name")) {
      transmission.name_ = trans_it->Attribute("name");
      if (transmission.name_.empty()) {
        ROS_ERROR_STREAM_NAMED("parser",
         "Empty name attribute specified for transmission.");
        continue;
      }
    } else {
      ROS_ERROR_STREAM_NAMED("parser",
       "No name attribute specified for transmission.");
      continue;
    }
    // Transmission type, 例如transmission_interface/SimpleTransmission
    TiXmlElement *type_child = trans_it->FirstChildElement("type");
    if(!type_child) {
      ROS_ERROR_STREAM_NAMED("parser",
       "No type element found in transmission '"
       << transmission.name_ << "'.");
      continue;
    }
    if (!type_child->GetText()) {
      ROS_ERROR_STREAM_NAMED("parser",
       "Skipping empty type element in transmission '"
       << transmission.name_ << "'.");
      continue;
    }
    transmission.type_ = type_child->GetText();
    // 加载 joints块
    if(!parseJoints(trans_it, transmission.joints_)) {
      ROS_ERROR_STREAM_NAMED("parser",
       "Failed to load joints for transmission '"
       << transmission.name_ << "'.");
      continue;
    }
    // 加载 actuators
    if(!parseActuators(trans_it, transmission.actuators_)) {
      ROS_ERROR_STREAM_NAMED("parser",
       "Failed to load actuators for transmission '"
       << transmission.name_ << "'.");
      continue;
    }
    // 保存加载成功的 TransmissionInfo
    transmissions.push_back(transmission);
  } // end for <transmission>
  if( transmissions.empty() ) {
    ROS_DEBUG_STREAM_NAMED("parser", "No valid transmissions found.");
  }
  return true;
}

该代码内部就一个循环, 遍历所有URDF内容中的transmission块, 逐个进行解析, 可以看到, name和type是必须填入的内容, 否则整个transmission出错, 但不影响别的transmission块的解析. 关于joint and actuator的解析代码如下:

bool TransmissionParser::parseJoints(
    TiXmlElement *trans_it,
    std::vector<JointInfo>& joints) {
  // 循环遍历所有的joint块, 也就是说, 一个transmission可能有多个joint相关联
  // 当然, SimpleTransmission是不允许这样的
  TiXmlElement *joint_it = NULL;
  for (joint_it = trans_it->FirstChildElement("joint"); joint_it;
       joint_it = joint_it->NextSiblingElement("joint")) {
    transmission_interface::JointInfo joint; // 初始化
    // Joint name, 例如"shoulder_lift_joint"
    if(joint_it->Attribute("name")) {
      joint.name_ = joint_it->Attribute("name");
      if (joint.name_.empty()) {
        ROS_ERROR_STREAM_NAMED("parser",
         "Empty name attribute specified for joint.");
        continue;
      }
    } else {
      ROS_ERROR_STREAM_NAMED("parser",
       "No name attribute specified for joint.");
      return false;
    }

    // 这儿就是前面我们没有看到的内容, Hardware interfaces, 在这个地方填入JointInfo
    TiXmlElement *hw_iface_it = NULL;
    for (hw_iface_it = joint_it->FirstChildElement("hardwareInterface"); hw_iface_it;
         hw_iface_it = hw_iface_it->NextSiblingElement("hardwareInterface")) {
      if(!hw_iface_it) {continue;}
      if (!hw_iface_it->GetText()) {
        ROS_DEBUG_STREAM_NAMED("parser",
         "Skipping empty hardware interface element in joint '"
         << joint.name_ << "'.");
        continue;
      }
      const std::string hw_iface_name = hw_iface_it->GetText();
      joint.hardware_interfaces_.push_back(hw_iface_name);
    } // end for "hardwareInterface"
    // 到这儿, 问题就来了. Barrett的描述文件中, Actuator有hardwareInterface, joint并没有
    // 理论来说, 解析到这儿, 会输出下面的错误
    // 并且, 该transmission块整个都将解析失败, 即不起作用.
    if (joint.hardware_interfaces_.empty()) {
      ROS_ERROR_STREAM_NAMED("parser",
       "No valid hardware interface element found in joint '"
       << joint.name_ << "'.");
      continue; // back to for "joint"
    }
    // xml_element_中的内容, 是除name, hardwareinterface之外的所有.
    std::stringstream ss;
    ss << *joint_it;
    joint.xml_element_ = ss.str();
    // Add joint to vector
    joints.push_back(joint);
  } // end for "joint"
  if(joints.empty()) {
    ROS_DEBUG_NAMED("parser","No valid joint element found.");
    return false;
  }
  return true;
}

bool TransmissionParser::parseActuators(TiXmlElement *trans_it, std::vector<ActuatorInfo>& actuators) {
  // 循环遍历所有的actuator块, 也就是说, 一个transmission可能有多个actuator相关联
  // 当然, SimpleTransmission是不允许这样的
  TiXmlElement *actuator_it = NULL;
  for (actuator_it = trans_it->FirstChildElement("actuator"); actuator_it;
       actuator_it = actuator_it->NextSiblingElement("actuator")) {
    // 初始化ActuatorInfo
    transmission_interface::ActuatorInfo actuator;
    // Actuator name, 例如"shoulder_lift_motor"
    if(actuator_it->Attribute("name")) {
      actuator.name_ = actuator_it->Attribute("name");
      if (actuator.name_.empty()) {
        ROS_ERROR_STREAM_NAMED("parser",
         "Empty name attribute specified for actuator.");
        continue;
      }
    } else {
      ROS_ERROR_STREAM_NAMED("parser",
       "No name attribute specified for actuator.");
      return false;
    }

    // 加载Hardware interfaces子标签
    // 注意, 在UR的描述部分, actuator是没有这个标签的. 将不会进入for 循环体
    TiXmlElement *hw_iface_it = NULL;
    for (hw_iface_it = actuator_it->FirstChildElement("hardwareInterface"); hw_iface_it;
         hw_iface_it = hw_iface_it->NextSiblingElement("hardwareInterface")) {
      if(!hw_iface_it) {continue;}
      if (!hw_iface_it->GetText()) {
        ROS_DEBUG_STREAM_NAMED("parser",
         "Skipping empty hardware interface element in actuator '"
         << actuator.name_ << "'.");
        continue;
      }
      const std::string hw_iface_name = hw_iface_it->GetText();
      actuator.hardware_interfaces_.push_back(hw_iface_name);
    }
    // ur相关的transmission到这儿的时候, empty()返回真
    if (actuator.hardware_interfaces_.empty()) {
      ROS_DEBUG_STREAM_NAMED("parser",
       "No valid hardware interface element found in actuator '"
       << actuator.name_ << "'.");
      // 这儿就是和joint块的差别, 不会返回到for "actuator", 将会继续解析.
      // continue; 
    }
    // Actuator xml element
    std::stringstream ss;
    ss << *actuator_it;
    actuator.xml_element_ = ss.str();
    // Add actuator to vector
    actuators.push_back(actuator);
  }
  if(actuators.empty()) {
    ROS_DEBUG_NAMED("parser","No valid actuator element found.");
    return false;
  }

  return true;
}

从上面的代码中, 可以将前面的三个疑问全部解决. hardwareinterface的子标签, joint 和 actuator都可以有, 不过, 区别在于: joint必须指定hardwareinterface, actuator可选的指定hardwareinterface. 其余子标签延迟到具体的TransmissionLoader去解析.

而我们是可以看到的, 上面所有关于barrett的transmission块中, joint标签下都没有定义hardwareinterface. 从代码来看, 这里必然会出现错误的. 在后面的示例中, 我们也将会看到

hardwareInterface指的是该关节提供的控制方式, 是位置控制(从名字中可以看出来), PositionJointInterface, 这是一个类, 该类继承于JointCommandInterface, 同时, 还有EffortJointInterface, VelocityJointInterface, 力矩/速度/位置关节接口, 都是继承于JointCommandInterface. 类间关系图如下:

2.5. `TransmissionInterfaceLoader`

那么, 又是谁来调用TransmissionParser工具类来获取TransmissionInfo?

答案是: TransmissionInterfaceLoader, 这个类调用TransmissionParser工具类来获取TransmissionInfo, 再利用TransmissionLoader来实例化Transmission. 这个类的构造函数定义见下方, 构造函数中将会传入hardware_interface::RobotHW* 和 RobotTransmissions*, 第一个是整个整个硬件的抽象, 包含软件与硬件的通信(读 and 写)的具体实现; 第二个类, 实质上就是一个InterfaceManager(共有继承于它, 未新增任何东西). 个人感觉, 理解到上述整个调用过程就已经足够了, 再往下, 其实对于大部分人来说都没有太多的必要去了解了. 不想再看下去的朋友, 可以直接到下一节了.

TransmissionInterfaceLoader::TransmissionInterfaceLoader(
    hardware_interface::RobotHW* robot_hw,
    RobotTransmissions* robot_transmissions)
      : robot_hw_ptr_(robot_hw),
        robot_transmissions_ptr_(robot_transmissions)
{
    // ClassLoader初始化, 这个东西是PluginLib里面的一个动态加载库中对象的工具.
    // 能够加载 ROS 包transmission_interface中的基类是
    // "transmission_interface::TransmissionLoader"的plugin
    transmission_class_loader_.reset(new
        TransmissionClassLoader("transmission_interface",
            "transmission_interface::TransmissionLoader"));

    // ClassLoader初始化, 这个东西是PluginLib里面的一个动态加载库中对象的工具.
    req_provider_loader_.reset(new 
        RequisiteProviderClassLoader("transmission_interface",
            "transmission_interface::RequisiteProvider"));

    // Throw if invalid
    if (!robot_hw) {
      throw std::invalid_argument("Invalid robot hardware pointer.");
    }
    if (!robot_transmissions) {
      throw std::invalid_argument("Invalid robot transmissions pointer.");
    }
    loader_data_.robot_hw            =  robot_hw_ptr_;
    loader_data_.robot_transmissions =  robot_transmissions_ptr_;
}

可以看到, 这个类的初始化是需要有其他内容传入, 所以这个类肯定还是有别的调用者, 而谁调用的这个类, 这儿就不展开了, 后面会提到. 这个类在transmission_interface中的类关系见下图所示:

3. 导入模型到Gazebo

上面命令一切无误的话, 现在应该能够看到前面图片所示的UR5 + Barrett的效果了. 接着, 我们来编写ur5bh_bingup.launch文件.

$ cd ~/csdn_ws/src/
$ catkin_create_pkg ur5bh_gazebo ur5bh_description gazebo_ros gazebo_ros_control joint_state_controller effort_controllers robot_state_publisher
$ mkdir ur5bh_gazebo/launch && cd ur5bh_gazebo/launch
$ gedit ur5bh_gazebo.launch

在打开的编辑器中, 复制入下述内容:

<?xml version="1.0"?>
<launch>

    <!-- startup simulated world -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />

    <!-- send robot urdf to param server -->
    <param name="robot_description" command="$(find xacro)/xacro.py '$(find ur5bh_description)/urdf/ur5bh.urdf.xacro'" />

    <!-- push robot_description to factory and spawn robot in gazebo -->
    <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
        args="-urdf -param robot_description -model robot -z 0.1"
        respawn="false" output="screen" />

</launch>

上述launch文件比较简单, 分为三部分:

第一部分, 跟gazebo相关的, 发起一个空世界;
第二部分, 类似于我们之前在命令行中执行的$ rosrun xacro xacro.py path/to/ur5bh.urdf.xacro, 将该命令的输出设置为ROS 参数服务器中名为robot_description的参数内容.
第三部分, 相当于$ rosrun gazebo_ros spawn_model -urdf -param robot_description -model robot -z 0.1.

保存后, 在命令行中运行: $ roslaunch ur5bh_gazebo ur5bh_gazebo.launch, 不出意外, 在命令行中的输出将会出现错误, 就是前述我们在TransmissionParser代码中所提到的, 关于Bareett描述中, hardwareinterface标签, 在joint中没定义的问题, 见下图:

在gazebo显示的Robot, 奇形怪状的见下图:

想让它在Gazebo中好看, 其实很简单, 就是修复transmission的问题就好了. 一个很简单的方法, 就是使用我们之前利用脚本产生的ur5bh.urdf文件, 在该文件中, 找到所有barrett的transmission块, 修该成如下所示的样子, 即在joint标签内, 加入hardwareinterface子标签.

<transmission name="bh_j23_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="bh_j23_joint">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="bh_j23">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
        <motorTorqueConstant>1</motorTorqueConstant>
    </actuator>
</transmission>

在我们的ur5bh_gazebo.launch文件中, 将<param name="robot_description" command="$(find xacro)/xacro.py '$(find ur5bh_description)/urdf/ur5bh.urdf.xacro'" />中的ur5bh.urdf.xacro改成ur5bh.urdf即可. 修改完成之后, 在命令行中执行:$ roslaunch ur5bh_gazebo ur5bh_gazebo.launch. 这个时候你应该能够看到下图所示的效果了.

原文链接：https://blog.csdn.net/sunbibei/article/details/53287975

目录

1. 说在前面

1.1. ROS Control

1.2. 机器人 – ur5bh

1.3. Step by Step

2. 解析URDF

2.1. TransmissionInfo

2.2. TransmissionLoader

2.3. SimpleTransmission

2.4. TransmissionParser

2.5. TransmissionInterfaceLoader