ROS講座103 armロボットのsim環境を作る

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |
| Gazebo | 7.0.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
armロボット用のsim環境を構築をします。アームの逆運動学を解くには数学的には6degだと必要十分に解けますが、moveitでは5、6、7自由度が解くには良いらしいです。

# アームの形を確認する

## ソースコード
urdfファイル：[arm_robot.urdf](https://github.com/project-srs/ros_lecture/blob/master/arm_lecture/urdf/arm_robot.urdf)

## 形の確認
```shell:rvizでの形状の確認
 roscd arm_lecture/urdf/
roslaunch urdf_tutorial display.launch model:=arm_robot.urdf 
```

![arm_rviz1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/4e500ca9-3e4a-37ba-ed9f-7e1591ed901d.png)

画像のように6軸のアームです。また最上部に位置指定用に`target_link`がpitchを90度まわして設置しています。

* (1)yaw軸：台座（青灰）とリンク（灰）
* (2)pitch軸：リンク（灰）とリンク（青灰）
* (3)pitch軸：リンク（青灰）とリンク（灰）
* (4)pitch軸：リンク（灰）とリンク（青灰）
* (5)roll軸：リンク（青灰）とリンク（灰）
* (6)yaw軸：リンク（灰）とリンク（赤）

# ros_controlの設定
回転軸は今まではros_controlの速度コントローラーでまわしていましたが、アームでは`effort_controllers/JointTrajectoryController`というインターフェイスを使ってすべての軸をまとめて1つのロボットとして動かします。また単純なtopicのIFとfollow_joint_trajectoryという名前のactionlibのIFの2つが発生します。

## ソースコード

urdfファイル：[arm_robot.urdf](https://github.com/project-srs/ros_lecture/blob/master/arm_lecture/urdf/arm_robot_sim.urdf)
先ほどのurdfにros_control関連のファイルを追加したものです。

```yaml:arm_lecture/config/trajectory_controller.yaml
arm_robot_trajectory_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - arm1_joint
    - arm2_joint
    - arm3_joint
    - arm4_joint
    - arm5_joint
    - arm6_joint
  gains:
    arm1_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm2_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm3_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm4_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm5_joint: {p: 100, d: 1, i: 1, i_clamp: 1}
    arm6_joint: {p: 100, d: 1, i: 1, i_clamp: 1}

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50
```

```xml:arm_lecture/launch/arm_robot_sim.launch
<?xml version="1.0" ?>
<launch>
  <arg name="model" default="$(find arm_lecture)/urdf/arm_robot_sim.urdf" />
  <param name="robot_description" textfile="$(arg model)" />

  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
  </include>

  <!-- gazebo model -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model arm_robot" />
  <rosparam file="$(find arm_lecture)/config/trajectory_controller.yaml" command="load"/>
  <node name="arm_robot_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="arm_robot_trajectory_controller joint_state_controller"/>
</launch>
```

## 実行

```shell
roslaunch arm_lecture arm_robot_sim.launch 
```

![arm_sim.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/d637ceee-86d3-b265-65ce-fa252da278b5.png)

初期状態はまっすぐ上のはずですが、大体これぐらい曲がったところで止まっています。これはgazeboにモデルが出てくるのとros_controlがロードされるのにタイムラグがあるために、その間に軸が自重で落ちるからです。

# 軸に指令を送る。
前述した`JointTrajectoryController`の型で軸に指令を送ってみます。

## ソースコード

```python:arm_lecture/scripts/joint_trajectory_publisher.py
#!/usr/bin/env python
# license removed for brevity
import rospy
import math
from std_msgs.msg import String
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def talker():
    rospy.init_node('joint_trajectory_publisher', anonymous=True)
    pub = rospy.Publisher('arm_robot_trajectory_controller/command', JointTrajectory, queue_size=10)
    rospy.sleep(0.5)

    msg = JointTrajectory()
    msg.header.stamp = rospy.Time.now()
    msg.joint_names = [ "arm1_joint", "arm2_joint", "arm3_joint", "arm4_joint", "arm5_joint", "arm6_joint" ]
    msg.points = [JointTrajectoryPoint() for i in range(6)]
    msg.points[0].positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    msg.points[0].time_from_start = rospy.Time(1.0)
    msg.points[1].positions = [0.0, math.pi/6.0, math.pi/6.0, math.pi/6.0, 0.0, 0.0]
    msg.points[1].time_from_start = rospy.Time(2.0)
    msg.points[2].positions = [0.5, math.pi/6.0, math.pi/6.0, math.pi/6.0, 0.5, 0.0]
    msg.points[2].time_from_start = rospy.Time(3.0)
    msg.points[3].positions = [-0.5, math.pi/6.0, math.pi/6.0, math.pi/6.0, -0.5, 0.0]
    msg.points[3].time_from_start = rospy.Time(4.0)
    msg.points[4].positions = [0.0, math.pi/6.0, math.pi/6.0, math.pi/6.0, 0.0, 0.0]
    msg.points[4].time_from_start = rospy.Time(5.0)
    msg.points[5].positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    msg.points[5].time_from_start = rospy.Time(6.0)

    pub.publish(msg)
    rospy.sleep(0.5)

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass
```

## 実行

```shell:1つ目のターミナル
roslaunch arm_lecture arm_robot_sim.launch 
```

```shell:2つ目のターミナル
rosrun arm_lecture joint_trajectory_publisher.py 
```

![arm_sim_commander.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/64479c6a-6b3c-0dba-4979-863956bb8d6f.gif)


# 参考

[joint_trajectory_controllerのリファレンス](http://wiki.ros.org/joint_trajectory_controller)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)