关于Moveit中运动学逆解算结果的获取

在某些情况下，我们可能只需要利用Moveit的逆解算结果，但是官方的API中并没有提供直接读取结算结果的函数，那么如果我们需要直接读取解算结果并应用到其他地方，可以按下面的步骤做。

引用基本的头文件

#include <cmath> //如果用到了
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>

main函数

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"GetResultExp01");
    //异步回调
    ros::AsyncSpinner spinner(1);
    spinner.start();
    //绑定在setup assistant设置的机械臂
    moveit::planning_interface::MoveGroupInterface arm("arm");
    //设置加速度因子和速度因子
    arm.setMaxAccelerationScalingFactor(1);
    arm.setMaxVelocityScalingFactor(1);
    
    
    //在这里加入自己的代码
    
    
    //关闭ROS
    ros::shutdown();
    return 0;
}

下面是自己加入的代码。

设置目标位置

geometry_msgs::Pose target_pose;   //目标位置
// 例如：x: 0.500917 y: 0.000072 z:0.409105 ox:-0.000005  oy:-0.995442  oz:-0.000018  ow:0.095368
target_pose.position.x = 0.500917;
target_pose.position.y = 0.000072;
target_pose.position.z = 0.409105;
target_pose.orientation.x = -0.000005;
target_pose.orientation.y = -0.995442;
target_pose.orientation.z = -0.000018;
target_pose.orientation.w = 0.095368;
arm.setPoseTarget(target_pose);

解算规划并读取规划结果

//创建一个（储存）计划的变量
moveit::planning_interface::MoveGroupInterface::Plan myplan;
//对计划进行生成，并获取是否成功的状态码
bool successState = (arm.plan(myplan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

//注意那个0，代表轨迹起始位点
//i代表各个关节，从0到i分别是setup assistant中按顺序定义的关节
//例如positions.at(0)就是代表第一个关节的关节角度
myplan.trajectory_.joint_trajectory.points.at(0).positions.at(i)
//at(myplan.trajectory_.joint_trajectory.points.size()-1)可以读取到最后一个轨迹点，也就是目标点，将数据导出即可
myplan.trajectory_.joint_trajectory.points.at(myplan.trajectory_.joint_trajectory.points.size()-1).positions.at(i)

在仿真中运行结果

arm.execute(myplan);