GAZEBO: 2自由度ロボットアーム

2DOF robot arm by gazebo

この記事が私が担当しているロボットプログラミングⅡの講義用です。

今回は2自由度のロボットアームモデルを作成し、キーボード操作により動かすサンプルです。

1. 作業ディレクトリの作成
端末を開き、以下のコマンドを実行する($は打ち込まない)。ロボットプログラミングの演習では~/prog2ディレクトリ(フォルダ)の中にいろいろなプログラムを作っていく。

$ mkdir -p ~/prog2/6arm
$ cd ~/prog2/6arm
$ gedit arm.cc

2. ソースコードの作成
プラグインとなるC++言語のソースコード(拡張子cc)を作成する。

以下のプログラムを「1.作業ディレクトリの作成」で開いたgedit(エディター)に次のソースコードをコピペしてarm.ccを保存する。

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/msgs/MessageTypes.hh>
#include <gazebo/common/Time.hh>
#include <stdio.h>

namespace gazebo
{
class MobileBasePlugin : public ModelPlugin
{
public:
    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        physics::WorldPtr world = physics::get_world("default");

        // modelへポインタを格納
        this->model = _parent;

        hinge1 = this->model->GetJoint("hinge1");
        hinge2 = this->model->GetJoint("hinge2");
        sensor = this->model->GetLink("sensor");

        for (int i=0; i< 3; i++)
        {
            THETA[i] = 0;
        }
        // このプラグイン用のパラメータをロード
        if (this->LoadParams(_sdf))
        {
            // アップテートイベントを聞く。シミュレーションの繰り返し時に
            // このイベントはブロードキャストされる。
            this->updateConnection
            = event::Events::ConnectWorldUpdateBegin(
                  boost::bind(&MobileBasePlugin::OnUpdate, this));
        }
    }
 
    bool LoadParams(sdf::ElementPtr _sdf)
    {
        // 制御用のgainパラメータを見つける
        if (!_sdf->HasElement("gain"))
        {
            gzerr << "param [gain] not found\n";
            return false;
        }
        else
        {
            // gainの値を取得
            this->gain = _sdf->Get<double>("gain");
        }
 
        // 成功時
        return true;
    }
 
    // getch関数は以下のウェブサイトの記事を使用
    // http://answers.ros.org/question/63491/keyboard-key-pressed/
    int getch()
    {
        static struct termios oldt, newt;
        tcgetattr(STDIN_FILENO, &oldt);           // save old settings
        newt = oldt;
        newt.c_lflag &= ~(ICANON);                 // disable buffering
        tcsetattr(STDIN_FILENO, TCSANOW, &newt);  // apply new settings
 
        int c = getchar();  // read character (non-blocking)
 
        tcsetattr(STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
        return c;
    }
 
    /*** キー入力関数 ***/
    void command()
    {
        int cmd = getch();
 
        switch (cmd)
        {
        case 'j':
            THETA[1] += M_PI/180;
            break;     // jキー
        case 'f':
            THETA[1] -= M_PI/180;
            break;     // fキー
        case 'k':
            THETA[2] += M_PI/180;
            break;     // kキー
        case 'd':
            THETA[2] -= M_PI/180;
            break;     // dキー
        default:
            std::cout << "Input j,f,k,d" << std::endl;
        }
    }
 
    /*** センサー情報の表示 ***/
    void printSensor()
    {
        // 関節角表示
        math::Angle angle1 = hinge1->GetAngle(0);   // 関節角の取得
        math::Angle angle2 = hinge2->GetAngle(0);
        // 度表示
        printf("angle1=%6.3f angle2=%6.3f ", angle1.Degree(), angle2.Degree());
        // radian表示
        //printf("angle1=%f angle2=%f", angle1.Radian(), angle2.Radian());
 
        // 手先位置表示
        math::Pose pose = sensor->GetWorldCoGPose();
        printf("x=%6.3f y=%6.3f \n", pose.pos.x,pose.pos.y);
    }
 
    /*** 関節を動かす ***/
    void moveJoint()
    {
        hinge1->SetAngle(0, THETA[1]);
        hinge2->SetAngle(0, THETA[2]);
    }
 
    // ワールド更新開始イベントから呼び出される
    void OnUpdate()
    {
        command();    // キー入力受付
        moveJoint();  // 関節の動かす
        printSensor();
    }
 
private:
    // モデルへのポインタ
    physics::ModelPtr model;
    physics::WorldPtr world;
 
    // ワールド状態のサブスクライブ(講読)
    transport::NodePtr node;
    transport::SubscriberPtr statsSub;
    common::Time simTime;
 
    // 更新イベントコネクションへのポインタ
    event::ConnectionPtr updateConnection;
    physics::JointPtr hinge1, hinge2;
    sensors::RaySensorPtr laser;
    physics::LinkPtr  sensor;
    double THETA[3]; // 関節目標角度 THETA[0]は未使用
    double gain;     // 比例ゲイン
};
 
// シミュレータへのプラグイン登録
GZ_REGISTER_MODEL_PLUGIN(MobileBasePlugin)
}

コピペしたらgeditの「保存」ボタンをクリックして~/prog2/6arm/の中に保存し、geditメニューバーの「ファイル(F)」→「終了(Q)」でgeditを終了する。

3. コンパイル設定ファイルの作成
gazeboではソースコードをコンパイル(ビルド)するのにcmakeというシステムを使う。先程から使っている端末で以下のコマンドを実行しgeditを起動する。

$ gedit CMakeLists.txt

CMakeLists.txtはcmakeの設定ファイル。次のソースコードをgeditにコピペし、CMakeLists.txtを保存する。保存場所は今までと同じ~/prog2/6armディレクトリ。保存したらgeditを終了する。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(Boost REQUIRED COMPONENTS system)
include_directories(${Boost_INCLUDE_DIRS})
link_directories(${Boost_LIBRARY_DIRS})

include (FindPkgConfig)
if (PKG_CONFIG_FOUND)
  pkg_check_modules(GAZEBO gazebo)
endif()
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})

add_library(arm SHARED arm.cc)
target_link_libraries(arm ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})

4.コンパイル
以下のコマンドを実行し、ソースコードをメイクする。

$ cd ~/prog2/6arm
$ mkdir build
$ cd build
$ cmake ../
$ make

5. モデルファイルの作成
以下のコマンドを実行し、モデルファイルを作成する。

$ cd ~/.gazebo/models
$ mkdir arm_robot1

以下のmodel.configファイルをgeditにコピペし、model.configファイルと名前を付けてarm_robot1ディレクトリに保存する。

<?xml version="1.0"?>
<model>
  <name>Arm Robot 1</name>
  <version>1.0</version>
  <sdf version='1.4'>model.sdf</sdf>

  <author>
   <name>My Name</name>
   <email>My mail address</email>
  </author>

  <description>
    My arm robot.
  </description>
</model>

同様にして、以下のmodel.sdfファイルを保存する。

<?xml version='1.0'?>
<sdf version='1.4'>

  <!-- アームロボット1 -->
  <model name="arm_robot1">
    <static>false</static> <!-- trueにすると静的モデル -->
    <link name='base'> <!-- リンク:土台 -->
    <pose>0 0 0.05 0 0 0 </pose> <!-- 位置(x,y,z)と姿勢(roll,pitch,yaw) -->
    <visual name='visual'> <!-- 表示用 -->
      <geometry> <!-- 形状情報 -->
	<cylinder> <!-- 円柱 -->
          <radius>0.04</radius> <!-- 半径 -->
          <length>0.1</length>  <!-- 長さ -->
        </cylinder>
      </geometry>
    </visual>
    </link>

    <link name='link1'> <!-- リンク1 -->
    <self_collide>0</self_collide>
    <pose>0.250 0 0.04 0 1.5707 0</pose> <!-- 位置(x,y,z)と姿勢(roll,pitch,yaw) -->
    <inertial>
      <inertia>
	<ixx>0.01</ixx>
	<ixy>0</ixy>
	<ixz>0</ixz>
	<iyy>0.01</iyy>
	<iyz>0</iyz>
	<izz>0.01</izz>
      </inertia>
      <mass>2.0</mass>
    </inertial>
    <visual name='visual'> <!-- 表示用 -->
      <geometry> <!-- 形状情報 -->
	<cylinder> <!-- カプセル -->
	  <radius>0.04</radius> <!-- 半径 -->
	  <length>0.5</length>  <!-- 長さ -->
	</cylinder>
      </geometry>
    </visual>
    </link>

    <link name='link2'> <!-- リンク2 -->
    <self_collide>0</self_collide>
    <pose>0.75 0 0.04 0 1.5707 0</pose> <!-- 位置(x,y,z)と姿勢(roll,pitch,yaw) -->
    <inertial>
      <inertia>
        <ixx>0.01</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.01</iyy>
        <iyz>0</iyz>
        <izz>0.01</izz>
      </inertia>
      <mass>2.0</mass>
    </inertial>

    <visual name='visual'> <!-- 表示用 -->
        <geometry> <!-- 形状情報 -->
          <cylinder> <!-- カプセル -->
            <radius>0.04</radius> <!-- 半径 -->
            <length>0.5</length>  <!-- 長さ -->
          </cylinder>
	</geometry>
      </visual>
  </link>

  <link name='sensor'> <!-- 位置センサ -->
    <pose>1.0 0 0.04 0 0 0 </pose> <!-- 位置(x,y,z)と姿勢(roll,pitch,yaw) -->
    <visual name='visual'> <!-- 表示用 -->
      <geometry> <!-- 形状情報 -->
        <sphere> <!-- 円柱 -->
          <radius>0.004</radius> <!-- 半径 -->
        </sphere>
      </geometry>
    </visual>
  </link>

  <!-- ジョイント(関節) revoluteは回転式(ヒンジ)-->
  <joint type="revolute" name="hinge0"> <!--土台を地面に固定 -->
    <child>base</child> <!-- 子(下位)リンク -->
    <parent>world</parent>  <!-- 親(上位)リンク -->
    <axis>
      <limit>
          <lower>0</lower>
          <upper>0</upper>
      </limit>
      <xyz>0 0 1</xyz> <!-- 回転軸ベクトル(x,y,z) -->
    </axis>
  </joint>

  <joint type="revolute" name="hinge1"> <!--第1関節 --> 
    <pose>0 0 -0.25 0 0 0</pose>
    <child>link1</child> <!-- 子(下位)リンク -->
    <parent>base</parent>  <!-- 親(上位)リンク -->
    <axis>
      <xyz>0 0 1</xyz> <!-- 回転軸ベクトル(x,y,z) -->
    </axis>
  </joint>

  <joint type="revolute" name="hinge2"> <!--第2関節 -->
    <pose>0 0 -0.25 0 0 0</pose>
    <child>link2</child> <!-- 子(下位)リンク -->
    <parent>link1</parent>  <!-- 親(上位)リンク -->
    <axis>
      <xyz>0 0 1</xyz> <!-- 回転軸ベクトル(x,y,z) -->
    </axis>
  </joint>

  <joint type="revolute" name="sensor_joint"> <!--センサの固定 -->
    <pose>0.02 -0.02 -0.2 0 0 0</pose>
    <child>sensor</child> <!-- 子(下位)リンク -->
    <parent>link2</parent>  <!-- 親(上位)リンク -->
    <axis>
      <limit>
          <lower>0</lower>
          <upper>0</upper>
      </limit>
      <xyz>0 0 1</xyz> <!-- 回転軸ベクトル(x,y,z) -->
    </axis>
 </joint>

 <plugin name="arm" filename="libarm.so" >
      <hinge1>hinge1</hinge1>
      <hinge2>hinge2</hinge2>
      <gain>0.1 </gain>
      <sensor>sensor</sensor>
      <ray_sensor>laser</ray_sensor>
 </plugin>
  </model>
</sdf>

6. 実行
以下のコマンドを実行し、gazeboを立ち上げよう! ここでは、gazeboを起動するのに–verboseのオプションをつけます。これは、標準出力に位置や姿勢を表示するためです。

gazeboが起動したら画面左側のinsert(挿入)タブから制作したarm robot 1を選択する。
d, f, j, kキーを入力するとロボットアームの関節が動いたら成功。今回はここまで。

$ export GAZEBO_PLUGIN_PATH=~/prog2/6arm/build:$GAZEBO_PLUGIN_PATH
$ gazebo --verbose

〇 Exercise
2自由度ロボットアームのサンプルプログラムに以下を実装しよう!

  • 運動学
    運動学の計算結果と位置センサの出力を比較し、プログラムが正しいか確認する。
  • 逆運動学
    逆運動学を実装しよう。
    任意の位置に直方体を表示し、アーム先端がそれを指すようなプログラムを実装する。2自由度の場合は解が2つあるので、数字キー1を押すと解1、数字キー2を押すと解2を選択する。

コメントを残す

メールアドレスが公開されることはありません。