二足歩行ロボット研修二足歩行ロボット研修（kora編）
ROSros_control

二足歩行ロボット研修（kora編）[20] ros_controlについて

二足歩行ロボット研修（kora編）[19] DYNAMIXEL Workbenchでモーションを再生する 二足歩行ロボット研修

こんにちは。koraです。

前回はロボットにモーションを再生させるところまでできました。しかし、この方法では予め定められたモーションしか再生できません。計算歩行をしたいと考えているので、動的にロボットの姿勢を制御する方法が必要です。そこで、今回はros_controlについて調査します。

ROScon 2014のスライド、ros_control Wiki、B3MでわかるROS CONTROLなどを参考にします。

ros_controlとは

ros_controlはROSにおける汎用的なロボットの制御用パッケージです。


引用：https://wiki.ros.org/ros_control

図だけではわかりにくいですが、重要なのはControllerとHardwareInterfaceです。ControllerはHardwareInterfaceを通してセンサ（エンコーダなど）から制御対象の状態（制御量）を受け取り、ROSインターフェース（メッセージなど）を通して指令値（目標値）を受け取ります。そして、Controllerはこれらの値からPID制御などで操作量を計算し、HardwareInterfaceを通してアクチュエータ（サーボモータなど）を操作します。

Controller

ros_controlに備え付けられたControllerには次のようなものがあります。独自のControllerを作ることもできます。
なお、アクチュエータが回転運動する場合、positionは角度、velocityは角速度、effortはトルクを意味します。

  • position_controllers: アクチュエータが位置制御のサーボモータなどの場合に使います。目標値としてpositionを受け取り、HardwareInterfaceに渡します。
  • velocity_controllers: アクチュエータが速度制御の場合に使います。目標値としてvelocityあるいはpositionを受け取り、velocityの場合はそのまま渡し、positionの場合はPID制御によってvelocityを計算して渡します。
  • effort_controllers: アクチュエータがトルク制御の場合に使います。目標値としてeffort、velocityあるいはpositionを受け取り、effortの場合はそのまま渡し、velocityあるいはpositionの場合はPID制御によってeffortを計算して渡します。
  • joint_state_controller: このControllerは特殊で、HardwareInterfaceに登録されているリソースの状態をパブリッシュします。
  • etc

HardwareInterface

HardwareInterfaceには次のようなものがあります。独自のHardwareInterfaceを作ることもできます。

  • Position Joint Interface: position_controllersと対応します。
  • Velocity Joint Interface: velocity_controllersと対応します。
  • Effort Joint Interface: effort_controllersと対応します。
  • Joint State Interface: joint_state_controllerと対応します。
  • etc

ros_controlの簡単なサンプル

ros_controlの動きを理解するために簡単なサンプルを作成します。

ロボットの定義

まず、制御するロボットのハードウェアを定義します。例として、2個の位置制御のサーボ（AとB）を持つロボット（MyRobot）を考えます。

ここではサーボの情報を取得するためのJointStateInterfaceと、サーボに送る指令値を取得するためのPositionJointInterfaceを定義します。コンストラクタ内で、それぞれのサーボの情報を格納する変数のアドレスをJointStateInterfaceに登録し、指令値を格納する変数のアドレスをPositionJointInterfaceに登録しています。

本来であればread()関数でサーボの状態を取得し、write()関数でサーボに司令値を送ります。今回は簡単のためにサーボとの通信は行わず、指令値を表示するだけにしています。

#include <ros/ros.h>
#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
{
public:
  MyRobot();
  ~MyRobot();
  void read();
  void write();
private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

MyRobot::MyRobot()
{
  // connect and register the joint state interface
  hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
  jnt_state_interface.registerHandle(state_handle_a);

  hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
  jnt_state_interface.registerHandle(state_handle_b);

  registerInterface(&jnt_state_interface);

  // connect and register the joint position interface
  hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);
  jnt_pos_interface.registerHandle(pos_handle_a);

  hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
  jnt_pos_interface.registerHandle(pos_handle_b);

  registerInterface(&jnt_pos_interface);
}

MyRobot::~MyRobot()
{
}

void MyRobot::read()
{
  pos[0] = 0.0;
  pos[1] = 0.0;
  vel[0] = 0.0;
  vel[1] = 0.0;
  eff[0] = 0.0;
  eff[1] = 0.0;
}

void MyRobot::write()
{
  ROS_INFO_STREAM("A command " << cmd[0] << " B command " << cmd[1]);
}

ROSノードの作成

次に、フィードバックループを回すROSノードを作成します。このノードはControllerを管理するControllerManagerと、先程定義したロボットのインスタンスを持ちます。Controllerはノードの起動が起動した後、ROSサービスを介してロードします。あとは①ロボットのread()、②ControllerManagerのupdate()、③ロボットのwrite()を繰り返すことでフィードバックループが実現します。（position_controllersを使う場合は指令値をそのまま伝えるのでフィードバック制御をしているわけではありませんが、他のControllerを使用する場合はここでフィードバック制御が実行されます。）

ちなみに、通常のC++のROSノードはシングルスレッドのため、ROSインターフェースのコールバックで遅延が発生するとフィードバックループの周期が乱れてしまいます。これを防ぐため、ros::AsyncSpinnerを使ってマルチスレッド化し、非同期的にコールバックが実行されるようにします。

#include <ros/ros.h>
#include <controller_manager/controller_manager.h>

#include <my_robot_control/my_robot.h>

int main( int argc, char* argv[] )
{
  ros::init(argc, argv, "my_robot");

  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  ros::AsyncSpinner spinner(1);
  spinner.start();

  ros::Time t = ros::Time::now();
  ros::Rate rate(10.0);

  while (ros::ok())
  {
    ros::Duration d = ros::Time::now() - t;
    ros::Time t = ros::Time::now();

    robot.read();
    cm.update(t, d);
    robot.write();

    rate.sleep();
  }
  return 0;
}

ROSノードの起動

使用するコントローラを定義する設定ファイルを作成します。

joint_state_controller:
  type: "joint_state_controller/JointStateController"
  publish_rate: 10

position_controller_a:
  type: "position_controllers/JointPositionController"
  joint: A

position_controller_b:
  type: "position_controllers/JointPositionController"
  joint: B

ノードを起動するlaunchファイルを作成します。先ほど作成した設定ファイルもここで読み込みます。
なお、ここでspawnerというノードも起動していますが、これは起動時にControllerをロードさせ、終了時にアンロードさせるためのノードです。

<launch>
  <group ns="my_robot">
    <rosparam file="$(find my_robot_control)/config/my_robot_control.yaml" command="load"/>

    <node name="controller_manager"
        pkg="controller_manager"
        type="spawner" respawn="false"
        output="screen"
        args="joint_state_controller
              position_controller_a
              position_controller_b"/>

    <node name="my_robot_control" pkg="my_robot_control" type="my_robot_control" output="screen" respawn="false" />
  </group>
</launch>

起動すると、次のような出力が得られます。指令値の変数を初期化していなかったので起動直後はいい加減な値が出力されていますが、Controllerのロードが完了すると初期化されていることが分かります。

$ roslaunch my_robot_control my_robot_control.launch

別のターミナルを開いて指令値を入力します。例として5を入力すると出力にも反映されることが確認できました。

$ rostopic pub /my_robot/position_controller_a/command std_msgs/Float64 "data: 5.0"

おわりに

今回は機能を限定した単純なサンプルを作成しました。やっていることは受け取ったROSメッセージを表示しているだけですが、ROSの標準的な入出力の流れがわかったことで、今後、MoveIt!やGazeboなどのROSパッケージとの連携や、第三者の作成した機能などが利用しやすくなると思います。

タイトルとURLをコピーしました