Moveit代码 集合（1）Robot Model and Robot State(机器人模型和机器人状态）

一、用到的ROS函数

1.ros::init（）初始化

  ros::init(argc, argv, "robot_model_and_robot_state_tutorial");//初始化ros节点，节点名为"robot_model_and_robot_state_tutorial，为避免节点名重复，可以在后面添加ros::init_options::AnoymousName参数
  ros::AsyncSpinner spinner(1);//多线程调用，1指调用一个线程
  spinner.start();

2.ros::shutdown(),终止ros进程

二、Robot Model and Robot State(机器人模型和机器人状态）

注意：机器人模型为固定值，不允许修改，所有操作均是对机器人状态进行操作，所以要将机器人模型的各种参数值复制到机器人状态中，并对机器人状态进行操作。

RbbotModel 和RobotState类是核心类，可以通过其访问机器人运动学

RobotModel类包含了机器人所有的关节（joint）和臂（link）之间的关系，包括关节限值属性，这些数据从URDF模型中加载。该类也将机器人关节和臂分离到planning group中。

Robotstate类包含了机器人在某时刻在某个确定点的信息，包括关节位置向量，也可以包含速度和加速度向量。这些信息可以被用来获取机器人当前状态的运动学信息，例如雅可比矩阵等。此类也提供了基于末端执行器位置设置机械臂位置的函数。

1.加载机器人模型，机器人的所有数据均存储在RobotModel中，且为const量，无法修改

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");//创加载机器人模型，"robot_description"为参数服务器中的参数，在.launch文件中定义，从ROS路径中寻找指定的urdf模型（panda）
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();//创建RobotModel类的智能指针，存储找到的机器人模型（通过robot_model_loader.getModel()获取），此数据为const值
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());//ROS_INFO是输出，等价于printf，输出机器人模型的Frame名字，c_str是将字符串转化为C语言中的字符串类型。

2.根据机器人模型建立机器人状态，设置机器人状态为默认值，创建机器人关节数据组，存储机器人模型信息和关节名信息

note:注意机器人模型为const值，不能修改，机器人状态可以修改

moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model)); //在堆区开辟空间，新建RobotState类，存储kinematic_model的机器人状态，RobotState量可以修改
kinematic_state->setToDefaultValues();//将kinematic_model的机器人状态设置为默认值
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");//通过kinematic_model->getJointModelGroup("panda_arm")将存储在RobotState类中的机器人模型数据，复制到JointModelGroup类的指针
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();//通过joint_model_group->getVariableNames()获取RobotState中的关节名，存储于新建的字符串向量中

3.获取关节值，给机器人状态赋值（机器人模型均为const量，不能修改）

std::vector<double> joint_values;//创建double类型的向量
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);//将从机器人模型（RobotState——kinematic_model）获取的关节模型数据和关节值数据复制到机器人状态（kinematic_state）中
//循环输出关节名和关节值
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
  ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}

4.获取关节限制

joint_values[0] = 5.57;//设置关节[0]的值为5.57
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);//将修改后的joint_values设置为机器人当前状态

ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));//判断机器人状态是否满足限制，并输出

kinematic_state->enforceBounds();//强制要求机器人状态符合机器人限制
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));//判断机器人状态是否满足限制，并输出

5.正运动学（从机器人关节空间到笛卡尔空间），通过当前机器人各关节值，计算末端执行器相对于基坐标系的位置，即为求解正运动学

kinematic_state->setToRandomPositions(joint_model_group);//将机器人状态设置为随机值
const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");//获取link8（即末端执行器）相对于基坐标系的的变换，存入创建的欧式变换矩阵end_effector_state，该矩阵为4*4
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");//输出平移变换
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");//输出旋转变换矩阵

6.逆运动学求解（Inverse Kinematics,IK），已知末端执行器相对于基坐标系的位置，求取各个关节值

double timeout = 0.1;//创建时间延迟变量
//求逆运动学，需要的参数为:目标位置，即末端执行器位置（end_effector_state）,时间延迟，即求解时间最大值（timeout），机器人模型（joint_model_group）
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, timeout);
//如果找到IK，循环输出机器人各关节值
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");
}

7.获取雅可比矩阵，行数为机械臂自由度数，列数为机械臂关节数量

Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);//创建参考点，欧拉角（0，0，0）
Eigen::MatrixXd jacobian;//创建雅可比矩阵，尺度x*x
//获取雅可比矩阵:计算雅可比矩阵的关节组，计算雅可比矩阵的link组名字，参考点（相对于link组中link的相对位置），结果雅可比矩阵
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");//输出雅可比矩阵

8.launch文件：roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch

<launch>
<!--加载urdf模型和srdf-->
  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>
<!--定义节点属性，节点名，包名，重命名节点名，出错不重启，日志输出在桌面-->
  <node name="robot_model_and_robot_state_tutorial"
        pkg="moveit_tutorials"
        type="robot_model_and_robot_state_tutorial"
        respawn="false" output="screen">
<!--加载运动学求解器-->
    <rosparam command="load"
              file="$(find panda_moveit_config)/config/kinematics.yaml"/>
  </node>
</launch>