一、如果想要得到當前的某個關節的位姿，特別是末端執行器，並且通過逆運動學介面得到當前的關節角度，需要下列的程式

1、標頭檔案

#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
 

分別對應了move_group的介面，注意，在kinetic中，move_group的介面標頭檔案名稱變為了

#include <moveit/move_group_interface/move_group_interface.h>

其他對應了planning_scene,robot_model與robot_model_loader,robot_state的相關標頭檔案

2、主程式

2.1 首先是各種變數的初始化。

    moveit::planning_interface::MoveGroup group("arm");
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    
    robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
    robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
    ROS_INFO("Model frame: %s",robot_model->getModelFrame().c_str());
    
    //利用RobotModel.我們可以建立一個RobotState,後者保留了機器人的配置。我們同樣可以得到了一個JointModelGroup,這代表了了一個具體的group的robot model。
    robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
    const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup("arm");
    const std::vector<std::string> &joint_name = joint_model_group->getJointModelNames();
    std::vector<double> joint_values;
 

除去move_group(move_group_interface)以及planning_scene_interface.我們還要建立robot_model_loader(通過“robot_description”)

進而建立robot_model::RobotModelPtr型別的robot_lodel(通過robot_model_loader.getModel())

建立robot_state::RobotStatePtr型別的robot_state(通過robot_model)

建立const的robot_state::JointModelGroup* joint_model_group(通過robot_state->getJointModelGroup(..))，定義了一個對應於某個group的joint group

得到vector型別的joint name(通過joint_model_group->getJointModelNames())

2.2 目標的傳入

    geometry_msgs::Pose target_pose1;
      target_pose1.orientation.w = 0.09;
      target_pose1.orientation.x = 0.68;
      target_pose1.orientation.y = -0.23;
      target_pose1.orientation.z = 0.69;
      target_pose1.position.x =0.19;
      target_pose1.position.y = -0.5;
      target_pose1.position.z = 0.52;
//      group.setNamedTarget("Home");
      group.setPoseTarget(target_pose1);
      
    moveit::planning_interface::MoveGroup::Plan my_plan;
    bool success = group.plan(my_plan);

    ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
        /* Sleep to give Rviz time to visualize the plan. */
    sleep(2.0);

    group.move();

非常基本的操作，即可以傳入位姿，也可以轉化到關節空間的目標。

2.3 得到各種當前的資訊

 robot_state = group.getCurrentState();
 const Eigen::Affine3d &end_effector_state = robot_state->getGlobalLinkTransform("j2n6s300_end_effector");
ROS_INFO_STREAM("Translation: "<<end_effector_state.translation());
ROS_INFO_STREAM("Rotation: "<<end_effector_state.rotation());

boolfound_ik=robot_state->setFromIK(joint_model_group,end_effector_state,10,0.1);
      if (found_ik)
      {
        robot_state->copyJointGroupPositions(joint_model_group,joint_values);
        for(std::size_t i=1;i<=joint_values.size();++i)
        {
          ROS_INFO("Joint %s: %f",joint_name[i].c_str(),joint_values[i]);
        }
      }
      else 
      {
        ROS_INFO("Did not find IK solution");
      }

最關鍵的就是group.getCurrentState()，該函式返回一個當前的robot_state::RobotStatePtr型別的變數，得到當前的狀態資訊。

可以通過robot_state->getGlobalLinkTransform("link name")得到某個關節的狀態，這裡的返回值是引用，用於傳入函式後實現傳入的形參的變化。

可以通過robot_state->setFromIK將之前定義的某個關節的狀態以及joint_model_group傳入，逆運動學求解的關節值傳入joint_model_group,使用robot_state->copyJointGroupPosition將joint_model_group的值複製到vector型別的joint_values中。

其中

      group.getCurrentJointValues()
      group.getCurrentPose()
      group.getCurrentRPY()

能得到當前的關節值，末端執行器的位姿，末端執行器的旋轉角row-pitch-yaw(XYZ)尤拉角。

另外，得到當前的狀態還有另一種方法

    robot_state::RobotState target_state =	group_->getJointValueTarget();
    target_state.update();
    //group_->getcurrentstate does not give the correct values
    robot_state::RobotState current_state(target_state);