ROS Moveit运动学探讨

本篇文章探讨moveit的运动学，moveit提供的API可以从URDF档中解手臂运动学。以Moveit范例连结做示范。
moveit使用RobotModel与RobotState这两个class做运动学相关的计算。RobotModel负责从URDF、SRDF
中抓取该手臂的几何参数，比如:杆件参数、关节参数..等等，RobotState则是可以进行运动学上的操作。

首先此范例在launch中载入上述需要的参数。首先执行robot_model_and_robot_state_tutorial.launch，这当中还执行一个planning_context.launch，planning_context内将档案panda_arm.urdf.xacro读取到robot_description内。panda_moveit_config)/config/panda_arm.srdf.xacro放入robot_description_semantic内。
其他参数:robot_description_planning、robot_description_kinematics也都放入相对应的档案joint_limits.yaml、kinematics.yaml。

程序码部分这参考连结一中将程序码加上注解，可以参考此说明。
第一段程序码:

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());

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();

第一行的robot_description，就是档案panda_arm.urdf.xacro，执行程序时可以在另一个视窗中使用rosparam list、rosparam get /robot_description 查询内容。

这一段使用RobotModelLoader作资料读取。使用RobotModelLoader是因为，RobotState宣告时可以放入自己跟RobotModel。至於程序码中的RobotStatePtr、RobotModelPtr，经过测试，将指令替换成RobotState、RobotModel，结果是一样的，只是Ptr是指标类型。robot_State实作连结

std::vector<double> joint_values;
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]);
}

这一段单纯使用copyJointGroupPositions指令取出个关节角度并且printf出来。copyJointGroupPositions的用法可以见RobotState的连结，copyJointGroupPositions有很多的多载，可以取出一个角度、全部的角度。

joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, 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"));

测试角度范围，satisfiesBounds会回传true、false来判断。这角度设定的资料储存在档案panda_arm_xacro内，也就是被launch所呼叫的档案。
使用enforceBounds之後，会把超过的角度修正到Limit内，以这题为例joint_values设为5.57，经过enforceBounds之後，把角度印出来看，joint_values[0]会变成2.89，2.89是panda_arm_xacro内的Limit设定参数)。

以上内容属於热身，下一段进入正题正反向运动学。

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");

ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");

这部分使用setToRandomPositions随机产生角度，自己测试时可以采用上一段的setJointGroupPositions自行给数值去代替。getGlobalLinkTransform是取出该link的资料，此范例用的是panda_link8，也可以换成其他link。

下一段反向运动学:

std::size_t attempts = 10;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);

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");
}

由於这运动学是使用数值解，一开始要先设定attempts也就是迭代次数，至於最佳的迭代次数还要进一步测试。
这范例中使用setFromIK，放入上一节正向运动学的end_effector_state，可以自行宣告一组Affine3d去定义想要的座标。

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");

最後一段是运动学中较困难的Jacobian。范例中的Jacobian为6x8 因为该手臂有8轴。第一个row到第三个row是速度，第四个row到最後是角速度。
不过这范例中的结果来分析，这里的Jacobian是没有乘上角速度，所以第四个row到第六个row其实是轴方向。所以如果要後续的计算，如:速度、角速度、力量分析，就要利用这结果去算