概要

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

ついでに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はそれぞれの環境ごとに設定してください．

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

　　
指定したネームスペースにてノードハンドラのオブジェクト生成．

ros::NodeHandle nh(NS);

　　
MoveGroupInterfaceのオブジェクトを生成します．
この時，チュートリアルではPLANNING_GROUPのみ指定しています．ネームスペースを使用していないデフォルト状態であればplanning_groupのみ指定すればよいのですが，今回はネームスペースを使用するため，MoveGroupInterface::Optionsクラス(構造体)を引数として渡しています．
ros::NodeHandleを引数として渡すことにより，ネームスペースを有効にすることができます．

moveit::planning_interface::MoveGroupInterface arm(moveit::planning_interface::MoveGroupInterface::Options(planning_group, ROBOT_DESCRIPTION, nh));

　
MoveGroupInterface::Optionsを初期化する際の引数は下記．

moveit::planning_interface::MoveGroupInterface::Options::Options ( const std::string& group_name,
                                                                   const std::string& desc = ROBOT_DESCRIPTION,
                                                                   const ros::NodeHandle& node_handle = ros::NodeHandle() 
                                                                 )

　
プランニングの設定．詳細は参考記事をご覧ください．

arm.setPlanningTime(0.05);
arm.setPlannerId("RRTConnect");
arm.setGoalTolerance(0.01);

　
ここでは現在のジョイント角を取得し，すべてのジョイント角に0.1[rad]追加した角を目標角度に設定しています．
目標角度を設定するときにはsetJointValueTarget()を使用します．

std::vector<double> jointValues = arm.getCurrentJointValues();
arm.setJointValueTarget(jointValues);

　
今回は目標角度の指定にstd::vector<double>を使用しましたが，その他にも下記のようなジョイント角の与え方ができるそうです．詳細はMoveGroupInterfaceのクラスリファレンスをご覧ください．

/*** ジョイント角の配列（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を取得することができ，エラーが起きた際はある程度はその原因がわかるようになっています．

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

まとめ

MoveGroupActionを使う方法もありますが，今回はAction部分の記載を楽したかったのでMoveGroupInterfaceを使ってみました．MoveGroupActionを使う場合はこちらの記事が参考になりそうです．

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

参考