GAZEBO: 4脚ロボット

legged2

  • ロボットモデルに不具合があったので修正しました。これlegged_robot_model150115.tgzを使用してください(2015-01-15)。
  • 授業時のエラーを訂正しました。変更はlegged.ccだけです。エラーの理由はGazeboバージョンによりAPIの仕様が変更になったためです(15年1月10日)。 

この記事が私が担当しているロボットプログラミングⅡの講義用です。
今回は4脚ロボットモデルを作成し、クロール歩行のシミュレーションを行います。これは拙著「簡単!実践!ロボットシミュレーション Open Dynamics Engineによるロボットプログラミング」のプログラム8.1 legged.cpp (P210)をGAZEBOに移植したものです。理論的な説明や式などは拙著をご覧頂ければ幸いです。

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

$ mkdir -p ~/prog2/7legged
$ cd ~/prog2/7legged
$ gedit legged.cc

2. 4脚ロボットのソースコード
プラグインとなるC++言語のソースコード(拡張子cc)を作成する。以下のプログラムを「1.作業ディレクトリの作成」で開いたgedit(エディター)に次のソースコードをコピペしてlegged.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>

#define LINK_NUM 3  // 全リンク数 (total number of links)
#define JT_NUM   3  // 全ジョイント数 (total number of joints)                                           
#define LEG_NUM  4  // 全脚数 (total number of legs) 

namespace gazebo
{
class MobileBasePlugin : public ModelPlugin
{
public:
  MobileBasePlugin()
  {
    l1 = 1.5*0.05, l2 = 0.3, l3  = 0.3;  // leg length
    r1 = 0.02, r2 = 0.02, r3 = 0.02 ;    // leg radius
    
    for (int i=0; i< 4; i++) {
      for (int j=0; j < 3; j++) { 	 				
         THETA[i][j] = 0; 			
      } 		
    } 	
   } 	

   void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 	
   { 		
      physics::WorldPtr world = physics::get_world("default"); // modelへポインタを格納
      this->model = _parent;
    
      hinge_lf1 = this->model->GetJoint("hinge_lf1");
      hinge_lf2 = this->model->GetJoint("hinge_lf2");
      hinge_lf3 = this->model->GetJoint("hinge_lf3");
      hinge_lr1 = this->model->GetJoint("hinge_lr1");
      hinge_lr2 = this->model->GetJoint("hinge_lr2");
      hinge_lr3 = this->model->GetJoint("hinge_lr3");
      hinge_rr1 = this->model->GetJoint("hinge_rr1");
      hinge_rr2 = this->model->GetJoint("hinge_rr2");
      hinge_rr3 = this->model->GetJoint("hinge_rr3");
      hinge_rf1 = this->model->GetJoint("hinge_rf1");
      hinge_rf2 = this->model->GetJoint("hinge_rf2");
      hinge_rf3 = this->model->GetJoint("hinge_rf3");
       
    // このプラグイン用のパラメータをロード
    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;
  }
  
  /*** 逆運動学の計算 (calculate inverse kinematics ***/
  void  inverseKinematics(double x, double y, double z,
    double *ang1, double *ang2, double *ang3,int posture)
  {
    double l1a = 0, l3a = l3 + r3/2;
    double c3 = (x*x + z*z + (y-l1a)*(y-l1a) - (l2*l2+l3a*l3a))/(2*l2*l3a);
    double s2 = (y-l1a) / (l2 + l3a*c3);
    double c2 = sqrt(1 - s2 * s2);
    double c1 = (l2 + l3a*c3)*c2/sqrt(x*x+z*z);
    // printf("c3=%f s2=%f c2=%f c1=%f \n", c3,s2,c2,c1);
    if (sqrt(x*x+y*y+z*z) > l2 + l3) {
      printf(" Target point is out of range \n");
    }
    
    switch (posture) {
    case 1: // 姿勢1 (posture 1)
      *ang1 =   atan2(x,-z) - atan2(sqrt(1 - c1*c1),c1);
      *ang2 = - atan2(s2,c2);
      *ang3 =   atan2(sqrt(1-c3*c3),c3); break;
    case 2: // 姿勢2 (posture 2)
      *ang1=   atan2(x,-z) + atan2(sqrt(1 - c1*c1),c1);
      *ang2= - atan2(s2,c2);
      *ang3= - atan2(sqrt(1-c3*c3),c3); break;
    case 3:  // 姿勢3 (posture 3)
      *ang1 =   M_PI + (atan2(x,-z) - atan2(sqrt(1 - c1*c1),c1));
      *ang2 = - M_PI +  atan2(s2,c2);
      *ang3 = - atan2(sqrt(1-c3*c3),c3); break;
    case 4:  // 姿勢4 (posture 4)
      *ang1 =  M_PI + atan2(x,-z) + atan2(sqrt(1 - c1*c1),c1);
      *ang2 = -M_PI + atan2(s2,c2);
      *ang3 =  atan2(sqrt(1-c3*c3),c3); break;
    }
  }


  /*** 関節を動かす ***/
  void moveJoint()
  {
    hinge_lf1->SetAngle(0, THETA[0][0]);
    hinge_lr1->SetAngle(0, THETA[1][0]);
    hinge_rr1->SetAngle(0, THETA[2][0]);
    hinge_rf1->SetAngle(0, THETA[3][0]);
    hinge_lf2->SetAngle(0, THETA[0][1]);
    hinge_lr2->SetAngle(0, THETA[1][1]);
    hinge_rr2->SetAngle(0, THETA[2][1]);
    hinge_rf2->SetAngle(0, THETA[3][1]);
    hinge_lf3->SetAngle(0, THETA[0][2]);
    hinge_lr3->SetAngle(0, THETA[1][2]);
    hinge_rr3->SetAngle(0, THETA[2][2]);
    hinge_rf3->SetAngle(0, THETA[3][2]);
  }

  /*** 歩行制御 (gait control) ***/
  void walk()
  {
    static int t = 0, steps = 0;
    int interval = 1000;
    
    if ((steps++ % interval)==0){
      t++;
    }
    else {  // 目標関節角度の設定 (set target gait angles)
      for (int leg_no = 0; leg_no < LEG_NUM; leg_no++) {
	for (int joint_no = 0; joint_no < JT_NUM; joint_no++) {
	  THETA[leg_no][joint_no] = gait[t%12][leg_no][joint_no];
	}
      }
    }
    moveJoint();
  }


  void calcAngle()  /*** 目標角度の計算 (Calculate target angles) ***/
  {
    double z0 = -0.4,z1 = -0.37; // z0:地面までの高さ(height to the ground),z1:最高到達点 (highest point)
    double y1 = 0.05, fs = 0.2;  // y1:左右の変位(defference between right and left),fs:歩幅 (foot step)
    double f1 = fs/4, f2 = fs/2, f3 = 3 * fs/4, f4 = fs;  // 一時変数 (temporal variables)
    double  traj[12][LEG_NUM][3] = { // 目標軌道点 (trajectory points)
      // leg0: left fore leg,  leg1: left rear leg
      // leg2: right rear leg, leg3: right fore leg
      // 左前leg0遊脚 左後leg1    右後 leg2    右前leg3
      {{ 0, y1,z0},{  0, y1,z0},{  0, y1,z0},{  0, y1,z0}},// 0 重心移動(move COG)
      {{f2, y1,z1},{  0, y1,z0},{  0, y1,z0},{  0, y1,z0}},// 1 離地(takeoff)
      {{f4, y1,z0},{  0, y1,z0},{  0, y1,z0},{  0, y1,z0}},// 2 着地(touchdown)
      {{f3,-y1,z0},{-f1,-y1,z0},{-f1,-y1,z0},{-f1,-y1,z0}},// 3 重心移動(move COG)
      //    leg0        leg1     leg2 遊脚(swing)  leg3
      {{f3,-y1,z0},{-f1,-y1,z0},{ f1,-y1,z1},{-f1,-y1,z0}},// 4 離地(takeoff)
      {{f3,-y1,z0},{-f1,-y1,z0},{ f3,-y1,z0},{-f1,-y1,z0}},// 5 着地(touchdown)
      {{f2,-y1,z0},{-f2,-y1,z0},{ f2,-y1,z0},{-f2,-y1,z0}},// 6 重心移動(move COG)
      //    leg0        leg1      leg2       leg3 遊脚(swing)
      {{f2,-y1,z0},{-f2,-y1,z0},{ f2,-y1,z0},{  0,-y1,z1}},// 7 離地(takeoff)
      {{f2,-y1,z0},{-f2,-y1,z0},{ f2,-y1,z0},{ f2,-y1,z0}},// 8 着地(touchdown)
      {{f1, y1,z0},{-f3, y1,z0},{ f1, y1,z0},{ f1, y1,z0}},// 9 重心移動(move COG)
      //   leg0    leg1 遊脚(swing)  leg2         leg3
      {{f1,y1, z0},{-f1, y1,z1},{ f1, y1,z0},{ f1, y1,z0}},// 10 離地(takeoff)
      {{f1,y1, z0},{ f1, y1,z0},{ f1, y1,z0},{ f1, y1,z0}} // 11 着地(touchdown)
    };
    
    double angle1, angle2, angle3;
    int posture = 2; // 姿勢(posture)
    for (int i = 0; i < 12; i++) {
      for (int k = 0; k < LEG_NUM; k++) {
	// 逆運動学
        inverseKinematics(traj[i][k][0],traj[i][k][1], 
	traj[i][k][2],&angle1, &angle2, &angle3,posture);
	gait[i][k][0] = angle1;
	gait[i][k][1] = angle2;
	gait[i][k][2] = angle3;
      }
    }
  }

  // ワールド更新開始イベントから呼び出される
  void OnUpdate()
  {
    static int epoch = 0;
    //std::cout << "epoch=" << epoch << std::endl;
    if (epoch++ == 0) calcAngle();
    walk();
  }

private:
  // モデルへのポインタ
  physics::ModelPtr model;
  physics::WorldPtr world;

  // ワールド状態のサブスクライブ(講読)
  transport::NodePtr node;
  transport::SubscriberPtr statsSub;
  common::Time simTime;

  // 更新イベントコネクションへのポインタ
  event::ConnectionPtr updateConnection;
  physics::JointPtr hinge_lf1, hinge_lf2, hinge_lf3; // 左前脚の関節
  physics::JointPtr hinge_lr1, hinge_lr2, hinge_lr3; // 左後
  physics::JointPtr hinge_rr1, hinge_rr2, hinge_rr3; // 右後
  physics::JointPtr hinge_rf1, hinge_rf2, hinge_rf3; // 右前
  //sensors::RaySensorPtr laser;
  //physics::LinkPtr  sensor;
  double THETA[LEG_NUM][LINK_NUM];    // 関節目標角度 
  double gait[12][LEG_NUM][JT_NUM] ;  // 目標角度(target angle of gait)                           
  double l1, l2, l3;  // 脚長 (lenth of links)   
  double r1, r2, r3;  // 脚半径(leg radius)
  double gain;     // 比例ゲイン
};

// シミュレータへのプラグイン登録
GZ_REGISTER_MODEL_PLUGIN(MobileBasePlugin)

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

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

$ gedit CMakeLists.txt

CMakeLists.txtはcmakeの設定ファイル。次のソースコードをgeditにコピペし、CMakeLists.txtを保存する。保存場所は今までと同じ~/prog2/7leggedディレクトリ。保存したら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(legged SHARED legged.cc)
target_link_libraries(legged ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})

4.コンパイル
以下のコマンドを実行し、ソースコードをメイクする。なお、以下を行ってうまくいかない場合は、7legged以下をまとめたこのファイル(7legged150110.tgz)をダウンロード、展開する。

$ cd ~/prog2/7legged
$ mkdir build
$ cd build
$ cmake ../
$ make

5. 4脚ロボットのモデルファイル

6. 実行
以下のコマンドを実行し、gazeboを立ち上げよう! gazeboが起動したら画面左側のinsert(挿入)タブから制作したlegged robotを選択する。4脚ロボットが歩行(クロール歩容)をしたら成功。今回はここまで。

$ export GAZEBO_PLUGIN_PATH=~/prog2/7legged/build:$GAZEBO_PLUGIN_PATH
$ gazebo --verbose

コメントを残す

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