MoveIt! 學習筆記3- MoveIt RobotModel and Robot State (讀取機器人關節等資訊，測試正解逆解是否可行)

此博文主要是用來記錄ROS-Kinetic 中，用於機器人軌跡規劃的MoveIt功能包的學習記錄。 

英文原版教程見此連結：http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html

 引： MoveIt RobotModel Robot_State這個教程主要是用來檢查URDF+SRDF檔案內容，檢視機器人關節限制，設定座標，檢測機器人是否能夠通過正解和逆解的方式，達到預設座標點；雖然在這個教程裡，這些功能用來做測試，但是，在以後的專案中，可以用其作為參考，進行運動學正逆解規劃測試等，很有學習必要。

注：MoveIt教程基本上都是通過官方案例程式，展示具體實現的過程和程式碼資訊，所以本博文采用在官方程式中，新增中文註解的方式，進行學習和記錄。

 

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Sachin Chitta, Michael Lautman*/

#include <ros/ros.h>

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

int main(int argc, char** argv)
{
  //Step0: 建立ROS節點，並開啟同步迴圈體
  ros::init(argc, argv, "robot_model_and_robot_state_tutorial");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // Step1： 讀取機器人模型資訊：建立RobotModelLoader物件，從ROS服務中讀取"robot_description"內的機器人模型資訊，
  //         之後建立一個RobotModelPtr物件， 將機器人模型存入其中。
  // .. _RobotModelLoader:
  //     http://docs.ros.org/kinetic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
  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());



  // Step2： 轉存機器人當前狀態：建立一個RobotStatePtr kinematic_state物件，並將上邊載入有機器人模型資料的數值傳入
  //        建立一個JointModelGroup* joint_model_group指標物件，讀取model裡面的JOintGroup
  //        建立一個String型別的向量，用來儲存joint——grope內部個關節的名稱
  //        注意！！重要！！ kinematic_state這個物件，是用來操作機器人運動等的核心
  //                      joint_model_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("panda_arm");

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



  // Step3： 轉存機器人關節狀態：建立一個double型別的向量，並從kinematic_state物件中，讀取機器人各個關節的當前位置，並使用for迴圈遍歷顯示
  std::vector<double> joint_values;
  kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); //從前邊input讀取，存入後邊的output
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
  }

  // Step4：設定一個關節值，並確認這個關節值是否超出限制值？
  //        從上邊已經存有各關節位置的vector物件中，單獨設定其中1軸的轉角。
  //        將設定完轉角的新的關節Vector，設定到機器人的kinematic_state內部。
  //        使用下邊兩條語句，檢測是否超限？ （經檢查，超限）
  //        將當前關節數值，強制設定為機器人新的limitation，然後再次檢查關節位置是否超限？（經檢查，未超限）
  // ^^^^^^^^^^^^
  // setJointGroupPositions() does not enforce joint limits by itself, but a call to enforceBounds() will do it.
  /* Set one joint in the Panda arm outside its joint limit */
  joint_values[0] = 5.57;
  kinematic_state->setJointGroupPositions(joint_model_group, joint_values);

  /* Check whether any joint is outside its joint limits */
  ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

  /* Enforce the joint limits for this state and check again*/
  kinematic_state->enforceBounds();
  ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));





  // Step5：運動學正解： 首先，使用setToRandomPositions語句，設定一個隨機的位置給：joint_model_group（也就是panda——arm）
  //                   之後，建立一個Eigen::Affine3d&四元數型別物件，從kinematic_state中，讀取設定完隨機位置之後的，末端執行其的座標
  //                   顯示最終的末端執行器的位置及轉角     
  // Forward Kinematics
  // ^^^^^^^^^^^^^^^^^^
  // Now, we can compute forward kinematics for a set of random joint
  // values. Note that we would like to find the pose of the
  // "panda_link8" which is the most distal link in the
  // "panda_arm" group of the robot.
  kinematic_state->setToRandomPositions(joint_model_group);//對運動組設定隨機位置
  const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");

  /* Print end-effector pose. Remember that this is in the model frame */
  ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
  ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");


  // Step6：運動學逆解： 設定運動學逆解引數，並進行規劃
  // Inverse Kinematics
  // ^^^^^^^^^^^^^^^^^^
  // We can now solve inverse kinematics (IK) for the Panda robot.
  // To solve IK, we will need the following:
  //
  //  * The desired pose of the end-effector (by default, this is the last link in the "panda_arm" chain):
  //    end_effector_state that we computed in the step above.
  //  * The number of attempts to be made at solving IK: 10
  //  * The timeout for each attempt: 0.1 s
  std::size_t attempts = 10;
  double timeout = 0.1;
  bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);

  // Now, we can print out the IK solution (if found):
  if (found_ik)
  {
    kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    for (std::size_t 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");
  }

  // Step7：獲取機器人雅各比矩陣
  // ^^^^^^^^^^^^^^^^
  // We can also get the Jacobian from the :moveit_core:`RobotState`.
  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");
  // END_TUTORIAL

  ros::shutdown();
  return 0;
}