MoveIt！入门：RobotModel、RobotState

先热个身，来个简单的random_pose例程

例程源码get_random_pose.cpp：

（本来想像tutorial代码中那样显示target_pose坐标，但是发现不行，暂时没找到函数，先这样吧）

//需要的头文件
#include 
#include 
#include 
#include 
#include 


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "get_random_pose");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    static const std::string PLANNING_GROUP = "arm";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    const robot_state::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    move_group.setRandomTarget();


    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
    visual_tools.deleteAllMarkers();

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (random pose) %s", success ? "" : "FAILED");

    Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
    text_pose.translation().z() = 0.65;

    //随机的坐标无法展示，终端显示找不到“gripper_frmae”的位置
    // geometry_msgs::PoseStamped target_pose;
    // target_pose = move_group.getPoseTarget("gripper_frame");
    
    //展示坐标和轨迹线，随机的坐标怎么展示？？？
    // visual_tools.publishAxisLabeled(target_pose.pose, "random_pose");
    visual_tools.publishText(text_pose, "GetRandomPose", rvt::WHITE, rvt::XLARGE);
    visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
    visual_tools.trigger();

   //move_group.move();//待定

    ros::waitForShutdown();

    return 0;
}

图示

因为是get random pose，因此在运行例程时会存在目标不可行的情况，终端会出现RRTConnect: Unable to sample any valid states for goal tree，多运行几次例程即可。机器人模型是自建的某宝机器人模型，建模过程可见前面的博文。

例程解析

头文件（貌似最后一行可以不加），遵循用哪些包写哪些头文件的原则。

#include 
#include 
#include 
#include 
#include 

节点初始化，然后开线程并启动线程。注意初始化节点名称需要与程序名称一样。

ros::init(argc, argv, "get_random_pose");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();

前面博文Move Group C++ Interface 源码解析中已经写了部分源码的解析。下面代码的顺序是，设置例程中的规划组，声明对象planning_scence_interface，定义一个robot_state::JointModelGroup类型的指针joint_model_group，包含规划组的信息，​调用setRandomTarget()将相应的action发送给move_group节点。

static const std::string PLANNING_GROUP = "arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

const robot_state::JointModelGroup* joint_model_group =
   move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

move_group.setRandomTarget();

MoveItVisualTools包提供了许多功能，可用于可视化RViz中的对象，机器人和轨迹以及调试工具。

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();

这里的代码主要是为轨迹可视化做准备，plan类的主要作用就是展示运动规划。当规划成功，则在终端显示相应信息。

moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) ==
     moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (random pose) %s", success ? "" : "FAILED");

下面是标题和轨迹可视化的步骤。

Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 0.65;

//随机的坐标无法展示，终端显示找不到“gripper_frmae”的位置
// geometry_msgs::PoseStamped target_pose;
// target_pose = move_group.getPoseTarget("gripper_frame");
    
//展示坐标和轨迹线，随机的坐标怎么展示？？？
// visual_tools.publishAxisLabeled(target_pose.pose, "random_pose");
visual_tools.publishText(text_pose, "GetRandomPose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);   visual_tools.trigger();

RobotState And RobotModel

先放一波官方文档的解释：

RobotModel类和RobotState是类访问机器人运动学状态的核心部分。

RobotModel类包含所有link和joint之间的关系，包括从URDF加载的关节限制属性。 RobotModel还将机器人的link和joint分离为SRDF中定义的规划组。

RobotState类包含有关机器人在snapshot in time（这个不知道怎么翻译，快照时间下？反正是瞬时状态吧，hahaha）的信息，存储关节位置的矢量以及可选的速度和加速度，可用于获取有关当前状态下机器人的运动信息，例如末端执行器的雅可比矩阵。RobotState还包含辅助函数，用于根据末端执行器位置（笛卡尔姿势）设置手臂位置以及计算笛卡尔轨迹。

例程源码

//#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "RobotModelAndRobotState");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface move_group("arm");
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    //RobotModelPtr，robot_model指针
    robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
    robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
    ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

    //RobotStatePtr,robot_state指针
    robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
    kinematic_state->setToDefaultValues();
    const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm");

    const std::vector& joint_names = joint_model_group->getVariableNames();

    std::vector joint_values;
    kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    for (int i = 0; i < joint_names.size(); i++){
        ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
    }

    joint_values[0] = 5.57;
    kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
    ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

    kinematic_state->enforceBounds();
    ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

    kinematic_state->setToRandomPositions(joint_model_group);
    const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("gripper_frame");

    ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
    ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");

    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
    visual_tools.deleteAllMarkers();

    std::size_t attempts = 10;
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    double timeout = 0.1;
    bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);
    if (found_ik){
        kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
        move_group.setJointValueTarget(joint_values);

        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
        ROS_INFO_NAMED("tutorial", "Visualizing plan %s", success ? "" : "FAILED");
        visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
        visual_tools.trigger();

        for (int i = 0; i < joint_names.size(); i++){
            ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
        }
    }else{
        ROS_INFO("Did not find IK solution");
    }

    Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
    Eigen::MatrixXd jacobian;
    kinematic_state->getJacobian(joint_model_group,kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),reference_point_position, jacobian);
    ROS_INFO_STREAM("Jacobian: \n" << jacobian << "\n");

    ros::shutdown();


    return 0;
}

图示

可以看出终端输出如下结果：

例程解析

初始化

首先实例化一个RobotModelLoader对象，该对象将在ROS参数服务器上查找机器人描述并构造一个RobotModel供我们使用。

定义一个指向RobotModel的指针kinematic_model，后面用处很大。

getModelFrame()，顾名思义，获取计算此模型的变换的帧（使用RobotState时）。此框架取决于根关节。因此，帧是从SRDF中提取的，或者假设它是根link的名称，一般根link为base_link。

//设置规划组
moveit::planning_interface::MoveGroupInterface move_group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

//RobotModelPtr，robot_model指针
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

定义一个指向RobotState的指针kinematic_state。

将机器人的状态都设为默认值，根据setToDefaultValues()定义，默认位置为0或者是最大界限和最小界限之间。

定义一个robot_state::JointModelGroup类型的指针joint_model_group，包含规划组arm的信息。

根据getVariableNames，按照它们存储在内存中的顺序获取构成此状态的变量的名称，并将其存放在可变数组joint_name中。

这里进行的一切操作均没有和move_group进行通讯，因此机器人不会动。

robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm");

const std::vector& joint_names = joint_model_group->getVariableNames();

获取每个关节值

根据copyJointGroupPositions()，按照在规划组中变量的顺序，将当前状态下规划组中的各个关节的位置值复制到另一个地方。注意，这不一定是RobotState本身的连续内存块，因此我们复制而不是返回指针。

然后输出每个关节的名称及其当前状态下的位置值。

std::vector joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for (int i = 0; i < joint_names.size(); i++){
    ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}

关节限制

setJointGroupPositions()，更新规划组中各关节的位置，但这个函数不会强制执行关节限制。enforceBounds()，调用该函数后就能对关节进行限制。具体可见终端输出结果。

joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);

ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

正向运动学

根据setToRandomPositions()，目前将规划组中的各个关节值设置为随机值。

然后计算末端执行器（当前规划组中最末端，本文为gripper_frame）在随机关节值状态下的正向运动学齐次变换矩阵（平移+旋转），并输出。

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("gripper_frame");

ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");

逆运动学

先为可视化做准备。

调用setFromIK解当前规划组arm的逆运动学问题，返回一个bool量。在解决该问题之前，需要如下条件：

• 末端执行器的期望位姿（一般情况下为当前规划组chain的最后一个连杆，本文为gripper_link）：也就是上面已经计算得到的齐次变换矩阵end_effector_state；
• 尝试解决IK的次数：10；
• 每次尝试的时间：0.1s.

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");   visual_tools.deleteAllMarkers();

std::size_t attempts = 10;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);

如果IK得到解，则驱动机器人按照计算得到的关节值进行运动，同时，打印计算结果。

if (found_ik){
    kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    move_group.setJointValueTarget(joint_values);

    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO_NAMED("tutorial", "Visualizing plan %s", success ? "" : "FAILED");
    visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
    visual_tools.trigger();

    for (int i = 0; i < joint_names.size(); i++){
       ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
    }
}else{
    ROS_INFO("Did not find IK solution");
}

计算末端执行器的雅克比矩阵

getJacobian()，先给定一个参考点reference_point_position坐标位置，该坐标位置是相对kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back())（也就是gripper_fame）而言的，，定义一个空的jacobian矩阵。

调用函数，计算并输出结果。

Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
Eigen::MatrixXd jacobian;
kinematic_state->getJacobian(joint_model_group,
                             kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
                             reference_point_position, jacobian);
ROS_INFO_STREAM("Jacobian: \n" << jacobian << "\n");