MoveIt 入門教學 - 加入Trac_IK運動學求解(Trac-IK Kinematics Solver)

MoveIt入門教學 - 加入Trac_IK運動學求解(Trac-IK Kinematics Solver)

说明

• 由於在使用MoveIt中最常見的KDL作為Kinematics Solver的過程中，在進行基本操作時擁有不錯的使用體驗。
• 但隨著任務需求的增加，在加入環境障礙物以及attach object等操作之後，會遇到執行運算時間過長的問題。甚至可能會因此而失敗(timeout)。
• 因此嘗試使用不同的Kinematics Solver來解決此一問題，目前測試使用Trac_IK，普遍上會擁有較佳的結果，甚至在某些條件下，運算速度可能會相差十倍以上，並且也比較不會出現失敗的狀況。
• 

甚麼是Trac_IK

• 来自ROS維基百科上的說明：Trac_IK係作為目前最常見的運動學編譯器(KDL)的改良版本，提供一更快速，且更穩定的運動學編譯器。在處理關節極限(joint-limited)的數值解上會具有較佳的結果，不會再增加額外的計算時間。
• 並且Trac_IK具有與KDL編譯器相近的API calls，僅需調整部分輸入參數即可切換使用。詳細計算內容請參閱ROS Wiki - Trac_IK中的說明。

MoveIt! Trac_IK

• MoveIt! Trac_IK是一種Open-Source的函式庫，提供一改善的解運動學的工具。
• 本文旨在說明如何在機器人的MoveIt系統中使用Trac_IK的强大功能
• 本文中MoveIt! Trac_IK的測試環境是ROS Kinetic，使用catkin编译。理論上可以工作再任何自由度的機械臂上。

準備

• 应该已经通过MoveIt!配置助手生成MoveIt!配置包

MoveIt! IKFast 安装

兩種安裝方式：二進制debs包或源安裝

Binary Install:

sudo apt-get install ros-kinetic-trac-ik-kinematics-plugin

源安装：（在catkin工作空間）
1. 至官網source中下載原始程式碼
2. 將package "trac_ik_kinematics_plugin"及"trac_ik_lib"放入catkin的工作空間中
3. 於機器人的配置文件中"/config/"找到"kinematics.yaml"文件，並將文件內的

kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 置換為 
=> kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

4. 調整參數

kinematics_solver_timeout (timeout in seconds, e.g., 0.005) and position_only_ik ARE supported.
solve_type can be Speed, Distance, Manipulation1, Manipulation2 (see trac_ik_lib documentation for details). Default is Speed.
kinematics_solver_attempts parameter is unneeded: unlike KDL, TRAC-IK solver already restarts when it gets stuck
kinematics_solver_search_resolution is not applicable here.

備註: 在完成Trac_IK的安裝之後，即可直接開啟使用。並且在之後的MoveIt_Setup_Assistant中，也會直接出現Trac_IK的選項可以直接選取。

參考資料：
ROS Wiki - Trac_IK
trac_ik - source
MoveIt!入门教程-生成IKFast插件

ROS筆記 - 機器人模型URDF

　　URDF (Unified Robot Description Format) - 統一機器人描述格式， 是一種特殊的XML文件格式，用來描述機器人的模型。

1. URDF語法規範

參考：XML specifications

2. URDF教程

      1. 使用URDF建立機器人模型
          了解如何構建可在Rviz中查看的機器人的可視化模型

      

      2. 建立可移動的URDF機器人模型
          了解如何在URDF中定義可移動關節

      3. 在URDF模型中加入碰撞與物理屬性
          了解如何加入關節碰撞與慣性特性，以及關節動學特性(friction, damping)

      4. 使用Xacro整理URDF文件
          了解如何Xacro精簡URDF文件內容

3. 建立URDF文件 

參考URDF: Create your own urdf file

3.1 link 和 joint

　　在URDF中，機器人主要結構透過link和joint來表示。link定義實際機器人的機構件，而前後link(parent link, child link)之間使用joint做連接，並可再設定joint的運動方式(fixed, revolute, contunuous...)。link與joint的位置關係是透過前後之間的相對位置來定義，child link的中心位置以joint的位置做為參考，joint則是相對於前一個joint(parent link的joint)的位置作參考。

　　下圖為ROS URDF教學的範例，將創建一個樹狀結構的機器人模型：

3.2 基礎模型
　　本節將從機器人的整體結構出發，不考慮過多的細節，可以將機器人通過如下的URDF表示：

   1 <robot name="my_robot">
   2   <link name="link1" />
   3   <link name="link2" />
   4   <link name="link3" />
   5   <link name="link4" />
   6 
   7   <joint name="joint1" type="continuous">
   8     <parent link="link1"/>
   9     <child link="link2"/>
  10   </joint>
  11 
  12   <joint name="joint2" type="continuous">
  13     <parent link="link1"/>
  14     <child link="link3"/>
  15   </joint>
  16 
  17   <joint name="joint3" type="continuous">
  18     <parent link="link3"/>
  19     <child link="link4"/>
  20   </joint>
  21 </robot>

設定完畢後，ROS為用戶提供了一個檢查URDF語法的工具

$ check_urdf my_robot.urdf

如果一切正常，將會顯示如下：

robot name is: test_robot

---------- Successfully Parsed XML ---------------

root Link: link1 has 2 child(ren)

    child(1):  link2

    child(2):  link3

        child(1):  link4

3.3 設定link參數
　　接著定義機器人機構件(link)的參數，在link中可以分成設定機構件的外觀、碰撞、慣性等項目，並分別設定機構外型<geometry> (box/cylinder/mesh等)、偏移量<origin xyz> (公尺)、旋轉量<origin rpy> (徑度)等參數。選擇自己需要的項目設定即可。

   1  <link name="my_link">
   2    <inertial>
   3      <origin xyz="0 0 0.5" rpy="0 0 0"/>
   4      <mass value="1"/>
   5      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
   6    </inertial>
   7 
   8    <visual>
   9      <origin xyz="0 0 0" rpy="0 0 0" />
  10      <geometry>
  11        <box size="1 1 1" />
  12      </geometry>
  13      <material name="Cyan">
  14        <color rgba="0 1.0 1.0 1.0"/>
  15      </material>
  16    </visual>
  17 
  18    <collision>
  19      <origin xyz="0 0 0" rpy="0 0 0"/>
  20      <geometry>
  21        <cylinder radius="1" length="0.5"/>
  22      </geometry>
  23    </collision>
  24  </link>

3.4 加入joint設定
　　在joint中可以設定關節的類型<joint type>、偏移量<origin>、旋轉軸<axis>、上下限<liniit lower/upper>等參數，

1  <robot name="test_robot">
   2    <link name="link1" />
   3    <link name="link2" />
   4    <link name="link3" />
   5    <link name="link4" />
   6   
   7    <joint name="joint1" type="fixed">
   8      <parent link="link1"/>
   9      <child link="link2"/>
  10      <origin xyz="5 3 0" rpy="0 0 0" />  12    </joint>
  13   
  14    <joint name="joint2" type="continuous">
  15      <parent link="link1"/>
  16      <child link="link3"/>
  17      <origin xyz="-2 5 0" rpy="0 0 1.57" />
  18      <axis xyz="-0.707 0.707 0" />
  19    </joint>
  20   
  21    <joint name="joint3" type="revolute">
  22      <parent link="link3"/>
  23      <child link="link4"/>
  24      <origin xyz="5 0 0" rpy="0 0 -1.57" />
  25      <axis xyz="0.707 -0.707 0" />  26      <limit effort="300" lower="-2.617" upper="1.983" velocity="1"/>  28    </joint>
  29  </robot>

3.5 顯示URDF圖形
　　設定完成之後，可以呼叫urdf_tutorial中提供的display.launch開啟rviz的3D模型，確認是否設定正確

$ roslaunch urdf_tutorial display.launch model:=/home/seven/dev/rosbook/seven_moveit_config/urdf/RCK100.urdf

3.6 載入mesh模型
　　在URDF <geometry>中，除了使用預設的幾何圖形外，亦可載入3D模型圖像


參考資料
<ROS> 機器人描述--URDF和XACRO

ROS - MoveIt 快速搭建機器人運動規劃平台

　　MoveIt是由ROS中一系列運動操作的功能包组成，包含運動規劃、操作控制、3D感知、運動學、碰撞檢測等等，並包含GUI控制介面。另外，MoveIt官網上還有相當詳細的教學與API說明。
　　MoveIt官網：http://moveit.ros.org/

MoveIt架構

下圖是MoveIt的系統架構圖

move_group是MoveIt的核心部分，可以綜合機器人的各獨立組件，為用戶提供一系列需要的動作指令和服務。從架構圖中我們可以看到，move_group類似於一個積分器，本身並不具備豐富的功能，主要做各功能包、插件的集成。它通過消息或服務的形式接收機器人上傳的點雲信息、joints的狀態消息，還有機器人的tf tree，另外還需要ROS的參數服務器提供機器人的運動學參數，這些參數會在使用setup assistant的過程中根據機器人的URDF模型文件，創建生成（SRDF和配置文件）。

一、安裝

sudo apt-get install ros-kinetic-moveit

 以下將使用《master ros for robotics programming》的案例進行學習，tutorial source code如下：

git clone https://github.com/qboticslabs/mastering_ros.git

二、Setup Assistant Tutorial

1. To start the MoveIt Setup Assistant:

roslaunch moveit_setup_assistant setup_assistant.launch

開啟「MoveIt Setup Assistant」之後，選擇「Create New MoveIt Configuration Package」建立新的Configuration Package。

這裡有兩個選擇，一個是新建一個配置功能包，一個是使用已有的配置功能包。這裡我們選擇新建配置功能包，並在程式碼中找到urdf文件(seven_dof_arm.urdf)。如果已有配置功能包，那麼新建後會覆蓋原有的文件。

2. Generate Self-Collision Matrix

然後點擊左側的第二項配置步驟，配置自碰撞矩陣，MoveIt！可以讓我們設置一定數量的隨機採樣點，根據這些點生成配裝參數，可想而知，過多的點會造成運算速度慢，過少的點會導致參數不完善，默認的採樣點數量是10000個，官方稱經過實踐，要獲得不錯的效果，10000是最低要求，我們就按照這個預設值生成碰撞矩陣。

3. Add Virtual Joints

虛擬關節主要是用來描述機器人在world坐標系下的位置，如果機器人是移動的，虛擬關節可以與移動基座關聯，不過一般的機械臂都是固定不動的，所以也可以不需要虛擬關節。

4. Add Planning Groups
Planning Group用於描述機器人的不同部分，例如定義手臂或末端夾具部分。這一步可以將機器人的多個組成部分（links，joints）集成到一個Planning Group當中。之後Motion Planning會針對個別的Planning Group完成運動規劃，在規劃的過程中還可以選擇運動學解析器（ kinematic solver）。

這裡創建兩個組，一個是機械臂部分（arm），一個是夾具部分（gripper）。

5. Add Robot Poses
Setup Assistant允許在配置中添加某些固定的姿勢（Pose），例如機器人的零點位置（Home）、初始位置等等，當然這些位置是用戶根據場景自定義的，不一定要和機器人本身的零點位置、初始位置相同。這樣做的好處就是我們使用MoveIt的API時，可以直接調用這些位置。

6. Label End Effectors
在添加了夾具的Planning Group之後，將這些群組指定為end-effectors可以在內部進行一些特殊操作。

7. Add Passive Joints
機器人上的某些關節可能在規劃控制的過程中不會使用到，可以先宣告出來。這裡沒有就不用管了。

8. Generate Configuration Files
最後一步，生成配置文件。一般會取名[robot_name]_moveit_config。這裡，會提示覆蓋已有的配置文件。

MotionPlanning

在完成配置之後即可退出 Setup Assistant，接著執行「demo.launch」看看：

$ roslaunch seven_moveit_config/launch/demo.launch

＊若是要執行Motion Planning的話，還需要再執行「move_group.launch」，導入Planning Library：

$ roslaunch seven_moveit_config/launch/move_group.launch

　　

這個介面是在rviz的基礎上加入了MoveIt Plugin，通過左下角的Plugin，我們可以選擇機械手臂的目標位置，進行運動規劃。或者拖動機械臂的前端，也可以改變機械臂的姿態。

在規劃頁面點擊「Plan and Execute」，MoveIt即會開始路徑規劃，並且可以看到機械臂開始向目標位置移動!

參考資料：
ROS MoveIt - tutorials
古月居　ROS探索总结—MoveIt基础
运动规划 (Motion Planning): MoveIt! 与 OMPL

ROS使用Dynamixel Package

●  對於Dynamixel 1.0 protocol的馬達，可以使用package [dynamixel-motor]。

1. $ sudo apt-get install ros-kinetic-dynamixel-motor
2. $ roslaunch dynamixel_tutorials contorller_manager.launch

●  但對於使 用Dynamixel 2.0 protocol的馬達，package dynamixel-motor會無法辨識。因此要改為使用dynamixel-workbench-controller

Step 1: Install ROS package

$ sudo apt-get install ros-kinetic-dynamixel-workbench-controller

Step 2: Create package

$ roscreate-pkg my_dynamixel_workbench_tutorial dynamixel_workbench_single_manager std_msgs roscpp

Step 3: Create a launch file for the single manager node

1. Make a launch file in ros package which we created

$ cd my_dynamixel_workbench_tutorial
$ mkdir launch
$ cd launch
$ vim single_manager.launch

2. Type or copy&paste code below to connect dynamixel_workbench_single manager packages and set parameters

<launch>
  <arg name="device_name" default="/dev/ttyUSB0"/>
  <arg name="baud_rate" default="115200"/>

  <param name="device_name"      value="$(arg device_name)"/>
  <param name="baud_rate"        value="$(arg baud_rate)"/>

  <node name="single_manager_tutorial" pkg="dynamixel_workbench_single_manager" type="dynamixel_workbench_single_manager" required="true" output="screen"/>
</launch>

3. Before we operating this package, we need to access permission for USB device

$ sudo chmod a+rw /dev/ttyUSB0

4. Now we can run tutorial package:

$ roslaunch my_dynamixel_workbench_tutorial single_manager.launch

Step 4: Check state of Dynamixel

Now, we can check a state of linked Dynamixel through /dynamixel_workbench_single_manager/motor_state topic:

$ rostopic echo /dynamixel_workbench_single_manager/motor_state

Step 5: Send command to Dynamixel

We operate a Dynamixel using command. Address names can be checked published message or using table command. 

 [CMD]torque_enable 1
 [CMD]goal_position 2048

#1. Run single_manager and single_manager_gui

And we run single_manager_gui:

$ rosrun dynamixel_workbench_single_manager_gui dynamixel_workbench_single_manager_gui

#2. Controlled by roscpp

github - test for ROS dynamixel_workbench_single_manager(set position)

參考資料：

ros dynamixel_workbench_single_manager/Tutorials