この記事が私が担当しているロボットプログラミングⅡの講義用です。
今回は２自由度のロボットアームモデルを作成し、キーボード操作により動かすサンプルです。
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"> <!--第１関節 -->
<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"> <!--第２関節 -->
<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
２自由度ロボットアームのサンプルプログラムに以下を実装しよう！
- 運動学
運動学の計算結果と位置センサの出力を比較し、プログラムが正しいか確認する。 - 逆運動学
逆運動学を実装しよう。
任意の位置に直方体を表示し、アーム先端がそれを指すようなプログラムを実装する。２自由度の場合は解が２つあるので、数字キー１を押すと解１、数字キー２を押すと解２を選択する。
コメント