Qiita Teams that are logged in
You are not logged in to any team

Log in to Qiita Team
Community
OrganizationAdvent CalendarQiitadon (β)
Service
Qiita JobsQiita ZineQiita Blog
9
Help us understand the problem. What is going on with this article?
@MoriKen

Controller と HardwareInterface との間の処理の仕組み(2. RobotHWSimのプラグインについて)

More than 5 years have passed since last update.

目次

  1. ロボットモデルの定義と登録
    1. URDFでロボットモデルを定義する
    2. HardwareInterfaceの存在意義
    3. URDFGazeboの設定をする
    4. ロボットモデルをパラメータサーバに登録する
  2. RobotHWSimのプラグインについて ← イマココ
    1. プラグインの定義
    2. プラグインで定義された処理が実行される場所
    3. RobotHWSinRobotHWとの対比
    4. プラグインとして使えるようにする
    5. DefaultRobotHWSimについて
  3. Controllerについて
    1. GazeboにロボットモデルをSpawnする
    2. Controllerを起動する
    3. ControllerManagerHardwareInterfaceとのマッピング

お題

はじめに

1. ロボットモデルの定義と登録」の説明において,gazebo.urdf.xacrorosbook_arm_hardware_gazebo/ROSBookArmHardwareGazeboというカスタムプラグインを指定した箇所がありました.この正体を棚上げにしていたので,ここで理解を深めることにします.

本家サイトの図において,下記ピンクで囲った箇所です.
gazebo_plugin.png

本記事用のまとめの図においては,下記の緑で囲った箇所です.
2.summary.png

プラグインの定義

このカスタムプラグインの実体は,rosbook_arm_hardware_gazeboパッケージの中にあり,C++で実装されています.大事なところだけ覗いてみます.

rosbook_arm_hardware_gazebo/include/rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h
  // 省略
#include <hardware_interface/robot_hw.h>
  // 省略
#include <gazebo_ros_control/robot_hw_sim.h>
  // 省略
namespace rosbook_arm_hardware_gazebo
{

class ROSBookArmHardwareGazebo : public gazebo_ros_control::RobotHWSim
{
public:

  ROSBookArmHardwareGazebo();

  bool initSim(const std::string& robot_namespace,
      ros::NodeHandle model_nh,
      gazebo::physics::ModelPtr parent_model,
      const urdf::Model* const urdf_model,
      std::vector<transmission_interface::TransmissionInfo> transmissions);

  void readSim(ros::Time time, ros::Duration period);

  void writeSim(ros::Time time, ros::Duration period);
  // 省略
private:
  // 省略
  std::vector<double> jnt_pos_;
  std::vector<double> jnt_vel_;
  std::vector<double> jnt_eff_;

  std::vector<double> jnt_pos_cmd_;

  std::vector<gazebo::physics::JointPtr> sim_joints_;

  // Hardware interface: joints
  hardware_interface::JointStateInterface    jnt_state_interface_;
  hardware_interface::PositionJointInterface jnt_pos_cmd_interface_;
  // 省略
};

} // namespace rosbook_arm_hardware_gazebo

#endif // ROSBOOK_ARM_HARDWARE_GAZEBO_H

ROSBookArmHardwareGazebo2.png

initSim(), readSim(), writeSim() メソッドが重要です.HardwareInterfaceControllerとのやりとりや,HardwareInterfaceGazeboとのやりとりを実現する部分です.これらのメソッドは,gazebo_ros_control::RobotHWSimクラスで純粋仮想メソッドとして宣言されているので,カスタマイズしたメソッドでオーバーライドしてあげます.定義については,下記のコードで見てみましょう.

rosbook_arm_hardware_gazebo/src/rosbook_arm_hardware_gazebo.cpp
  // 省略
#include <rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h>

namespace rosbook_arm_hardware_gazebo
{
  using namespace hardware_interface;
  // 省略
  bool ROSBookArmHardwareGazebo::initSim(const std::string& robot_namespace,
      ros::NodeHandle nh,
      gazebo::physics::ModelPtr model,
      const urdf::Model* const urdf_model,
      std::vector<transmission_interface::TransmissionInfo> transmissions)
  {
    using gazebo::physics::JointPtr;

    // Cleanup
    sim_joints_.clear();
    jnt_pos_.clear();
    jnt_vel_.clear();
    jnt_eff_.clear();
    jnt_pos_cmd_.clear();

    // Simulation joints
    sim_joints_ = model->GetJoints();
    n_dof_ = sim_joints_.size();

    std::vector<std::string> jnt_names;
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_names.push_back(sim_joints_[i]->GetName());
    }

    // Raw data
    jnt_pos_.resize(n_dof_);
    jnt_vel_.resize(n_dof_);
    jnt_eff_.resize(n_dof_);
    jnt_pos_cmd_.resize(n_dof_);

    // Hardware interfaces
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_state_interface_.registerHandle(
          JointStateHandle(jnt_names[i], &jnt_pos_[i], &jnt_vel_[i], &jnt_eff_[i]));

      jnt_pos_cmd_interface_.registerHandle(
          JointHandle(jnt_state_interface_.getHandle(jnt_names[i]), &jnt_pos_cmd_[i]));

      ROS_DEBUG_STREAM("Registered joint '" << jnt_names[i] << "' in the PositionJointInterface.");
    }

    registerInterface(&jnt_state_interface_);
    registerInterface(&jnt_pos_cmd_interface_);

  // 省略
    }
  // 省略
    return true;
  }

  void ROSBookArmHardwareGazebo::readSim(ros::Time time, ros::Duration period)
  {
    for (size_t i = 0; i < n_dof_; ++i)
    {
      jnt_pos_[i] += angles::shortest_angular_distance
          (jnt_pos_[i], sim_joints_[i]->GetAngle(0u).Radian());
      jnt_vel_[i] = sim_joints_[i]->GetVelocity(0u);
      jnt_eff_[i] = sim_joints_[i]->GetForce(0u);
    }
  }

  void ROSBookArmHardwareGazebo::writeSim(ros::Time time, ros::Duration period)
  {
    // Enforce joint limits
    jnt_limits_interface_.enforceLimits(period);

    // Compute and send commands
    for (size_t i = 0; i < n_dof_; ++i)
    {
      const double error = jnt_pos_cmd_[i] - jnt_pos_[i];
      const double effort = pids_[i].computeCommand(error, period);

      sim_joints_[i]->SetForce(0u, effort);
    }
  }

} // namespace rosbook_hardware_gazebo

PLUGINLIB_EXPORT_CLASS(rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo, gazebo_ros_control::RobotHWSim)

重要な三つのメソッドがオーバーライドされていることが確認できます.概要は下記のとおりです.

  • initSim()
    • パラメータサーバに登録されたロボットモデルから,joint 名を取得します.
    • 取得したjoint 名でJointStateのハンドルを作成し,Hardware Resource Interfaceをインターフェースとして登録します.状態の読み出しに使うのがhardware_interface::JointStateInterface jnt_state_interface_で,指令に書き込みに使うのがhardware_interface::PositionJointInterface jnt_pos_cmd_interface_です.(これらはいずれもhardware_interface::HardwareResourceManagerクラスのサブクラスのインスタンスです.)
    • 後々ControllerHardwareInterfaceを介してやりとりできるように,JointStateInterfacePositionJointInterfaceのハンドル作成時に,joint にまつわるメンバ変数(jnt_pos_, jnt_vel_, jnt_eff_, jnt_pos_cmd_)のポインタを登録しています.
  • readSim()
    • 現在のGazeboにおけるロボットモデルのjoint の状態(位置,速度,トルク)を取得して,先ほどinitSim()で渡したメンバ変数を介してHardwareInterfaceに取得した情報を教えてあげます.
  • writeSim()
    • Gazeboにおけるロボットモデルのjoint に指令値を送ります.今回のサンプルの場合,hardware_interface::PositionJointInterface jnt_pos_cmd_interface_で取得した位置指令から力指令を計算し,Gazeboに送ります.

上述のイメージを図で表すと,下図のようになります.

RobotHWSim_HardwareInterface.png

ここまで見れば,この3つのメソッドが,プラグインの動作を決定づける重要項目であることが理解できます.

プラグインで定義された処理が実行される場所

では,これらのメソッドはいつ呼ばれるのでしょうか?

ROSGazeboを連携させると,GazeboRosControlPluginという,GazeboのプラグインとROS間のインターフェースを制御するためのクラスが働いてくれます.ロボットモデルをSpawnする際に,robot_descriptionの情報が渡されます.使用するプラグインの情報がその中に組み込まれているので,GazeboRosControlPluginはどのプラグインを使えば良いのかを知ることができます.

では,上記プラグインのメソッドが実際に呼ばれる場所がどこになるのかが気になります.調べると,下記のリポジトリにありました.
github: ros-controls/gazebo_ros_control

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp
  // 省略
// Called by the world update start event
void GazeboRosControlPlugin::Update()
{
  // Get the simulation time and period
  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
  ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
  ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;

  robot_hw_sim_->eStopActive(e_stop_active_);

  // Check if we should update the controllers
  if(sim_period >= control_period_) {
    // Store this simulation time
    last_update_sim_time_ros_ = sim_time_ros;

    // Update the robot simulation with the state of the gazebo model
    robot_hw_sim_->readSim(sim_time_ros, sim_period);

    // Compute the controller commands
  // 省略
    controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
  }

  // Update the gazebo model with the result of the controller
  // computation
  robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
  last_write_sim_time_ros_ = sim_time_ros;
}
  // 省略

Update()メソッドはシミュレーション周期毎にコールバックされます.本コールバック関数の登録は同クラスのメソッドであるLoad()で行われています.

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp
// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
  // 省略
    // Listen to the update event. This event is broadcast every simulation iteration.
    update_connection_ =
      gazebo::event::Events::ConnectWorldUpdateBegin
      (boost::bind(&GazeboRosControlPlugin::Update, this));
  // 省略
}

Load()は,プラグインのエントリポイントであり,本メソッド内でGazeboROSを連携するための様々な初期化が行われています.

さて,話を戻して,肝心のUpdate()メソッドの大きな流れは次の通りです.

  1. シミュレーション周期と制御周期を比較します
    • 制御周期分時間が進んでいたら,Controllerの状態を更新するため,以降の処理に移ります.
  2. Gazeboモデルの状態を取得し,Controllerで使用するロボットモデルの状態を更新します.
    • ここで,RobotHWSim::readSim()の登場です.プラグインのコードでオーバライドしたメソッドがここで走ります.
    • 具体的には,前述した通り,jnt_pos_, jnt_vel_, jnt_eff_を更新します.
  3. 更新したロボットモデルの情報を元に,Controllerによる制御則の計算を実行します.
    • controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs); の部分です.controller_manager_は,controller_manager::ControllerManagerクラスのインスタンスです.
    • 本インスタンスは自分が管理すべきControllerを全て知っているので,ControllerManager::update()メソッド一つで全コントローラを更新できます.
    • 例えば,ControllerJointTrajectoryControllerであれば,前述したjnt_pos_cmd_が更新されるのがここです.
  4. Controllerで算出した結果を,Gazeboモデル側に反映させます.
    • ここで,RobotHWSim::writeSim()が登場します.
    • 今回の例では,位置指令であるjnt_pos_cmd_を元に力指令を計算して,Gazebo側に書き込んでいます.

RobotHWSinRobotHWとの対比

参考として,Gazebo利用時に使われるRobotHWSimを,実機適用時に使われるRobotHWと対比させます.RobotHWのWikiに記載されたmain()の記述を見れば,シミュレータ側との違いを比較でき,理解が深まると思います.

declaration_of_MyRobot
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW
{
 // 省略
};

hardware_interface::RobotHWを継承して,MyRobotというオリジナルロボット用のサブクラスを作成しています.
これをどう使うのか.main()を見てみます.

main_routine
main()
{
  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();
     sleep();
  }
}

read()write()は,前述したreadSim()writeSim()と対応します.ただし,実際のロボットのハードウェアの仕様(read()ならエンコーダ等,write()ならアクチュエータ等)に合わせて,自分でドライバ部分を実装する必要があります.
cm.update()は,まさに前述のcontroller_manager_->update()と同じ役割です.cmの宣言の部分を見ると,controller_manager::ControllerManagerのコンストラクタの引数にMyRobot robotのポインタを渡しています.cmは,robotの情報を全部知っていることになります.

実は,ここが重要です.詳細な挙動は後述しますが,ControllerManager側は,RobotHWまたはRobotHWSimの情報を知らないと,にっちもさっちもいかないことになるのです.実際,先ほどのgazebo_ros_control_plugin.cppの中でも,ControllerManagerの初期化はLoad()メソッド内で下記のように行われています.

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp
// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
 // 省略
    // Create the controller manager
    ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
    controller_manager_.reset
      (new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
 // 省略
}

RobotHWRobotHWSimでは,下記のような違いが有ります.

  • RobotHWを使った先ほどのサンプルでは,ControllerManagerの初期化時にコンストラクタでRobotHWのインスタンスのポインタ渡していました.
  • GazeboRosControlPluginでは,reset()というメソッドでRobotHWSimの情報をControllerManagerに渡しています.
    • コンストラクタの段階で,どんなロボットがやってくるのか,GazeboRosControlPluginでは知ったこっちゃありません.
    • 入れ物だけ作って,後でロボットの情報を上書きしてあげるイメージです.

robothw_robothwsim_controllermanger_init.png

いずれにしろ,ControllerManagerにロボットモデルの情報を渡していることは,共通しているようです.

以上が,プラグインの動作の理解を深めるために解析した内容です.

ただ,上述した説明ではRobotHWの具体的な実装は省略されています.そこで,本節の最後にRobotHWの良いサンプルについてご紹介します.

  • minimum_ros_control(Github: yoneken/minimum_ros_control)
    • RobotHWのサンプルとしては,yonekenさんによるminimum_ros_controlというリポジトリが有用ですので,ここでご紹介させていただきます.
    • このサンプルでは,RobotHWを使う上で必要となる最低限の要素で構成された,非常にシンプルなロボットが扱われている,極めて見通しの良い素晴らしいサンプルです.RobotHWの理解を深める上で,ぜひ一読することをオススメします.
    • 実機を組むときにはこのminimum_ros_controlをベースにカスタマイズしていくと,効率的に開発ができると思います.

ここで,gazebo_ros_pkgGazebo内のロボットを制御する場合と,minimum_ros_controlで実機を制御する場合のソフトウェア構成図を比較してみます.


gazebo_summary.png

real_robot_sammary.png

モジュールとして異なる部分は,上記図中において緑の矢印で示した部分ですが,本質的には全く同一の構成を取っていることが分かります.
使用するコントローラが,JointTrajectoryControllerJointPositionControllerで異なっていますが,これは設定というかロボットの制御の仕方の違いというレベルで,GazeboだろうがReal Robotだろうが共通で入れ替えできる部分です.ただ,周期やゲインの設定がある場合には,シミュレーションか実機かで設定値を切り替えるのが実用的でしょう.この辺りの話は,
次のエントリでも触れます.

プラグインとして使えるようにする

ここまで,プラグインの定義方法とその仕組みについて述べました.しかし,実はこれまでの作業だけではまだ使える状態にはありません.ROSシステム側に対して,プラグインを有効にするための情報を教えてあげる必要があります.
そこで,pluginlibの仕組みを使って,上記コードを正式にプラグイン化します.

  • プラグインとして出力するための設定
    • プラグイン用のファイルとして出力されるように,コード側に追記が必要です.
    • コード内のどこでもよいので,PLUGINLIB_EXPORT_CLASS(登録対象のクラス名, 登録対象のクラスの親クラス名)のマクロを書きます.
    • 先ほどの例では,最終行に書いてありました.これが一般的なやり方とのことです.
rosbook_arm_hardware_gazebo/src/rosbook_arm_hardware_gazebo.cpp
// ぜーんぶ省略して,最終行のみ
PLUGINLIB_EXPORT_CLASS(rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo, gazebo_ros_control::RobotHWSim)
  • プラグイン説明ファイルの登録
    • 上記マクロでは書ききれない情報を登録します.プラグインの仕様とかはこちらです.
    • ROSシステムがこのプラグインを自動で検出・読込できるようにするために必要とのことです.
    • rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazeboの例では下記のとおりです.
rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo_plugins.xml
<library path="lib/librosbook_arm_hardware_gazebo">
  <class
    name="rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo"
    type="rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo"
    base_class_type="gazebo_ros_control::RobotHWSim">
    <description>
      ROS Book arm simulation interface.
    </description>
  </class>
</library>
  • ROSパッケージシステムへのプラグインの登録
    • ROSシステム全体でこのプラグインを使えるようにするための設定をします.
    • package.xml<export>タグを追加します.
    • rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazeboの例では下記のとおりです.
rosbook_arm_hardware_gazebo/package.xml
  <export>
    <gazebo_ros_control plugin="${prefix}/rosbook_arm_hardware_gazebo_plugins.xml"/>
  </export>

ファイルとしての設定は以上です.以降の処理はpluginlibを参考に進めればOKです.

DefaultRobotHWSimについて

作成したROSBookArmHardwareGazeboでは,JointStateInterfacePositionJointInterfaceを自分で宣言しましたが,DefaultRobotHWSimではどうしているのでしょうか?デフォルトで用意されたプラグインを使う場合,私達が編集をする余地はありません.
ちょっとコードを覗いてみます.

gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h
// 省略
namespace gazebo_ros_control
{
class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
// 省略
{
// 省略
  hardware_interface::JointStateInterface    js_interface_;
  hardware_interface::EffortJointInterface   ej_interface_;
  hardware_interface::PositionJointInterface pj_interface_;
  hardware_interface::VelocityJointInterface vj_interface_;
// 省略
}
// 省略
}

JointStateInterfaceEffortJointInterfacePositionJointInterfacemVelocityJointInterfaceのインスタンスの宣言がオンパレードです.
ユーザ定義の設定ファイルからどんなインターフェースが降ってくるかなど,DefaultRobotHWSimからしたら分からないことなので,代表的なインターフェースについては予め土台を作っておくようです.これをどう扱うのか,initSim()を見てみます.

gazebo_ros_control/src/default_robot_hw_sim.cpp
// 省略
namespace gazebo_ros_control
{
// 省略
bool DefaultRobotHWSim::initSim(
  const std::string& robot_namespace,
  ros::NodeHandle model_nh,
  gazebo::physics::ModelPtr parent_model,
  const urdf::Model *const urdf_model,
  std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// 省略
  n_dof_ = transmissions.size(); 
// 省略
  // Initialize values 
  for(unsigned int j=0; j < n_dof_; j++) 
  { 
// 省略
    std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
// 省略
    const std::string& hardware_interface = joint_interfaces.front();
// 省略
    // Add data from transmission
    joint_names_[j] = transmissions[j].joints_[0].name_;
// 省略
    if(hardware_interface == "EffortJointInterface")
    {
      // Create effort joint interface
      joint_control_methods_[j] = EFFORT;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_effort_command_[j]);
      ej_interface_.registerHandle(joint_handle);
    }
    else if(hardware_interface == "PositionJointInterface")
    {
      // Create position joint interface
      joint_control_methods_[j] = POSITION;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_position_command_[j]);
      pj_interface_.registerHandle(joint_handle);
    }
    else if(hardware_interface == "VelocityJointInterface")
    {
      // Create velocity joint interface
      joint_control_methods_[j] = VELOCITY;
      joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
                                                     &joint_velocity_command_[j]);
      vj_interface_.registerHandle(joint_handle);
    }
// 省略
  }
// 省略
}
// 省略
}

URDF<transmission>タグ内の<hardwareInterface>タグで指定したインターフェース種別を読みだして,その種別に応じて登録対象のインターフェースを切り替えています.だから,Gazeboを使うときには,<transmission>の設定でHardwareInterfaceの種別を入力する必要があったのです.

おわりに

今回は,RobotHWSimの動作にフォーカスを当てて解析を行いました.プラグインの作成方法と絡めながら,周辺のモジュールとどのように関わっているかも少し理解できるようになりました.

次回は,「3. Controllerについて」説明します.ここまで分かれば,GazeboROSの連携に関する,一通りの流れがつかめるようになります.

9
Help us understand the problem. What is going on with this article?
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
MoriKen
社会人博士課程で博士(工学)になりました。頑張ります。

Comments

No comments
Sign up for free and join this conversation.
Sign Up
If you already have a Qiita account Login
9
Help us understand the problem. What is going on with this article?