MoveIt!学习笔记3-MoveItRobotModelandRobotState（读取。
。。
此博⽂主要是⽤来记录ROS-Kinetic 中，⽤于机器⼈轨迹规划的MoveIt功能包的学习记录。
英⽂原版教程见此链接：
引： 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();
apple car
// Step1：读取机器⼈模型信息：创建RobotModelLoader对象，从ROS服务中读取"robot_description"内的机器⼈模型信息，
//        之后创建⼀个RobotModelPtr对象，将机器⼈模型存⼊其中。
// .. _RobotModelLoader:
//    /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();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());卖车估价
// Step2：转存机器⼈当前状态：创建⼀个RobotStatePtr kinematic_state对象，并将上边加载有机器⼈模型数据的数值传⼊
福特野马gt1000//        创建⼀个JointModelGroup* joint_model_group指针对象，读取model⾥⾯的JOintGroup
//        创建⼀个String类型的向量，⽤来存储joint——grope内部个关节的名称
//        注意！！重要！！ kinematic_state这个对象，是⽤来操作机器⼈运动等的核⼼
雷克萨斯gs250
//                      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
// 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_anslation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_ation() << "\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;
}