MoveGroupInterfaceでジョイント角を指定してマニピュレータを動かす方法

# 概要

MoveIt!でジョイント角を指定してマニピュレータを動かす方法がパッと出てこなかったのでメモ．
基本的には[ROS講座104 moveitでオリジナルのアームを使う](https://qiita.com/srs/items/5501a5d5059e4596f8f0)を参考にしました．プログラムの詳細を知りたい場合や，ジョイント角ではなく位置を指定したい場合には[こちら](https://qiita.com/srs/items/5501a5d5059e4596f8f0)を参考にするとわかりやすいです．

ついでに`MoveGroupInterface::setJointValueTarget()`を使用してマニピュレータに司令を送る方法もメモ．

# 環境

- Ubuntu16.04
- kinetic

### 前提環境

- MoveIt!を使える環境が既に整っている

# サンプルコード

## 全体

まずはサンプルコードの全体． 
詳細はサンプルコードの後に記載します．

```set_joint_angle.cpp
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

const std::string NS = "namespace";
const std::string PLANNING_GROUP = "arm";
const std::string ROBOT_DESCRIPTION = NS + "/robot_description";

int main(int argc, char** argv)
{
    ros::init(argc, argv, "set_joint_state");
    ros::NodeHandle nh(NS);

    ros::AsyncSpinner spinner(2);
    spinner.start();

    // 必要なロボットパラメータの取得   
    moveit::planning_interface::MoveGroupInterface arm(moveit::planning_interface::MoveGroupInterface::Options(PLANNING_GROUP, ROBOT_DESCRIPTION, nh));

    /* planningの設定 */
    ROS_INFO("set planning parameters");
    arm.setPlanningTime(0.05);
    arm.setPlannerId("RRTConnect");
    arm.setGoalTolerance(0.01);

    /* 現在のジョイント角を取得し，指定ジョイント角を計算 */
    ROS_INFO("get current joint values");
    std::vector<double> jointValues = arm.getCurrentJointValues();
    for(auto& joint: jointValues)
        joint += 0.1;

    /* マニピュレータにジョイント角をセットしゴールを送信 */
    ROS_INFO("set joint value");
    arm.setJointValueTarget(jointValues);
    moveit::planning_interface::MoveItErrorCode ret;
    ret = arm.move();
    if (!ret)
        ROS_WARN("Faild: %i", ret.val);
    ros::Duration(0.1).sleep();
    ROS_INFO("finish!");

    return 0;
}
```

## 詳細

マニピュレータごとにネームスペースを設定している場合を想定し，ネームスペースの文字列を`NS`に設定．
`PLANNING_GROUP`や`ROBOT_DESCRIPTION`はそれぞれの環境ごとに設定してください．

```cpp
const std::string NS = "namespace";
const std::string PLANNING_GROUP = "arm";
const std::string ROBOT_DESCRIPTION = NS + "/robot_description";
```
　　
指定したネームスペースにてノードハンドラのオブジェクト生成．

```cpp
ros::NodeHandle nh(NS);
```
　　
`MoveGroupInterface`のオブジェクトを生成します． 
この時，[チュートリアル](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html)では`PLANNING_GROUP`のみ指定しています．ネームスペースを使用していないデフォルト状態であれば`planning_group`のみ指定すればよいのですが，今回はネームスペースを使用するため，`MoveGroupInterface::Options`クラス(構造体)を引数として渡しています．
`ros::NodeHandle`を引数として渡すことにより，ネームスペースを有効にすることができます．


```cpp
moveit::planning_interface::MoveGroupInterface arm(moveit::planning_interface::MoveGroupInterface::Options(planning_group, ROBOT_DESCRIPTION, nh));
```
　
`MoveGroupInterface::Options`を初期化する際の引数は下記．

```cpp
moveit::planning_interface::MoveGroupInterface::Options::Options ( const std::string& group_name,
                                                                   const std::string& desc = ROBOT_DESCRIPTION,
                                                                   const ros::NodeHandle& node_handle = ros::NodeHandle() 
                                                                 )	
```
　
プランニングの設定．詳細は[参考記事](https://qiita.com/srs/items/5501a5d5059e4596f8f0)をご覧ください．

```cpp
arm.setPlanningTime(0.05);
arm.setPlannerId("RRTConnect");
arm.setGoalTolerance(0.01);
```
　
ここでは現在のジョイント角を取得し，すべてのジョイント角に0.1[rad]追加した角を目標角度に設定しています．
目標角度を設定するときには`setJointValueTarget()`を使用します．

```cpp
std::vector<double> jointValues = arm.getCurrentJointValues();
arm.setJointValueTarget(jointValues);
```
　
今回は目標角度の指定に`std::vector<double>`を使用しましたが，その他にも下記のようなジョイント角の与え方ができるそうです．詳細は[`MoveGroupInterface`のクラスリファレンス](http://docs.ros.org/kinetic/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroupInterface.html)をご覧ください．

```cpp
/*** ジョイント角の配列（std::vector<double>） ***/
// メモ：必ず`getJointValueTarget()`にて返ってくるジョイントの数，順番と同じ数，順番の値を保持していなければならない．
// ジョイントが6つあれば要素数は6つ．
std::vector<double> jointValues = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
arm.setJointValueTarget (jointValues)

/*** ジョイント名とジョイント角をペアとしたmap（std::map<std::string, double>） ***/
// メモ１：指定したジョイント名が見つからない場合は例外を送出する
// メモ2: 指定されていないジョイントは元の値を保持する
std::map<std::string, map> jointMap = {std::make_pair("joint1", 0.0), std::make_pair("joint1", 0.0)};
arm.setJointValueTarget (jointMap)

/*** moveitで使用するRobotState（robot_state::RobotState）***/
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(kinematic_model));
robot_state->setToRandomPositions(kinematic_model->getJointModelGroup("arm"));
arm.setJointValueTarget(robot_state);

/*** ジョイント名とジョイント角（std::string, std::vector<double>） ***/
// メモ1：std::vector<double>は必ず一つの値のみ格納
// メモ2：指定されていないジョイントは元の値を保持する
std::vector<double> jointValue = {0.0};
arm.setJointValueTarget("joint0", jointValue);

/*** ジョイント名とジョイント角（std::string, double） ***/
// メモ1：ジョイント名は必ず1自由度のジョイントを指定しなければならない
// メモ2：指定されていないジョイントは元の値を保持する
std::vector<double> jointValue = {0.0};
arm.setJointValueTarget("joint0", jointValue);

/*** ROSのジョイントメッセージ（sensor_msgs::JointState） ***/
// メモ：指定されていないジョイントは元の値を保持する
sensor_msgs::JointState joint_state;
joint_state.name = {"joint0", "joint1"};
joint_state.position = {0.0, 0.0};
arm.setJointValueTarget(joint_state);

/*** ゴール位置から逆運動学を計算 ***/
// いくつかあるが，今回は割愛．
```
　
最後に`arm.move()`でマニピュレータに司令を出します．
このとき，`arm.move()`の返り値として`MoveItErrorCode`を取得することができ，エラーが起きた際はある程度はその原因がわかるようになっています．

```cpp
ret = arm.move();
if (!ret)
    ROS_WARN("Faild: %i", ret.val);
```


# まとめ

`MoveGroupAction`を使う方法もありますが，今回はAction部分の記載を楽したかったので`MoveGroupInterface`を使ってみました．`MoveGroupAction`を使う場合は[こちら](https://qiita.com/tattaka/items/384cc5d4c0f6f92a5809)の記事が参考になりそうです．

ROSの名前空間を使いこなせるようになりたい．

# 参考
- [MoveIt! Move Group C++ Interface](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html)
- [ROS講座104 moveitでオリジナルのアームを使う](https://qiita.com/srs/items/5501a5d5059e4596f8f0)
- [moveit::planning_interface::MoveGroupInterface Class Reference](http://docs.ros.org/kinetic/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroupInterface.html)
- [Pose to PoseのためのMoveGroupAction](https://qiita.com/tattaka/items/384cc5d4c0f6f92a5809)