【ROS-MoveIt！原始碼學習】ROS中機器人模型的構建（Build RobotModel）
阿新 • • 發佈：2018-11-25
1.本文討論的內容和目的
本文從ROS MOVEIT!的入門教程moveit_tutorial中的運動學模型（kinematic_model_tutorial.launch.cpp）入手，目的是想搞明白：
（1）ROS是如何匯入機器人模型；（本文內容）
（2）如何呼叫逆運動學演算法求解。（以後再寫）
搞明白這兩件事後，就能夠自己定義一個機械臂，然後呼叫自己編寫的逆運動學演算法。
2.ROS中的機器人模型表示方法
首先說一下ROS中機器人形式，這裡只說移動機器人和機械臂兩種形式，對移動機器人的基本要求是其整體能夠從一個狀態運動到另一個狀態，而對機械臂的基本要求是希望機械臂的末端從一個狀態移動到另一個狀態，在運動過程中同時使移動機器人和機械臂滿足一些運動學和動力學約束等。如果希望機械臂能夠避障的話，還需要採用運動規劃（Motion Planning）的策略。
2.1 ROS如何處理複雜的機器人模型
如果使用者的機器人CAD模型只有一隻機械臂的話（例如LWR機械臂），那麼問題就很簡單：針對機械臂所有的關節（7個），給定一個搬運的任務，呼叫現有的逆解演算法就可以了。但是如果機器人的模型是類似於PR2機器人的形式，問題就比較複雜了，因為PR2機器人包含有兩隻手臂，兩個手爪，底座是一個小車，一共有80多個關節，既有移動機器人部分也有機械臂部分。在對PR2設計任務的時候，可能在某一時刻只需要對右臂進行規劃，也就是隻需考慮右臂的7個轉動關節。所以這個時候就體現出了SRDF檔案機制的優越性了，SRDF檔案中使用了將多個的關節重組為若干個關節組合 這樣的策略，這樣就可以對特定的關節組合進行規劃，例如將PR2的右臂共7個關節組成名稱為“right_arm”的關節組和。
圖1 LWR的簡化模型
圖2 pr2機器人
2.2. ROS中的URDF檔案和SRDF檔案
剛才提到了SRDF檔案，其實在ROS中，構建機器人的模型需要將URDF檔案和SRDF配合使用，ROS可以通過launch檔案將PR2的URDF檔案和SRDF檔案上傳到ROS引數伺服器中，然後就可以例項化RobotModelLoader，從ROS引數伺服器中查詢名為“robot_description”的PR2機器人模型。然後例項化後的RobotModel可以呼叫特定的關節組合進行規劃。
3. 編寫節點顯示pr2機器人模型的資訊
現在假設不知道moveit_tutorial提供的pr2機器人模型的具體情況（moveit_tutorial的連結），當然我們可以通過檢視urdf檔案和srdf檔案，但是這裡我通過編寫程式來檢視。程式的邏輯大概可以表示為：
3.1 launch檔案
launch檔案（kinematic_model_tutorial.launch）的內容為：
1<launch>
2 <include file="$(find pr2_moveit_config)/launch/planning_context.launch">
3 <arg name="load_robot_description" value="true"/>
4 </include>
5
6 <node name="kinematic_model_tutorial"
7 pkg="moveit_tutorials"
8 type="kinematic_model_tutorial"
9 respawn="false" output="screen">
10 <rosparam command="load" file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
11 </node>
12</launch>
其中第2-4行命令表示開啟另一個launch檔案（planning_contex.launch），內容如下：
1<launch>
2 <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
3 <arg name="load_robot_description" default="false"/>
4
5 <!-- Load universal robotic description format (URDF) -->
6 <param if="$(arg load_robot_description)" name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'" />
7
8 <!-- The semantic description that corresponds to the URDF -->
9 <param name="robot_description_semantic" textfile="$(find pr2_moveit_config)/config/pr2.srdf" />
10
11 <!-- Load to the parameter server yaml files -->
12 <group ns="robot_description_planning">
13 <rosparam command="load" file="$(find pr2_moveit_config)/config/joint_limits.yaml"/>
14 </group>
15
16 <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
17 <group ns="robot_description_kinematics">
18 <rosparam command="load" file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
19 </group>
20
21</launch>3.2 讀取ROS的引數伺服器的所有引數
在終端輸入指令“rosparam list”，會顯示出所有的引數，結果如下，可以看到第一個就是機器人模型的引數"robot_description"。
/robot_description
/robot_description_kinematics/left_arm/kinematics_solver
/robot_description_kinematics/left_arm/kinematics_solver_attempts
/robot_description_kinematics/left_arm/kinematics_solver_search_resolution
/robot_description_kinematics/left_arm/kinematics_solver_timeout
/robot_description_kinematics/left_arm_and_torso/kinematics_solver
/robot_description_kinematics/left_arm_and_torso/kinematics_solver_attempts
/robot_description_kinematics/left_arm_and_torso/kinematics_solver_search_resolution
/robot_description_kinematics/left_arm_and_torso/kinematics_solver_timeout
/robot_description_kinematics/right_arm/kinematics_solver
/robot_description_kinematics/right_arm/kinematics_solver_attempts
/robot_description_kinematics/right_arm/kinematics_solver_search_resolution
/robot_description_kinematics/right_arm/kinematics_solver_timeout
/robot_description_kinematics/right_arm_and_torso/kinematics_solver
/robot_description_kinematics/right_arm_and_torso/kinematics_solver_attempts
/robot_description_kinematics/right_arm_and_torso/kinematics_solver_search_resolution
/robot_description_kinematics/right_arm_and_torso/kinematics_solver_timeout
/robot_description_planning/joint_limits/l_elbow_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_elbow_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_elbow_flex_joint/max_acceleration
/robot_description_planning/joint_limits/l_elbow_flex_joint/max_velocity
/robot_description_planning/joint_limits/l_forearm_roll_joint/angle_wraparound
/robot_description_planning/joint_limits/l_forearm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_forearm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_forearm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/l_forearm_roll_joint/max_velocity
/robot_description_planning/joint_limits/l_shoulder_lift_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_shoulder_lift_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_shoulder_lift_joint/max_acceleration
/robot_description_planning/joint_limits/l_shoulder_lift_joint/max_velocity
/robot_description_planning/joint_limits/l_shoulder_pan_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_shoulder_pan_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_shoulder_pan_joint/max_acceleration
/robot_description_planning/joint_limits/l_shoulder_pan_joint/max_velocity
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/l_upper_arm_roll_joint/max_velocity
/robot_description_planning/joint_limits/l_wrist_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_wrist_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_wrist_flex_joint/max_acceleration
/robot_description_planning/joint_limits/l_wrist_flex_joint/max_velocity
/robot_description_planning/joint_limits/l_wrist_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/l_wrist_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/l_wrist_roll_joint/max_acceleration
/robot_description_planning/joint_limits/l_wrist_roll_joint/max_velocity
/robot_description_planning/joint_limits/r_elbow_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_elbow_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_elbow_flex_joint/max_acceleration
/robot_description_planning/joint_limits/r_elbow_flex_joint/max_velocity
/robot_description_planning/joint_limits/r_forearm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_forearm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_forearm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/r_forearm_roll_joint/max_velocity
/robot_description_planning/joint_limits/r_shoulder_lift_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_shoulder_lift_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_shoulder_lift_joint/max_acceleration
/robot_description_planning/joint_limits/r_shoulder_lift_joint/max_velocity
/robot_description_planning/joint_limits/r_shoulder_pan_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_shoulder_pan_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_shoulder_pan_joint/max_acceleration
/robot_description_planning/joint_limits/r_shoulder_pan_joint/max_velocity
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/max_acceleration
/robot_description_planning/joint_limits/r_upper_arm_roll_joint/max_velocity
/robot_description_planning/joint_limits/r_wrist_flex_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_wrist_flex_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_wrist_flex_joint/max_acceleration
/robot_description_planning/joint_limits/r_wrist_flex_joint/max_velocity
/robot_description_planning/joint_limits/r_wrist_roll_joint/has_acceleration_limits
/robot_description_planning/joint_limits/r_wrist_roll_joint/has_velocity_limits
/robot_description_planning/joint_limits/r_wrist_roll_joint/max_acceleration
/robot_description_planning/joint_limits/r_wrist_roll_joint/max_velocity
/robot_description_semantic
/rosdistro
/roslaunch/uris/host_robotic_ideapad_y470__40618
/rosversion
/run_id3.3 編寫節點程式
為了演示ROS如何構建機器人模型，對節點kinematic_model_tutorial.launch.cpp進行了一些增刪，去掉了對各個關節的極限設定，正運動學和逆運動學的內容，這些以後有機會再寫。
#include <ros/ros.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <vector>
#include <string>
int main(int argc, char **argv)
{
ros::init (argc, argv, "right_arm_kinematics");
ros::AsyncSpinner spinner(1);
spinner.start();
//例項化RobotModelLoader，從ROS引數伺服器中查詢名為“robot_description”的機器人模型
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
std::cout<<"robot_model_loader done!"<<std::endl;
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
//列出pr2機器人中所有的關節模型的名稱和關節型別
std::cout<<"----------Get All JointModels(Name and Type)----------"<<std::endl;
for(size_t i=0;i<kinematic_model->getJointModels().size();++i)
{
std::cout<<i+1<<" : Joint model Name: "<<kinematic_model->getJointModels()[i]->getName()<<std::endl;
std::cout<<"Joint model Type: "<<kinematic_model->getJointModels()[i]->getTypeName()<<std::endl;
}
std::cout<<std::endl;
//列出所有的關節組合的名稱
std::cout<<"----------Get All Jointmodelgroups(Names)----------"<<std::endl;
for(size_t i=0;i<kinematic_model->getJointModelGroupNames().size();++i)
{
std::cout<<i+1<<" JointModelGroup"<<": "<<kinematic_model->getJointModelGroupNames()[i]<<std::endl;
}
std::cout<<std::endl;
//列出關節組合“right_arm”中的所有的關節模型的名稱和關節型別
std::cout<<"----------Get right_arm's Jointmodel(Names and Type) ----------"<<std::endl;
const robot_state::JointModelGroup* right_arm_joint_model_group = kinematic_model->getJointModelGroup("right_arm");
for(size_t i=0;i<right_arm_joint_model_group->getJointModelNames().size();++i)
{
std::cout<<i+1<<" Name: "<<right_arm_joint_model_group->getJointModelNames()[i]<<std::endl;
std::cout<<"Type: "<<right_arm_joint_model_group->getJointModels()[i]->getTypeName()<<std::endl;
}
std::cout<<std::endl;
ros::shutdown();
return 0;
}
在終端輸入命令：
roslaunch moveit_tutorials kinematic_model_tutorial.launch[ INFO] [1496657572.700761585]: Loading robot model 'pr2'...
[ WARN] [1496657572.716620179]: The root link base_footprint has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
robot_model_loader done!
----------Get All JointModels(Name and Type)----------
1 : Joint model Name: world_joint
Joint model Type: Planar
2 : Joint model Name: base_footprint_joint
Joint model Type: Fixed
………………
87 : Joint model Name: r_torso_lift_side_plate_joint
Joint model Type: Fixed
88 : Joint model Name: torso_lift_motor_screw_joint
Joint model Type: Revolute
----------Get All Jointmodelgroups(Names)----------
1 JointModelGroup: arms
2 JointModelGroup: base
3 JointModelGroup: head
4 JointModelGroup: left_arm
5 JointModelGroup: left_arm_and_torso
6 JointModelGroup: left_gripper
7 JointModelGroup: right_arm
8 JointModelGroup: right_arm_and_torso
9 JointModelGroup: right_gripper
10 JointModelGroup: torso
11 JointModelGroup: whole_body
----------Get right_arm's Jointmodel(Names and Type) ----------
1 Name: r_shoulder_pan_joint
Type: Revolute
2 Name: r_shoulder_lift_joint
Type: Revolute
3 Name: r_upper_arm_roll_joint
Type: Revolute
4 Name: r_upper_arm_joint
Type: Fixed
5 Name: r_elbow_flex_joint
Type: Revolute
6 Name: r_forearm_roll_joint
Type: Revolute
7 Name: r_forearm_joint
Type: Fixed
8 Name: r_wrist_flex_joint
Type: Revolute
9 Name: r_wrist_roll_joint
Type: Revolute
[kinematic_model_tutorial-2] process has finished cleanly
log file: /home/gpeng/.ros/log/84904d0c-49d7-11e7-9538-74e50b6fd36e/kinematic_model_tutorial-2*.log
結果顯示了moveit_tutorial提供的pr2.urdf機器人共有88個關節模型，11個關節組合（包括“right_arm”）,"right_arm"組合中包含9個關節，其中2個是fixed的，7個是revolute，猜測只有7個revolute的關節參與了規劃。