[ROS MoveIT 系列] 認識機器人模型與狀態

<圖片來源 : MoveIt>

一、前言

在機器人程式設計中，機器人模型與機器人狀態的類別是核心的類別，在機器人正向學中扮演著重要角色。

機器人模型的類別涵蓋所有連桿和關節間的關係，包括從 URDF 載入的關節極限屬性。機器人模型也分別將運動規劃中的連桿和關節資訊，定義在 SRDF 檔案中。

機器人狀態類別則是用以像快照般記錄當前的機器人資訊，包含關節位置的向量、速度與加速度的值，藉以獲得關於機器人的正向運動學資訊。

機器人狀態類別也包含一些小幫手的函式，用來設定基於 end effector (末端效應器) 位置的機器手臂的位置 (卡式座標位置)，並計算卡式座標中的軌跡。

二、實作

啟動教學 Launch 檔，指令如下 :

roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch

輸出將如下，每個人實作的數值也會不同，是因為我們使用了隨機關節數值。

<source : greatway9999>

三、重點解析

官方完整的程式碼，可於此連結查看。

1.將 RobotModelLoader 進行實例化

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
moveit::core::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

首先將 RobotModelLoader 這個物件進行實例化。RobotModelLoader 可以讓我們在 ROS 的參數服務器中查找機器人的描述資訊，且建構一個讓我們使用的 "RobotModel"。

2.建構 RobotState

moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");

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

使用 "RobotModel"，我們可以建構一個 "RobotState" 來維護機器人的設定檔。

設定所有關節的預設值

然後，我們會獲得一個 "JointModelGroup"，"JointModelGroup" 是以一個特別的群組來代表機器人模型，例如 熊貓機器人的 "panda_arm" 。

3. 取得熊貓手臂狀態的當前值

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

可以透過上述程式來取得儲存在熊貓手臂狀態的當前值。

4.執行關節極限

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

setJointGroupPositions()  這個函式並不會自行執行關節極限，但會呼叫 enforceBounds() 來執行。

5. 正向運動學

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& 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");

到此，我們就可以計算一組隨機關節值的正向運動學。我們會傾向尋找 "panda_link8" 的姿態，因為 "panda_link8" 在 機器人的"panda_arm"群組中是最遠的連桿。

6.反向運動學

double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, timeout);

我們可以求解熊貓機器手臂的反向運動學之解。遵照以下兩個重點 :

• 末端效應器的預想姿態 (預設為 "panda_arm" 最末節) : " end_effector_state " 在上個步驟已經計算了。
• timeout (時間延遲) : 0.1秒

接著為秀出 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.求得 Jacobian ( Jacobian矩陣)

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

8. Launch file

讓 Launch 檔能順利執行程式的 2 個關鍵 :

• 從參數伺服器中讀取 熊貓機器手臂的 URDF 與 SRDF 檔。
• 從參數伺服器中，將自 MoveIT設定幫手產生的 kinematics_solver (正向運動學解析器) 的設定檔載入。

<launch>
  <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>

9.對機器任狀態進行除錯

以下指令可進行系統的內部檢查。

rosrun moveit_ros_planning moveit_print_planning_model_info

---

參考資料 :

• MoveIt : Robot Model and Robot State