ROSの勉強　第34弾：ロボットアームの作成（2）

# #プログラミング ROS< ロボットアームの作成（2） >

##はじめに
1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第34弾として，「ロボットアームの作成（2）」を扱う．


##環境
#####仮想環境
|||
|:-:|:--|
|ソフト|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. ナビゲーション等の標準的なアルゴリズムを適用する．**

ロボットアームを例にその流れを確認していく．
今回は，3についてまとめてロボットアームの作成（2）として扱うこととする．

##手順3:ロボットのモデリング: URDF
例：CougarBot

URDFCougarBotの物理的なモデルをURDFファイルとして作成
**このモデルは，rvizでロボットを可視化し，Gazeboでシミュレーションし，MoveItで動きを計画するのに使用される**．

###運動学的な部分
* 台座は作業台にしっかり取り付けられている．（例：ボルトで固定）
* 最初の関節は左右に「胴体」を回転する「腰」
* 次に「肩」「肘」「手首」であり，それぞれは「上腕」「前腕」「手」の上げたり下げたりする．

このロボットのモデルには，**5つのリンク｛台座・胴体・上腕・前腕・手｝があり，4つの関節｛腰・肩・肘・手首｝で接続**されている．

####モデリング
#####台座
台座のモデリングにおいて，注意すべきところがある．台座は作業台にしっかりと固定されている．これをURDFの構成で表現するには，worldという名前の特別なリンクを作成し，低い円柱でモデル下台座を固定関節で接続する．

```xml:base.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>
	</link>
	<!--台座と作業台（world）をつなぐ関節-->
	<joint name="fixed" type="fixed">
		<parent link="world"/>
		<child link="base_link"/>
	</joint>

	
</robot>
```
![rviz1](https://user-images.githubusercontent.com/61382602/112959829-c90e5a80-917e-11eb-9454-f2bc47f1a1ad.PNG)

alpha値を0.5にすると...
![rviz2](https://user-images.githubusercontent.com/61382602/112959827-c875c400-917e-11eb-85f9-d3080bda510c.PNG)

以降，alpha値を0.8にして表示している．
#####胴体
胴体は背の高い細い円柱で，腰を介して台座に接続されている．腰関節はz軸の周りに亜移転する無限回転機構型の関節（continuous）である．

```xml:add_torso.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>
	</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>
  	</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>


</robot>
```
![add_torso](https://user-images.githubusercontent.com/61382602/112959820-c7dd2d80-917e-11eb-9a7e-d7a87f5b7bf3.gif)
#####上腕
上腕も胴体と同様，細い円柱（ただし短い）でモデリング．ロボット側からは，上腕は胴体の右（外）側に肩を介して接続されている．

```xml:add_upper.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>
	</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>
  	</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>
  	</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>


</robot>
```
![add_upper](https://user-images.githubusercontent.com/61382602/112959823-c875c400-917e-11eb-97c5-223bacad7788.gif)
#####前腕
前腕も胴体と同様，細い円柱（ただし短い）でモデリング．ロボット側からは，前腕は上腕の左（内）側に肘を介して接続されている．

```xml:add_lower.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>
	</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>
  	</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>
  	</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>
    </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>

    
</robot>
```
![add_lower](https://user-images.githubusercontent.com/61382602/112962683-9d40a400-9181-11eb-8260-011bdde38b09.gif)

#####手
前腕の端の手首を介して取り付ける．

```xml:add_hand.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>
	</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>
  	</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>
  	</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>
    </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>
    </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>
```
![add_hand](https://user-images.githubusercontent.com/61382602/112959808-c57ad380-917e-11eb-8ffe-c95a662a6d1e.gif)

#####launchファイル
なお，それぞれのrvizでの様子は，次のlaunchファイルをモデル指定して実行したものである．

```xml:display.launch
<launch>

  <arg name="model" default="$(find make_robot2)/urdf/base.urdf"/>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />

  <node if="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
  <node unless="$(arg gui)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>
```

モデル指定での実行

```:add_hand.urdfを用いる場合
roslaunch make_robot2 display.launch model:=urdf/add_hand.urdf
```

##感想
今回で，1からモデリングを行うのは移動ロボットのときを含めて2回目である．とりあえず，形だけの簡易モデルの構築なら，自力でできそうだと感じた．多少URDFファイルの扱いに慣れてきたということであろうと思いたい．


##参考文献
プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　  　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行