前言
又要重新用ROS了,強迫症發作+在家無聊,就順便把以前遺留的這個問題也解決下。
問題描述
在使用UR包的時候,會有這個問題:
ros.rosconsole_bridge.console_bridge: Skipping virtual joint ‘fixed_base’ because its child frame ‘base_link’ does not match the URDF frame ‘world’
在github上面好像也很多年了,到現在也沒有解決,,還看的頭暈。
例如:
https://github.com/ros-industrial/universal_robot/pull/284
https://github.com/ros-industrial/universal_robot/issues/347
沒時間的話,直接看解決方法4即可。方法1-3是我的試錯記錄。
瞎猜的可能原因(事實證明,真的是瞎猜,別看這部分了)
至於原因,理一下virtual joint
的作用及URDF
與相應的SRDF
,就會有大概想法了。
- 在
Moveit setup assistant
中,virtual joint
的parent frame
會選擇world
。 - 在
默認UR包
中,查看SRDF
,會發現parent frame
也是world
,child link
是base_link
。 - 但是在
ur5_robot.urdf.xacro
中,由如下片段:
<link name="world" />
<joint name="world_joint" type="fixed">
<parent link="world" />
<child link = "base_link" />
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>
- 這就導致一個問題,兩個world會不會起衝突?
(瞎猜的,我主要還是看這裏的link命名成world不爽)
解決方法1
上面問題284裏面給了一種方法,就是刪去ur5_robot.urdf.xacro
裏面的片段,再修改一下相應的幾個launch文件
。
我在他的基礎上,還修改了gazebo
中對應的模型,然後出了兩個問題。一個是,gazebo
中機器人底座不固定了(這個問題會在方法三中解釋);另一個是提示kdl求解器出問題,需要添加一個虛擬杆件或者要取消base link
上的慣性標籤。
但是如果是需要取消慣性標籤,宏命令這裏要怎麼處理,感覺很麻煩。。
(剛剛注意到這個答主修改了回答,已經跟方法二一樣了 )
注意到,如果直接使用Moveit setup assistant
,就會覆寫一些文件。
爲了避免可能的問題,因此推薦 方法三,也即答主的方法。
解決方法2
反正看把link
命名成world
不爽,就改這個好了。改這個成功的話,也不用修改launch文件
。
把ur5_robot.urdf.xacro
(在ur_description/urdf
)中的片段改成:
<link name="world_v" />
<joint name="world_v_joint" type="fixed">
<parent link="world_v" />
<child link = "base_link" />
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>
重新用Moveit setup assistant
,修改virtual joint
,parent_frame
爲world
, child_link
爲world_v
即可。
(我目前還不知道這樣修改後生成的文件,是否會影響默認UR包內的其他文件。我是另外創建空白工作空間進行實驗的。好像是Moveit setup assistant
直接生產的配置文件在使用時會有些問題)
最後,打開rviz
查看機器人,可以看到Fixed Frame
也是修改後的link
名。非常明瞭。
(我忘記查看原來情況下的Fixed Frame
名了)
(默認UR包
裏的,Fixed Frame
是 world
)
roslaunch ur5_moveit_config demo.launch
(說明:我沒有測試過方法二在gazebo中的表現情況)
解決方法3
修改urX_robot.urdf.xacro
及相關SRDF
。
其中,urX_robot.urdf.xacro
,按方法二修改。
urX.srdf
在urX_moveit_config/config
,中的child_link
改爲world_v
<virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="world_v" />
但是,這就造成一個問題,機器人底座不固定。參考鏈接, 可以看到本應固定的底座在不斷移動。
查閱原因,URDF中就是需要一個名爲world
的link
來固定機器人底座。因此,誕生方法四。
If you would like your URDF model to be permanently attached to the world frame (the ground plane), you must create a “world” link and a joint that fixes it to the base of your model. RRBot accomplishes this with the following:
解決方法4(推薦,無後遺症)
保持urX_robot.urdf.xacro
不變,僅修改SRDF
爲中的virtual_joint
下的child_link
:
<virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="world" />
經過之前不斷測試,發現child_link
可以選擇urX_robot.urdf.xacro
下的各個link
…
然後就終於解決了這個問題。
另,需要說明的事,在gazebo中,使用的empty_world,當機械臂構型與Z=0的地面發生碰撞時,就執行報錯了(不斷抖動)。
[ WARN] [1585016493.340250154, 72.187000000]: Controller failed with error code PATH_TOLERANCE_VIOLATED
[ WARN] [1585016493.340652594, 72.188000000]: Controller handle reports status ABORTED
[ INFO] [1585016493.340796763, 72.188000000]: Completed trajectory execution with status ABORTED …
[ INFO] [1585016493.489273731, 72.272000000]: ABORTED: Solution found but controller failed during execution
我的現在的解決方法是,修改urX_robot.urdf.xacro
升高機器人位置。
# 題外話
之所以想要解決這個問題,還有個原因是,以前給UR配置ikfast一直沒成功。杆件裏面有個world的名稱實在是膈應,感覺都是它的鍋。
現在配置IKFAST,就沒有world這個名稱了。當然也配置成功了。