至於為什么叫框架， 一是因為靈巧手的3維模型沒有按照基本的設計要求畫，正常來說，設計機器人機構之前應該設計好機構需要多少個自由度/DOF，每個自由度是旋轉/revolute類型還是滑移/prismatic類型，各個關節的自由度限位/limits，各個連桿的長度/a，每個自由度之間的扭角/α，偏移/d，整個機器人的工作空間/workspace等等； 二是當前搭建的框架能實現的function和控制很基礎，靈巧手其實是一個多機器人協同的系統，需要在MoveIt!的代碼上進行更改，至少抓取矩陣這種概念在MoveIt!上是沒有的，還有很多地方需要修改。
總的來說，當前的框架不夠deep，但基本的模樣已經有了。
該博客同時將幫助更加順利的完成以下工作，希望能對各位有所幫助，源碼鏈接見文末：
-
單純的通過ROS控制Gazebo中的機器人模型
-
通過MoveIt!直接控制現實中的機器人
-
MoveIt!與Gazebo聯合仿真
博客目錄：
-
使用Solidworks設計靈巧手，通過sw2urdf插件生成ROS包
-
機器人MoveIt!配置文件生成
-
URDF適配Gazebo的簡單修改
-
ROS節點控制Gazebo中機器人模型
-
Moveit!控制現實中的機器人
-
MoveIt!與Gazebo聯合仿真
Ⅰ. 使用Solidworks設計靈巧手，通過sw2urdf插件生成ROS包
用Solidwork設計，一是這樣相對於自己搭URDF更專業，二是有插件支持，方便生成URDF。
幾個需要注意的點：
-
每個link都單獨放入總裝配體中，最好不要用子裝配體
-
設計好裝配體后，使用sw2urdf插件定義各個關節連桿的從屬關系，在這之前，使用D-H方法自己畫出各個軸和坐標系，不要自動生成，這樣生成的URDF99%會錯位。另外旋轉軸的類型對於機械臂這種機器人一般選擇revolute，移動的小車這種機器人就選擇continous，以上兩者的區別在於如果選擇前者會在URDF文件中定義關節限位，固定的關節選擇fixed。
-
如果你的機器人底座是固定的，那么你最好把base_link整體z坐標大於0，不然到時放到Gazebo的時候會與地面干涉，造成不必要的麻煩。
Ⅱ. 機器人MoveIt!配置文件生成
現在從Windows轉到Ubuntu，將前面生成的URDF包放在工作空間， 前提是已經裝好了MoveIt!，建議使用源碼安裝，方便閱讀和修改源碼．
打開MoveIt_Setup_assitants，參照官方文檔設置
roslaunch moveit_setup_assistant setup_assistant.launch
加載URDF文件，按照順序一一設置即可，后面要修改也再次可以加載ＭoveIt!配置文件修改．其中有幾個要注意的地方：
-
干涉檢測那一部分自動生成后轉到矩陣表示，檢查是否符合自己的要求
-
virtual_joint的目的一般是為了將base_link和world坐標系相固連，在后面修改URDF的部分中為適配Ｇazebo已經添加了base_link和world的虛擬關節，所以在這里不再需要添加，各位自行決定．
-
planning_group按照每個手指分組．
-
Robot_Pose這一部分可以檢測你的機器人關節的限位，如果動不了，一般是URDF中關節硬限位和軟限位沒有設置好．
-
ROS_Control這塊根據具體的控制器類型選擇，先按組自動添加，然后再修改控制器類型，如果和Gazebo聯合仿真的話，選擇position_controllers/JointTrajectoryController這一類型，原因是由MoveIt!和仿真環境或實際機器人的通信方式所決定，具體可在MoveIt!官網介紹．當然以上兩者聯合仿真的話也不一定就要position_controllers/JointTrajectoryController這種控制器，你也可以選擇其它控制器，然后將MoveIt!的client發出的action消息寫個Server端進行解析，然后用過ROS_Control進行Gazebo控制．
move_group talks to the controllers on the robot using the FollowJointTrajectoryAction interface. This is a ROS action interface. A server on the robot needs to service this action - this server is not provided by move_group itself. move_group will only instantiate a client to talk to this controller action server on your robot.
-
Simulation這部分生成適配Gazebo的URDF文件，在原來基礎上添加了傳動/Transmission和連桿慣量以及gazebo_ros_control插件，這樣才能真實的模擬真實物理環境，把這個URDF文件作為后面用的機器人描述文件．
Ⅲ. URDF適配Gazebo的簡單修改
-
添加一個virtual_link用以連接base_link和world，類型為固定．
-
設置好關節的軟硬限位和旋轉方向．
-
檢查慣性等屬性是否正確，gazebo_ros_control可以設置你的機器人的命名空間，注意這個命名需要和之后的controller.yaml文件和MoveIt!配置生成的ros_control.launch中的命名空間一致，不然到時候節點會找不到另一個命名空間的控制器．
Ⅳ. ROS節點控制Gazebo中機器人模型
要實現ROS節點控制Gazebo中機器人模型只需要運行gazebo.launch和一個自定義的關節角發布節點．
MoveIt!配置生成的ROBOT_moveit_config包中含有諸多launch文件和config文件，建議都看看以了解其原理．
gazebo.launch負責加載Gazebo中的空世界模型和機器人URDF模型，以及controller_manager，運行該文件會加載/config/ros_controllers.yaml中的參數到ROS的參數服務器，ros_controllers.yaml是較為關鍵的一環，格式不對會導致運行出錯．比如我之前配置MoveIt!的時候，如果選擇控制器類型為position_controllers/JointPositionController，可以這樣寫（部分代碼）:
mutifingerhand: joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 J11_controller: type: position_controllers/JointPositionController joint: J11 pid: {p: 100.0, i: 0.01, d: 1} J12_controller: type: position_controllers/JointPositionController joint: J12 pid: {p: 100.0, i: 0.01, d: 1}
以上的控制器名就是之前在MoveIt!中配置的名字，也和ros_control.launch中的控制器名一致，注意我這里的命名空間是mutifingerhand，所以在ros_control.launch和URDF中的命名空間也當是如此．
接下來就是寫一個發布各關節控制的消息發布節點，根據Gazebo的官方示例，消息類型為std_msgs::Float64，話題名為/mutifingerhand/J11_controller/command，參照這樣的格式寫即可．
Ⅴ. Moveit!控制現實中的機器人
要實現ROS節點控制控制現實中的機器人只需要運行demo.launch和一個自定義的基於moveit的規划節點．
demo.launch加載機器人RVIZ配置文件和move_group等節點，其中在下面代碼中有個fake_execution決定了控制實際機器人或者仿真，還是直接進行fake_control，在這里把它改為false.
<include file="$(find mutifingerhand_moveit_config)/launch/move_group.launch"> <arg name="allow_trajectory_execution" value="true"/> <arg name="fake_execution" value="true"/> <arg name="info" value="true"/> <arg name="debug" value="$(arg debug)"/> <arg name="pipeline" value="$(arg pipeline)"/> </include>
另外，上文中講到了MoveIt!通訊的機制，MoveIt!作為client需要一個機器人端的Server與其對應，同時ros_controllers.yaml控制器類型需要修改成position_controllers/JointTrajectoryController，注意這些都要寫在controller_list下，具體結構參考下面的代碼：
joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 50 controller_list: - name: finger1_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true joints: - J11 - J12 - J13 - J14 - J15
和前面Gazebo端的ros_controller.yaml有點類似，但是不同．大部分博文中這兩個yaml文件被分成了兩個，但其實放在同一個文件里也可以，各取所需，反正都是要加載到ROS參數服務器的．
Server端如何寫，我這里有個參考，完整代碼移步文末的Github鏈接．
#include <ros/ros.h> #include "actionlib/server/action_server.h" #include "actionlib/server/server_goal_handle.h" #include <control_msgs/FollowJointTrajectoryAction.h> class ControllerServerAction { protected: ros::NodeHandle nh_; actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> action_server; actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_; control_msgs::FollowJointTrajectoryResult result_; public: ControllerServerAction(std::string name) : action_server(nh_, name, boost::bind(&ControllerServerAction::pubGoalJoints, this, _1), false) { action_server.start();
至於自定義的基於moveit的規划節點，參照MoveIt!官方教程寫就行，記得加上代碼：
// Moving to a pose goal /* Uncomment below line when working with a real robot */
move_group.move();
這是讓機器人發送規划的軌跡的關鍵一步．
Ⅵ. MoveIt!與Gazebo聯合仿真
關鍵在於ros_controller.yaml的配置，Gazebo端和MoveIt!端的controller.yaml文件可以放在一個文件，兩者的格式分別就是上文中第四第五節中講到的格式，運行demo_gazebo.launch文件同時加載Gazebo和MoveIt!，不需要寫Server了，Gazebo這端默認作為了Server端．要控制的話寫一個上文中第五節一樣的節點就行．
控制器類型得是position_controllers/JointTrajectoryController．
如圖我這里是控制了第四根手指彎曲，把PID參數調到機器人沒有抖動，先調P，然后調ID參數，I負責消除穩態累計誤差，D負責噪聲影響．
Github項目鏈接
如有問題可在評論區留言，或者私信，或者本人主頁咨詢或者Github提出，歡迎交流探討．
不過這種東西也就是工具，精髓在於MoveIt!的源碼，所以是建議大家源碼安裝．