Gazebo筆記(六)
2019年8月7日(三)
目錄：
一、參考的網站
二、在 urdf 中，link 和 joint 的建立方法 (有 rpy )
三、在gazebo建出手掌的model
四、讓 joint 動起來 (延續上一篇跳過的部份)
一、參考的網站
(1) urdf_tutorial (上一篇有提過)
(2)
二、在 urdf 中，link 和 joint 的建立方法 (有 rpy )
(1) 使用 rviz
借用前面 urdf_tutorial 的環境，來實際測測看 link、joint、xyz、rpy，彼此間的關係。
會使用 rviz 而不直接用 gazebo 來測的原因：
因為 gazebo 會把 fixed joint 的圖示給直接忽略，要看到 joint 必須要把 type 改成 非fixed ，不太好觀察。
而在 rviz 中，每一個 fixed 或 非fixed 的 joint 都會清楚的顯示出來。
rviz、gazebo的註解：
在rviz，按著shift再按滑鼠左鍵，可以平移整個畫面。
在gazebo，按著shift再按滑鼠左鍵，可以旋轉畫面。(很好用)
(2) 使用修改後的 03-origins.urdf
對位於 /catkin_ws/src/urdf_tutorial/urdf 的 03-origins.urdf 進行修改：
在 /catkin_ws/src/urdf_tutorial 執行 roslaunch (開啟 rviz)：
roslaunch urdf_tutorial display.launch model:=urdf/03-origins.urdf結果圖：
基本上，可以把那些小座標軸當成 joint 。
那些小座標軸指明了 xyz的方向 (往上是z，剩下兩個可用右手定則判斷)。
分析程式：
(a) 固定 robot 在某個地方
<!-- for fixing the robot -->
<link name="world"/> <joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint> <link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</visual>
</link>
world 的用途是固定 robot 在某個地方。
<joint name>那部份是在宣告一個關節，並且指定前一個 link 和下一個 link。
<link name> 中，照理來說會有<visual>、<collision>、<inertial>三個東西。但這裡為求簡便只寫了最重要的 <visual>，剛好 rviz 的環境也允許我只寫<visual> (在 gazebo 會多要求 <inertial>)。
在 <link name=”base_link”>中，有<visual>，裡面有<geometry>，更裡面有<cylinder>，這裡表示它宣告了一個圓柱體，高0.6公尺，半徑0.2公尺。第一個 link 因為沒有特別註明 origin (後面其他 link 有)，所以他的圓柱中心在(0 0 0)，可以看到圓柱一半在 xy 平面上方，一半在 xy 平面下方。
(b)
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0 -0.22 0.25" rpy="0 0 0"/>
<!-- change rpy here -->
</joint> <link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.3"/>
</visual>
</link>
<joint name=”base_to_right_leg” type=”fixed”> 宣告一個名為 base_to_right_leg 的固定 joint (就是姿勢固定的意思)。
這個 joint 連接了base_link 和 right_leg。
<origin xyz>表示這個 joint 的位置和前一個 joint位置的相對座標，像是這邊的例子，前一個 joint位置在(0 0 0)，
那麼這個 joint 的位置會在 (0 -0.22 0.25)，可以對照 rviz圖看(那個小座標軸)
，如下圖。
<link name=”right_leg”>的 <visual>中，有<box size=” ”>，這宣告了一個長方體，它的 x有0.6公尺，y有0.1公尺，z有0.2公尺。
這個長方體的位置：我們可以看到
<origin rpy="0 0 0" xyz="0 0 -0.3"/>預設 rpy 都是0，可以忽略。
<origin xyz >代表著長方體的中心位置和 joint位置的相對座標。
joint 的位置在( 0 -0.22 0.25)，所以長方體的中心位置在( 0 -0.22 -0.05)，
可以觀察 rviz，發現長方體中心稍稍的低於 xy 平面。
(c)
<joint name="right_leg_to_my_leg" type="fixed">
<parent link="right_leg"/>
<child link="my_leg"/>
<origin xyz="0 -0.5 0"/>
</joint> <link name="my_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin xyz="0 0 -0.1"/>
</visual>
</link>
<joint name>宣告一個叫做 right_leg_to_my_leg 的固定 joint 。
<origin xyz>表示這個 joint 的位置和前一個 joint位置的相對座標。
<link name>宣告一個叫做 my_leg 的 link。
它是長方體。
<origin xyz >代表著長方體的中心位置和 joint位置的相對座標。
(3) 如果改變 rpy 的值
在 urdf 中，有兩個地方會有rpy的參數，一個在<joint>，一個在<link>，這邊測試看看在不同地方改rpy會有什麼效果。
(a) 更改 joint 的 rpy
這邊更改 base_to_right_leg 的 rpy。把 rpy 設定成 (0 1.57 0)。
這樣會使 joint 繞著 y 軸轉 1.57 rad (1圈是6.28 rad)。
同時可發現，後面的物件會連帶的動。
註解：y 軸方向需要自己看那些小座標軸指到哪裡
(他們會隨著轉動而變化)。
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0 -0.22 0.25" rpy="0 1.57 0"/>
<!-- change rpy here -->
</joint>原圖：
結果圖(更改完 rpy)：
仔細觀察可以發現，joint的小座標軸改變方向了：
如果把rpy改成 (0 1.57 1.57)：
這個地方很特別。我們想要它繞著z軸轉，雖然它也真的是繞z軸轉，
但它認定的z軸是 rpy 都是0的z軸。
註解：如果 rpy 三個參數都有值，那麼它會依序r p y的順序轉動
(我之前試出來的結果)
(b) 更改 link 的 rpy
先把程式碼回復到最原本的樣子。( 03-origins.urdf )
這邊更改名叫 right_leg 的 link。將 rpy 改成 (0 1.57 0)。
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57 0" xyz="0 0 -0.3"/>
</visual>
</link>這會使長方體繞著自己的中心旋轉，
且這裡我們特別指定繞 y 軸旋轉 1.57 rad。
原圖：
更改rpy後的圖：
註解：
旋轉 link 的rpy不會影響到後面的物件，只會改變自己的姿勢。
同時可以發現，那些小座標軸都沒改變。
三、在gazebo建出手掌的model
(1) 使用gazebo
這裡我是借用之前rrbot教學的環境，來進行手掌的建模。
所以可能會有一些多餘未刪的程式碼。
之前的 rrbot tutorial：
(2) 更改位於rrbot_gazebo內的 rrbot_world.lauch
位址在 /catkin_ws/src/gazebo_ros_demos/rrbot_gazebo/launch
只要更改第20行～第23行就好，這裡會指定gazebo去畫出哪一個urdf，
inorder '$(find rrbot_description)/urdf/my_hand_version3.xacro'" />註：$(find rrbot_description)會自己去尋找rrbot_description package的路徑
註：預設是/urdf/rrbot.xacro，這裡我更改為/urdf/my_hand_version3.xacro，
這樣它就會去畫出我指定的urdf了
第29行～第31行先不用管他，這個是為了解決pid error所加上去的，之後要讓手掌動時會提到。
(3) 建立 my_hand_version3.xacro
(這個檔案還沒完成，可能會不定時更新)
在/catkin_ws/src/gazebo_ros_demos/rrbot_description/urdf
建立 my_hand_version3.xacro ：(因為有點長，所以就用付網址的方式)
https://gist.github.com/pythongo1/c64868cdd316077eb80abbd9be247ab9
my_hand_version3.xacro 會引用到位於同個資料夾的 rrbot.gazebo 和 materials.xacro 。
rrbot.gazebo： (是拿教學的直接改，所以有些用不到)
https://gist.github.com/pythongo1/5474275f82006b3b4527424cbf5a033e
materials.xacro ：(教學的，沒改過。)
https://gist.github.com/pythongo1/39f66a31f6cc5974a13b9e0521f97546
(4) 分析 my_hand_version3.xacro
(a) 宣告這個檔案是 xacro檔
xmlns:xacro="http://www.ros.org/wiki/xacro"(b) 引入位於同個資料夾的 rrbot.gazebo、materials.xacro
在 rrbot.gazebo中，
有 gazebo_plugin (這個很重要，要記得加)：
註：我把<robotNamespace>的裡面設成/。 (原本是 rrbot )
(原因待補)
更改顏色的方法：
<gazebo reference="hand_base">
<material>Gazebo/Yellow</material>
</gazebo>註：顏色的英文(如 Yellow) 記得第一個字要大寫，不然它不會error，也不會改顏色
註：原本以為在 my_hand_version3.xacro中，改<link>內的<material>，就可以改顏色，但試了很久顏色都沒變化。最後才發現這個改顏色方法。
(c) 中間很多的 joint、link
就是手掌的參數，把手掌建立出來。
一開始建手掌的話，建議是把所有 joint 的type都是設成fixed，這樣比較好弄。建完固定姿勢的手掌後，再慢慢把他們改成continuous或其他非fixed型態(改成非fixed型態，其實要改不少東西或加一些東西，所以從fixed比較好)
如果所有joint都是fixed，那麼假如rrbot.gazebo和materials.xacro都有寫好，就可以直接執行
roslaunch rrbot_gazebo rrbot_world.launch來觀察手掌的樣子。
(但因為上面付的檔案，裡面的joint很多都是 non-fixed的，所以這種體驗的感覺可能會不一樣)
(d) 最底下的transmission
每一個非fixed type (如 continuous)的joint，都要有一個transmission。
transmission 和控制關節的轉動有關，使用方法一併在下面一起講。
四、讓 joint 動起來 (延續上一篇跳過的部份)
(1) 參考的網站
這個網站函蓋了大部分要做的事，不過還是有一些東西沒提到，要自己加。
(2) 記得加上 gazebo plugin
我是加在 rrbot.gazebo，可以看自己要加在哪裡，記得引用就好。
<robotNamespace>裡面塡 / 。如果填其他名字，執行controller相關指令時，會有一些 error，如：
[WARN] [1565680912.734540, 196.867000]: Controller Spawner couldn’t find the expected controller_manager ROS interface
照理來說應該有辦法解決，只是我目前還找不到QQ。
以下是我試著找解決辦法時，發現的一些頗有用的文章：
(a)
(b)
(3) Spawning Controllers (產生 控制器)
(a) 建立 joint.yaml
在某個config資料夾中(我是在 /catkin_ws/src/urdf_sim_tutorial/config，當然也可以自己建一個config資料夾)，建立 joint.yaml
joint.yaml ：
joint.yaml 會指定我們的第一個 controller。
這個 controller 會發布各個joint的state情況，到 /joint_state topic 。
(b) 建立 my_hand_control.launch
在 /catkin_ws/src/gazebo_ros_demos/rrbot_control/launch 建立
my_hand_control.launch ：
在<rosparam>中，file 填入 .yaml 檔的所在位置，ns 填入你想用的代稱。
在<node name>那邊，args 填入你所命名的代稱ns。
(c) 執行 roslaunch
執行 roslaunch，開啟 controller ，發布消息到 /joint_state：
roslaunch rrbot_control my_hand_control.launch可以執行 rostopic echo /joint_state ，來得到這個話題上的消息：
rostopic echo /joint_state
---------------------------------------------------------
header:
seq: 652
stamp:
secs: 13
nsecs: 331000000
frame_id: ''
name: []
position: []
velocity: []
effort: []大概會是像上面那樣，name以下都沒東西。name以下東西，在加了 transmission 後會出現。
(4) Transmissions
在 /catkin_ws/src/gazebo_ros_demos/rrbot_description/urdf 資料夾中，有我們的 my_hand_version3.xacro ，在這裡加入 transmission：
<!-- transmission finger5 -->
<transmission name="finger5_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="finger5_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="hand_base_to_finger5">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
</transmission>這個transmission是為了某個non-fixed joint (hand_base_to_finger5) 而寫的。
<transmission name>、<actuator name>都填入自己想填的名字即可。
<joint name>則一定要填對。
每一個 非fixed (non-fixed) joint 都要加上transmission。
這時候再一次執行(3)_(c)，會發現下方有東西了。
(5) joint control
(a) finger5_0.yaml
在 /catkin_ws/src/gazebo_ros_demos/rrbot_control/config 中(之後很多.yaml 檔都會放這裡)，建立 finger5_0.yaml 。
finger5_0.yaml ：
type: "position_controllers/JointPositionController"
joint: hand_base_to_finger5joint 一定要填對，對應到 transmission 內的 joint。
(b) 之後在 my_hand_control.launch 加入 <rosparam>
在/catkin_ws/src/gazebo_ros_demos/rrbot_control/launch，my_hand_control.launch：
多load了一個 .yaml 檔，file 填入 .yaml檔的位置。
<node name>那邊，在 args 加入新的代稱。
my_hand_control.launch 最後的樣子：
(c) 建立 gazebo_ros_control_params.yaml (教學沒有)
為了解決掉這個error ：
No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/hand_base_to_finger5
這個 error 一直指出缺少pid，最後找到了以下方法解決。
在 /catkin_ws/src/gazebo_ros_demos/rrbot_control/config
建立 gazebo_ros_control_params.yaml ：
把 joint 名稱寫上去，之後加上pid。
經過不斷的增加joint後，最後會長的樣子：
在 /catkin_ws/src/gazebo_ros_demos/rrbot_gazebo/launch 的 rrbot_world.launch中，加入：
load 這個 gazebo_ros_control_params.yaml。
rrbot_world.launch 最後的樣子：
(d) 更改 joint limit
在 my_hand_version3.xacro。
因為 type 設定成 continuous，所以可以增加以下兩個東西。
加入 axis，指定旋轉軸(會依據當時的 joint 的座標軸來判斷方向)。
加入 limit，限制力的大小，限制速度的大小。
(e)
重複(4)transmission~(5)joint control，直到把所有non-fixed joint 加完。
下一步就可以執行他們，並且讓 joint 轉動。
這裡統整一下要做的事(一直重複做的部份)：
把 joint type 改成 continuous
先加 transmission
加上各別對應 yaml
更改 my_hand_control.launch ( load )
更改 gazebo_ros_control_params.yaml (為了pid)
更改 joint limit
(6) 讓 joint 動起來
執行：
// 在一個新終端機
roslaunch rrbot_gazebo rrbot_world.launch
//在一個新終端機
roslaunch rrbot_control my_hand_control.launch
註：可用shift+左鍵，來旋轉畫面。
看一下所有 topic ：
rostopic list
可以藉由發布消息到以下 topic 來控制 joint 的動作：
/finger1_controller/command (finger1是大拇指比較特別，動動看就知道了)
/finger1_rotate_axis_controller/command
/finger2_0_controller/command (食指和掌心的關節)
/finger2_1_controller/command (食指的第一關節)
/finger3_0_controller/command (中指和掌心的關節)
/finger3_1_controller/command (中指的第一關節)
/finger4_0_controller/command (無名指和掌心的關節)
/finger4_1_controller/command (無名指的第一關節)
/finger5_0_controller/command (小拇指和掌心的關節)
/finger5_1_controller/command (小拇指的第一關節)發布消息，轉動小拇指：
rostopic pub /finger5_0_controller/command std_msgs/Float64 "data: 1"
rostopic pub /finger5_1_controller/command std_msgs/Float64 "data: 0.5"
發布消息，轉動大拇指：
rostopic pub /finger1_rotate_axis_controller/command std_msgs/Float64 "data: -1.3"
rostopic pub /finger1_controller/command std_msgs/Float64 "data: 1"
