使用moveit!控制真實機械臂(3)——修改moveit配置文件來控制真實機械臂(9月27日更新)

要想使用moveit來控制真實機械臂,我們需要修改配置文件夾下的幾個文件,因爲默認生成的moveit配置文件中,所使用的部分參數是針對虛擬機械臂的,你可以在rviz環境下觀察模型的運動,但真正的控制信號並不會發出來。具體要修改以下幾個地方:
##1、demo.launch文件中參數fake_execution的值改爲false

<!--此段代碼來自moveit配置文件demo.launch-->
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find aubo_i5_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="false"/> //請看這個參數
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

通過參數的名字也很好理解,就是啓用真實機械臂執行方式。接下來,demo.launch文件會啓動move_group.launch文件,這是下一個要修改的文件。

##2、修改moveit_controller_manager參數。
move_group.launch文件中,moveit_controller_manager在選擇參數值時,“unless”前面那個velue值要修改,寫一個自己機器人名稱作爲前綴

<!-- Trajectory Execution Functionality -->
  <include ns="move_group" file="$(find aubo_i5_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="aubo_i5" unless="$(arg fake_execution)"/>  //請看這裏
    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
  </include>

這段代碼要看懂,moveit_controller_manager的參數是有選擇的,要麼等於“aubo_i5”(我使用的名稱),要麼等於“fake”,這要取決於後面的參數fake_execution,而這個參數我們上一步已經改爲了false,即當前moveit_controller_manager應該等於“aubo_i5”,接下來,這個參數會傳遞給trajectory_execution.launch.xml文件,該文件中有下面這句話

<include file="$(find aubo_i5_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />

按照原先的設置,本應該啓動fake_moveit_controller_manager.launch.xml,你可以去moveit配置文件夾下去找,這個文件是存在的,而現在,一個叫做aubo_i5_moveit_controller_manager.launch.xml將會被啓動,而這個文件moveit也應該已經幫你創建好了,當你使用先前的moveit嚮導加載機器人模型時,機器人模型中寫明的機器人名稱(name屬性),就會作爲前綴寫入這個文件的文件名,所以,“aubo_i5”這個名稱源自於你模型文件裏寫明的機器人名稱,請前後保持統一,理解各個文件之間的調用關係。
##3、修改aubo_i5_moveit_controller_manager.launch.xml文件

雙擊打開這個文件,發現裏面只寫了一對launch標籤,基本算是一個空文件,說明moveit早就幫你留好了位置,等着你來填充,填什麼呢?我們可以參考moveit官網中關於controllers Configuration Tutorial這一部分,裏面清楚的給出了一份填寫建議:

<launch>
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- load controller_list -->
<rosparam file="$(find my_robot_name_moveit_config)/config/controllers.yaml"/>
</launch>

請將上面這段代碼複製到你的對應文件中,將my_robot_name_moveit_config修改爲你自己的moveit配置文件目錄名稱。

##4、創建controllers.yaml配置文件
上一步的代碼最後一句指向了config目錄下一個叫做controllers.yaml的文件,這個文件很重要,決定了你所使用的moveit控制器的基本參數。我們打開配置文件夾中的config目錄,發現只有fake_controllers.yaml,所以,現在要做的就是複製一份這個文件,然後將名字改爲controllers.yaml,打開這個新文件,將文件修改成如下形式:

controller_list:
  - name: "aubo_i5"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - shoulder_joint
      - upperArm_joint
      - foreArm_joint
      - wrist1_joint
      - wrist2_joint
      - wrist3_joint

你的文件很可能寫的不是這樣,但理解參數含義就好

**name:**這裏你可以寫一個與你機器人相關的名稱,方便你使用

action_ns:follow_joint_trajectory ,follow_joint_trajectory 是後續action的名稱的一部分,有關action的內容在下一章裏,將來你需要填一個action的名稱,這個名稱就在這個配置文件裏確定了,就是name/action_ns所代表的對應字符串的組合。

type: FollowJointTrajectory ,這個類型是ros下的自帶action類型,是一種控制機械臂運動軌跡的數據結構,請原樣填寫,將來你可能還想控制手抓之類的結構,所填寫的類型都是不一樣的。

**joints:**這裏是你機器人(機械臂)的關節名稱,這些名稱源自你的機器人模型文件,我們的controllers.yaml文件複製於fake_controllers.yaml文件,這部分應該是自動生成的。

到此爲止,moveit配置文件中所有應該修改的文件就修改完了,moveit已經準備好了將規劃的軌跡以action的形式發送給真實機械臂,後面的內容將是如何利用好action這裏的信息,實現真正的機械臂控制。


以下內容爲9月27日更新內容


1、解決真實機械臂與joint_states_publisher消息衝突的問題
當控制真實機械臂時,moveit的配置文件中,demo.launch文件中關於joint_states_publisher的一段代碼需要註釋掉,因爲該程序也在發佈機械臂關節狀態,而且是一個模擬的狀態,與真實機械臂狀態是不相符的,若不註釋掉該代碼,rviz下顯示的機械臂狀態總是在不停的跳變(衝突的結果),因此,控制真實機械臂時記得註釋掉這一部分。

<!-- We do not have a robot connected, so publish fake joint states -->
  <!--node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="$(arg use_gui)"/>
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node-->

2、解決真實機械臂軌跡執行時間超時的問題。
moveit配置文件中有幾個參數還需要添加上,否則後續真實機械臂執行軌跡時會報超時的錯誤。
在這裏插入圖片描述
需要在move_group.launch文件中增加幾個參數,來延長允許執行軌跡的時間。
找到<node name="move_group"> 所對應的標籤組,在其中加入以下參數

<param name="trajectory_execution/allowed_execution_duration_scaling" value="6"/>      //允許軌跡執行時間的一個放大倍數,可以根據實際情況自行修改
<param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/>            //超時的一個百分比範圍

或者加入下面的參數,直接關掉監視軌跡執行狀況的一個monitoring

<param name="trajectory_execution/execution_duration_monitoring" value="false"/>

修改完後,今後機械臂運到過程中,命令行就不會出現超時的錯誤了。
在這裏插入圖片描述

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章