#プログラミング ROS< ロボットアームの作成（3） >
はじめに
1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第35弾として，「ロボットアームの作成（3）」を扱う．
環境
仮想環境
| ソフト | VMware Workstation 15 |
| 実装RAM | 2 GB |
| OS | Ubuntu 64 ビット |
| isoファイル | ubuntu-mate-20.04.1-desktop-amd64.iso |
コンピュータ
| デバイス | MSI |
| プロセッサ | Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz |
| 実装RAM | 8.00 GB (7.89 GB 使用可能) |
| OS | Windows (Windows 10 Home, バージョン：20H2) |
ROS
| Distribution | noetic |
| シミュレーション | gazebo |
ロボットアームの作成
ROSを使って，ほとんどの新しいロボットを制御する手順は次のようである．
1. ROSのメッセージインタフェースを決める．
2. ロボットのモータ用ドライバを書く．
3. ロボットの物理構造を書く．
4. Gazeboのシミュレーションで使用できるようにモデルに物理的特性を追加する．
5. tfを介して座標変換データを配信し，rvizでそれを可視化する．
6. センサを追加する．ドライバとシミュレーションのサポートも必要．
7. ナビゲーション等の標準的なアルゴリズムを適用する．
ロボットアームを例にその流れを確認していく．
今回は，4についてまとめてロボットアームの作成（3）として扱うこととする．
手順4:Gazeboでのシミュレーション
例：CougarBot
URDFモデルは，ロボットの運動学上の構造と外観を記述するが，
モデルをシミュレーションするのに必要な物理的特性について何も教えてくれない．
GazeboでCougarBotをシミュレートするために，モデル内のすべてのリンクに新しいタグを2つ追加する必要がある．
- ＜collision＞ (干渉)
visualと同じように，このタグはロボットの身体の大きさと形を定義する．このタグで定義された情報は他のオブジェクトとどのように作用するかを計算するために使われる．collisionのgeometry（幾何学情報）タグはvisualのgeometryタグと同じになるはずだが，多くの場合異なる．- なぜ？
visualだと見た目重視にメッシュを使って複雑な形を用いる場合もあるが，collisionの場合，干渉検出のための処理の効率化のために単純な形状（箱や円柱など）を使用することが多いため．
- なぜ？
※collisionタグにはmaterialタグは必要でない
- ＜inertial＞（慣性）
このタグはリンクの質量と慣性モーメントを定義する．これらは万有引力の法則に従ってリンクを動かすのに必要．
これら2つのタグをと同じ階層（linkタグの中）に書く．
Gazeboに読み込む
必要な操作をlaunchファイルにまとめる．ここで最低限必要となるのは次の操作である．
- ロボットのURDFモデルをパラメータサーバにロードする
- （空のワールドで）Gazeboを起動する
- ROSのサービスコールを使用して，Gazeboの中にそのロボットのインスタンスを生成する．
この際のURDFデータはパラメータサーバから読み込む
launchファイル
cougarbot.launch
<launch>
<!--CougarBotのURDFモデルをパラメータサーバにロードする-->
<param name = "robot_description" textfile = "$(find make_robot2)/urdf/cougarbot.urdf"/>
<!--空のワールドでGazeboを開始する-->
<include file = "$(find gazebo_ros)/launch/empty_world.launch"/>
<!--GazeboでCougarBotを生成し，パラメータサーバからのその記述を受ける-->
<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model cougarbot"/>
</launch>
パラメータサーバは慣例として/robot_descriptionという名前で保存する．（ほかの名前を使うこともできるが，結果として多くのツールのデフォルトの設定を変更しなければならなくなる）
以下では，これを用いて実装するが，urdfファイルの部分は実行したいものに合わせて変更している．
実装
以下では，構築したURDFファイルを示す．
ソースコード
cougarbot1.urdf
<?xml version="1.0"?>
<robot name="cougarbot">
<!--作業台に固定されているようにするための架空のリンク"world"-->
<link name="world"/>
<!--台座のモデリング-->
<link name="base_link">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<material name="silver">
<color rgba="0.75 0.75 0.75 1"/>
</material>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
<inertia ixx="0.0027" iyy="0.0027" izz="0.005" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--台座と作業台（world）をつなぐ関節-->
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
<!--胴体のモデリング-->
<link name="torso">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.5" radius="0.05"/>
</geometry>
<material name="silver">
<color rgba="0.75 0.75 0.75 1"/>
</material>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.5" radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<inertia ixx="0.02146" iyy="0.02146" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--胴体と台座をつなぐ関節（腰）-->
<joint name="hip" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="torso"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.05"/>
</joint>
<!--上腕のモデリング-->
<link name="upper_arm">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<material name="silver"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<inertia ixx="0.01396" iyy="0.01396" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--上腕と胴体をつなぐ関節（肩）-->
<joint name="shoulder" type="continuous">
<axis xyz="0 1 0"/>
<parent link="torso"/>
<child link="upper_arm"/>
<origin rpy="0 1.5708 0" xyz="0.0 -0.1 0.45"/>
</joint>
<!--前腕のモデリング-->
<link name="lower_arm">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<material name="silver"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<inertia ixx="0.01396" iyy="0.01396" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--前腕と上腕をつなぐ関節（肘）-->
<joint name="elbow" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_arm"/>
<child link="lower_arm"/>
<origin rpy="0 0 0" xyz="0.0 0.1 0.35"/>
</joint>
<!--手のモデリング-->
<link name="hand">
<!--外観-->
<visual>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="silver"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<inertia ixx="0.00042" iyy="0.00042" izz="0.00042" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--手と前腕をつなぐ関節（手首）-->
<joint name="wrist" type="continuous">
<axis xyz="0 1 0"/>
<parent link="lower_arm"/>
<child link="hand"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.425"/>
</joint>
</robot>
次に，先ほどのlaunchファイルを実行した様子を示す．
実行の様子
この時点では，重力で下方向に引っ張られ，グニャグニャ．
⇒follow_joint_trajectory/joint_statesインタフェースを介して前関節を制御する手助けをしてくれるものが必要．
-
ros_controlプラグイン
- follow_joint_trajectoryを介して次に取るべき新しい軌跡を受け取る
-
ros_joint_state_publisherプラグイン
- joint_statesデータを配信する．
※関節を動くようにするためには，ros_controlプラグインが必要．このプラグインを追加するには，少し作業が必要となる．シミュレーションで用いるすべての制御コードは実際のハードウェアでも使える．これを実現するには，コントローラとそれをサポートする下部構造に特別な抽象化と設定が必要となり，その両方が複雑さを増す．複雑さが増す代わりに，同じコードをシミュレーションと本物のロボットの両方で実行できる．これは大きなトレードオフである．
最初に，すべての関節に対して，適合するトランスミッション（変速機）を定義する必要がある．
- モータの出力とそのモータが取り付けられている関節との間で起こることをモデル化
トランスミッションを定義できたら，ros_controlプラグインを追加できる．
ソースコード：プラグインを追加
cougarbot.urdf
<?xml version="1.0"?>
<robot name="cougarbot">
<!--作業台に固定されているようにするための架空のリンク"world"-->
<link name="world"/>
<!--台座のモデリング-->
<link name="base_link">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<material name="silver">
<color rgba="0.75 0.75 0.75 1"/>
</material>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
<inertia ixx="0.0027" iyy="0.0027" izz="0.005" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--台座と作業台（world）をつなぐ関節-->
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
<!--胴体のモデリング-->
<link name="torso">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.5" radius="0.05"/>
</geometry>
<material name="silver">
<color rgba="0.75 0.75 0.75 1"/>
</material>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.5" radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<inertia ixx="0.02146" iyy="0.02146" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--胴体と台座をつなぐ関節（腰）-->
<joint name="hip" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="torso"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.05"/>
</joint>
<!--上腕のモデリング-->
<link name="upper_arm">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<material name="silver"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<inertia ixx="0.01396" iyy="0.01396" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--上腕と胴体をつなぐ関節（肩）-->
<joint name="shoulder" type="continuous">
<axis xyz="0 1 0"/>
<parent link="torso"/>
<child link="upper_arm"/>
<origin rpy="0 1.5708 0" xyz="0.0 -0.1 0.45"/>
</joint>
<!--前腕のモデリング-->
<link name="lower_arm">
<!--外観-->
<visual>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<material name="silver"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<cylinder length="0.4" radius="0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<inertia ixx="0.01396" iyy="0.01396" izz="0.00125" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--前腕と上腕をつなぐ関節（肘）-->
<joint name="elbow" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_arm"/>
<child link="lower_arm"/>
<origin rpy="0 0 0" xyz="0.0 0.1 0.35"/>
</joint>
<!--手のモデリング-->
<link name="hand">
<!--外観-->
<visual>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="silver"/>
</visual>
<!--物理的特性-->
<!--干渉-->
<collision>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<!--慣性-->
<inertial>
<mass value="1.0"/>
<inertia ixx="0.00042" iyy="0.00042" izz="0.00042" ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!--手と前腕をつなぐ関節（手首）-->
<joint name="wrist" type="continuous">
<axis xyz="0 1 0"/>
<parent link="lower_arm"/>
<child link="hand"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.425"/>
</joint>
<!--トランスミッションの追加-->
<!--腰関節用-->
<transmission name="tran0">
<type>transmission_interface/SimpleTransmission</type>
<!--関節の指定-->
<joint name="hip">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<!--アクチュエータの定義（ギア比1の減速機）-->
<actuator name="motor0">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!--肩関節用-->
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<!--関節の指定-->
<joint name="shoulder">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<!--アクチュエータの定義（ギア比1の減速機）-->
<actuator name="motor1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!--肘関節用-->
<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<!--関節の指定-->
<joint name="elbow">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<!--アクチュエータの定義（ギア比1の減速機）-->
<actuator name="motor2">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!--手首関節用-->
<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<!--関節の指定-->
<joint name="wrist">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<!--アクチュエータの定義（ギア比1の減速機）-->
<actuator name="motor3">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!--ros_controlプラグインをロード-->
<gazebo>
<plugin name="control" filename="libgazebo_ros_control.so"/>
</gazebo>
</robot>
次に，ros_controlが提供するコントローラのうちどれを使用するかを指定し，設定する．ここでは，（速度や加速度といったゴールや制約ではなく）関節の位置の軌道を受け取ることのできるものが必要．
ソースコード：コントローラのYAMLファイルを作成
controllers.yaml
arm_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- hip
- shoulder
- elbow
- wrist
ROSのパラメータサーバに読み込む．
cougarbot.launch
<launch>
<!--CougarBotのURDFモデルをパラメータサーバにロードする-->
<param name = "robot_description" textfile = "$(find make_robot2)/urdf/cougarbot.urdf"/>
<!--空のワールドでGazeboを開始する-->
<include file = "$(find gazebo_ros)/launch/empty_world.launch"/>
<!--GazeboでCougarBotを生成し，パラメータサーバからのその記述を受ける-->
<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model cougarbot"/>
<!--コントローラをROSのパラメータサーバに読み込む-->
<rosparam file = "$(find make_robot2)/yaml/controllers.yaml" command = "load"/>
<!--コントローラ作成-->
<node name = "controller_spawner" pkg = "controller_manager" type = "spawner" args = "arm_controller"/>
</launch>
実行の様子
ここで，扱われているトピックを確認しておく．
/arm_controllerのfollow_joint_trajectoryという名前空間には，アクションインタフェースを構築するトピックが含まれており，通常コントローラはこれらを使う．一方，commandトピックも提供されているため，少し調べておく．
ロボットアームの作成（1）でfollow_joint_trajectoryアクションが受け取るゴールを見た時に示したtrajectory_msgs/JointTrajectoryメッセージがある．
これを使って，マニュアル的にロボットの指令を出すことができる．ここで，ロボットが確かに指示に従って動くのかということを確認しておく．
以下に実行したコマンドとそのときの実行の様子を示す．
実行コマンド
ターミナル1
roslaunch make_robot2 cougarbot.launch
ターミナル2
rostopic pub /arm_controller/command trajectory_msgs/JointTrajectory '{joint_names: ["hip", "shoulder", "elbow", "wrist"], points: [{positions: [0.1, -0.5, 0.5, 0.75], time_from_start: [1, 0]}]}' -1
実行の様子
注意
教材に沿って進める中でうまくいかなかったことを記載しておく．
先ほどのコマンドにより，ロボットの挙動を見るところで，time_from_start: [1, 0]とあるが，実際にはtime_from_start: [1.0, 0.0]となっていた．しかしながら，これでは次のような警告が出た．
警告
[WARN] [1617115489.826817]: Inbound TCP/IP connection failed: <class 'struct.error'>: 'required argument is not an integer' when writing '1000000000.0'
警告といえど，何もロボットが動かないという致命的な状況であった．警告のメッセージからどこか整数でなければならない箇所があると思い，少しいじってみると，どうやらtime_from_start: [1.0, 0.0]が原因だとわかった．そこで，上に示したように整数にしたところ警告はなくなり，ロボットもうまく動いた．
感想
注意に書いた内容で随分と時間を費やしてしまった．なかなか間違っているところに気づけなかったが，気づけたときはよかったと安心するとともに，次に進めるという高揚感を感じた．学習内容としては，移動ロボットと似ていると思えたが，関節を扱うということでトルクを働かせないとグニャグニャになってしまうというところが全く異なる点であった．
参考文献
プログラミングROS Pythonによるロボットアプリケーション開発
Morgan Quigley, Brian Gerkey, William D.Smart 著
河田 卓志 監訳
松田 晃一，福地 正樹，由谷 哲夫 訳
オイラリー・ジャパン 発行