OpenRRでxarmを動かす

## はじめに
openrrはrustで書かれたrobot用フレームワークです。今回openrrを使用しxarmという中国製のロボットアームを動作させてみました。

## 環境
ubuntu 18.04
ROS melodic
urdf_viz 0.25.0
openrr 0.0.5

## インストール
- openrrとurdf_vizのインストール:https://qiita.com/OTL/items/3df938f61a4520c10b70
- xarm_rosをインストール:https://qiita.com/hoshianaaa/items/dc0f7302299063c4801c
- urdf参照用にホームにxarm_rosクローン```cd ~/;git clone https://github.com/xArm-Developer/xarm_ros
```

## urdf-vizで操作


```bash:urdf_vizの実行
cd ~/xarm_ros/xarm_description/urdf
urdf-viz xarm6_robot.urdf.xacro
```

操作方法はビジュアライザーに記載の通りで,それに加えて

- "w":x方向に0.1平行移動
- "s":x方向に-0.1平行移動
- "d":y方向に+0.1平行移動
- "a":y方向に-0.1平行移動

の機能もあります。

操作している様子は下の動画のようになります。

<iframe width="560" height="315" src="https://www.youtube.com/embed/sMqnZgSZkgA" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

#openrrでurdf_vizのxarmを動かす

コンフィグファイルを作成します。
まずジョイント名、エンドジョイント名の確認、、、

```
$ urdf-viz xarm6_robot.urdf.xacro
root [⚓] => /world/
    world_joint [⚓] => /link_base/
        joint1 [⚙+Z] => /link1/
            joint2 [⚙+Z] => /link2/
                joint3 [⚙+Z] => /link3/
                    joint4 [⚙+Z] => /link4/
                        joint5 [⚙+Z] => /link5/
                            joint6 [⚙+Z] => /link6/
                                joint_eef [⚓] => /link_eef/

end_link_names = ["joint_eef"]
DoF=6
joint names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
```

![Screenshot from 2021-04-11 23-49-55.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/c5e2d4d5-608a-4c66-a658-bfa06c27f1fd.png)

urdf_vizの結果から、動作させるジョイント名はjoint1,joint2,joint3,joint4,joint5,joint6と分かります。
また先端のジョイント名はjoint_eefと分かります。

以上を考慮しopenrr-apps/configのur10_robot_client_config_for_urdf_viz.tomlをもとに、コンフィグファイルを次のように作成しました。


```diff_toml:xarm6_robot_client_config_for_urdf_viz.toml
[[urdf_viz_clients_configs]]
name = "arm"
- joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
+ joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]

[openrr_clients_config]
- urdf_path = "../../../universal_robot/ur_description/urdf/ur10_robot.urdf.xacro"
+ urdf_path = "../../xarm_ros/xarm_description/urdf/xarm6_robot.urdf.xacro"
- self_collision_check_pairs = ["shoulder_pan_joint:elbow_joint"]
+ self_collision_check_pairs = ["joint1:joint3"]

[[openrr_clients_config.collision_check_clients_configs]]
name = "arm_collision_checked"
client_name = "arm"

[[openrr_clients_config.ik_clients_configs]]
name = "arm_ik"
client_name = "arm_collision_checked"
solver_name = "arm_ik_solver"

[openrr_clients_config.ik_solvers_configs.arm_ik_solver]
- ik_target = "ee_fixed_joint"
+ ik_target = "joint_eef"

[[openrr_clients_config.joints_poses]]
pose_name = "zero"
client_name = "arm_collision_checked"
positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

[[openrr_clients_config.joints_poses]]
pose_name = "ready"
client_name = "arm_collision_checked"
positions = [0.0, -0.7, 1.4, -0.7, 1.57, -3.14]
```

送信するコマンドは次のようにしました。

```txt:xarm6_cmd_urdf_viz.txt

openrr_apps_robot_command list

openrr_apps_robot_command send_joints arm -j 0=0.0 1=-0.5  2=0.0 3=0.0   4=0.0 5=0.0
openrr_apps_robot_command send_joints arm -j 0=0.0 1=0.0  2=0.0 3=0.0   4=0.0 5=0.0

openrr_apps_robot_command move_ik arm_ik --x=0.5 --y=0.0 --z=0.1
openrr_apps_robot_command move_ik arm_ik --x=0.2 --y=0.0 --z=0.1

openrr_apps_robot_command move_ik arm_ik --x=0.5 --y=0.0 --z=0.1
openrr_apps_robot_command move_ik arm_ik --x=0.2 --y=0.0 --z=0.1

openrr_apps_robot_command execute_command -- date
openrr_apps_robot_command execute_command -- ls -a

```

以上作成した２つのファイルのパスを通して、```openrr_apps_robot_command```を実行

```
./openrr_apps_robot_command \
  --config-path=./config/xarm6_robot_client_config_for_urdf_viz.toml \
  load_commands ./command/xarm6_cmd_urdf_viz.txt
```

結果は動画のようになります。

<iframe width="560" height="315" src="https://www.youtube.com/embed/Gvjdnz4THfU" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

# openrrでgazeboのxarmを動かす

ros gazeboでxarmを起動

```
roslaunch xarm_gazebo xarm6_beside_table.launch
```

configを以下のように作成

```diff_toml:xarm6_robot_client_config_for_ros.toml
+ use_move_base_urdf_viz_web_client = false
+ use_navigation_urdf_viz_web_client = false

- [[urdf_viz_clients_configs]]
+ [[ros_clients_configs]]
name = "arm"
joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
+ controller_name = "/xarm/xarm6_traj_controller"
+ complete_allowable_errors = [0.02, 0.02, 0.02, 0.02, 0.02, 0.02]


[openrr_clients_config]
urdf_path = "../../xarm_ros/xarm_description/urdf/xarm6_robot.urdf.xacro"
self_collision_check_pairs = ["joint1:joint3"]

[[openrr_clients_config.collision_check_clients_configs]]
name = "arm_collision_checked"
client_name = "arm"

[[openrr_clients_config.ik_clients_configs]]
name = "arm_ik"
client_name = "arm_collision_checked"
solver_name = "arm_ik_solver"

[openrr_clients_config.ik_solvers_configs.arm_ik_solver]
ik_target = "joint_eef"

[[openrr_clients_config.joints_poses]]
pose_name = "zero"
client_name = "arm_collision_checked"
positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

[[openrr_clients_config.joints_poses]]
pose_name = "ready"
client_name = "arm_collision_checked"
positions = [0.0, -0.7, 1.4, -0.7, 1.57, -3.14]
```

実行

```
./openrr_apps_robot_command \
  --config-path=./config/xarm6_robot_client_config_for_ros.toml \
  load_commands ./command/xarm6_cmd_urdf_viz.txt
```

実行じエラー回避のためurdf_vizの実行も必要でした。
そして、結果は次のようになりました。


<iframe width="560" height="315" src="https://www.youtube.com/embed/p4t-9ihlgiA" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

以上で終了です。
gazeboで動作したため実機でもほぼ同じように動作すると考えられます。
また今回の設定などのファイルは以下のリポジトリにまとまっています。

https://github.com/hoshianaaa/openrr-xarm

# 参考

https://qiita.com/Masa0u0/items/60c5719eb12fa64a06bf