ROS学习笔记-使用moveitAPI进行运动学逆解

代码编写

#include <string>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "moveit_fk_demo");
    // 创建一个异步spinner，用于在后台启动一个线程进行消息处理
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // 创建一个MoveGroupInterface对象，用于与机器人进行交互
    moveit::planning_interface::MoveGroupInterface arm("manipulator");

    // 获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);

    // 当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    // 设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

    // 设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);

    // 设置机器人终端的目标位置
    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = 0.70692;
    target_pose.orientation.y = 0.0;
    target_pose.orientation.z = 0.0;
    target_pose.orientation.w = 0.70729;

    target_pose.position.x = 0.2593;
    target_pose.position.y = 0.0636;
    target_pose.position.z = 0.1787;

    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();
    //arm.setPoseTarget(target_pose);
    arm.setApproximateJointValueTarget(target_pose);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(plan);

    // 打印运动规划结果信息
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    // 让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan);
    sleep(1);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);

    // 关闭ROS节点，释放资源
    ros::shutdown(); 

    return 0;
}

代码运行结果

在这里插入代码片

topic分析

/move_group/cancel
/move_group/feedback
/move_group/goal
/move_group/result
/move_group/status

movegroup的一个action，通过goal发送rviz端plan之后的目标位姿的每个关节的角度, movegroup之后通过result返回一个根据goal得到的多点轨迹。

/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status

在rviz中检查轨迹没有问题，按下execute后，rviz向movegroup发送之前接收到的result作为执行的goal，movegroup将执行的结果通过result发送给rviz。