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	τj=nτa	x˙j=x˙an	xj=xan+xoff
Joint to actuator	τa=τjn	x˙a=nx˙j	xa=n(xj−xoff)

其中:

公式中x , x˙ and τ 分別代表 position, velocity and effort.
下標 a and j 用來標記actuator 和 joint.
xoff 標記電機與joint zeros之間的偏移量(用joint position值表示).
n 是傳遞比例(the transmission ratio), 能夠通過傳動帶的輸入輸出半徑之比或齒輪的齒數來計算.
- 若n>1 , 則這個系統是速度減速器/力矩放大器; 若n∈(0,1) , 正好相反, 是速度放大器, 力矩減速器.
- 若n<0 , 則代表反向.

到此, 上面SimpleTransmission的功能和定義就清晰可見了. 其中兩個私有成員變量, 就是分別對應n 和xoff . 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. 這個時候你應該能夠看到下圖所示的效果了.

ROS Control之Transmission

目錄

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`

3. 導入模型到Gazebo

985 碩士程序員，空窗 4 個月沒有 Offer！

一文搞懂 Spring 循環依賴

賽博鬥地主——使用大語言模型扮演Agent智能體玩牌類遊戲。

VScode右鍵打開(添加到右鍵)

記一次 .NET某工控視覺自動化系統卡死分析

WindowsServer--SQL Server搭建主從同步實現讀寫分離 - 事務性分發

java由於越界導致的報錯

Ubuntu14.04環境下ROS indigo 安裝

Count And Say

機器人描述--URDF和XACRO

Gazebo Ros Control 及 Controller運用

Guided Policy Search

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

ROS Control之Transmission

目錄

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

3. 導入模型到Gazebo

2.1. `TransmissionInfo`

2.2. `TransmissionLoader`

2.3. `SimpleTransmission`

2.4. `TransmissionParser`

2.5. `TransmissionInterfaceLoader`