ROS下移动机械臂导航与Ｍoveit!抓取仿真实现

作者－杭州电子科技大学智能机器人实验室

功能实现：将六自由度机械臂与移动底盘组合，然后实现在Gazebo里面导航与机械臂抓取动作

URDF模型链接：

评论有人询问模型文件，现将链接贴上：

https://pan.baidu.com/s/1Tg7IqzRFK2D87AwNLoHnEw 
提取码：372f 

（一）URDF模型组合

第一步主要是将单独的移动底盘以及机械臂雷达等URDF模型进行组合，由于Gazebo具有一定的物理属性，再进行模型组合时不能直接将几个模型直接连贯起来，否则会出现底盘无法移动或者直接倾倒的现象，而是要根据各模型的惯性矩阵以及质量合理设置．这里参考师弟的修改模型，他将机械臂的惯性矩阵inertial matrix以及质量mass进行修改，改成和移动底盘同一数量级的属性，结果可以运行，模型效果图如下（模型参考胡春旭教学资料）：

其中，上方为机械臂，下方为移动底盘，底盘中间放置雷达．

终端会报No p gain specified for pid这种错，可以忽略．

（二）导航配置

参考胡春旭资料即可，这里不做过多赘述．

（三）机械臂配置

１．Moveit! assistant 配置参数

对上述组合的模型进行机械臂参数配置

rosrun moveit_setup_assistant moveit_setup_assistant

１．加载模型：

２．碰撞点配置：

３．规划组设置：

４．初始位姿：

５．末端执行：

６．多余关节设置：

这里要注意，因为移动底盘的左右轮是不纳入机械臂规划范围内的，所以要将这两个关节剔除．
其他设置保持默认即可，最后会生成一个config包．

运行里面的demo.launch文件，会出现机械臂仿真界面，可自行在里面进行简单的仿真操作,如下图所示．

roslaunch arm_with_mbot_moveit_config demo.launch

２．文件修改

第一步我们只是在rviz里面进行了简单的仿真，没有真实机械臂，我们可以使用gazebo进行仿真，ros为我们提供了action接口，因此我们需要打通moveit与gazebo之间的通信，具体方法如下：

（１）创建关节轨迹文件

moveit规划好轨迹之后会输出一个＂FollowJointTrajectory＂的action，里面包含轨迹点，需要将这些信息转入gazebo中的joint位置,所以需要创建一个关节控制文件trajectory_control.yaml文件:

arm:
  arm_joint_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6

    gains:
      joint1:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint2:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint3:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint4:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint5:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint6:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}


  gripper_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - finger_joint1
    gains:
      finger_joint1:  {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}

（2）创建关节轨迹加载文件

arm_trajectory_controller.launch:

（3）创建controller_gazebo文件

在moveit配置好的config包下面中的创建controllers_gazebo.yaml文件:添加控制器命名空间，以保持gazebo中controller发布的action与moveit那边对接．

controller_manager_ns: controller_manager
controller_list:
  - name: arm/arm_joint_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6

  - name: arm/gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_joint1
      - finger_joint2

（４）修改Moveit配置的config包里的launch文件

找到Moveit配置的config包里面的launch包，然后里面有一个mrobot_moveit_controller_manager.launch.xml文件：注意，这个文件名字引人而异，务必找到与下面内容相符的文件，而且千万不要自己创建一个文件，否则没用：
将最后一行的yaml名字改成步骤（３）中的文件名．

（５）创建关节状态控制文件

创建一个arm_gazebo_joint_states.yaml文件：
主要目的是发布机械臂关节状态和tf变换

arm:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

创建一个arm_gazebo_states.launch文件：

胡春旭这里还有个发布tf的命令，我这里并没有加是因为我的启动仿真环境文件里已经添加了，不需要重复发布．

（６）创建机械臂控制启动文件

创建一个arm_bringup_moveit.launch文件：
第一个启动是我的仿真环境

moveit_planning_execution.launch文件内容：
主要是用来启动move_group

 # The planning and execution components of MoveIt! configured to 
 # publish the current configuration of the robot (simulated or real)
 # and the current state of the world as seen by the planner
 
  
 
 # The visualization component of MoveIt!
 

  
  
     
    [/arm/joint_states]
  

（四）仿真实现

依次启动下面文件：

source devel/setup.bash
roslaunch mbot_gazebo arm_bringup_moveit.launch
roslaunch mbot_navigation nav_cloister_demo.launch＃导航启动

仿真图：

代码实现导航之后机械臂抓取操作：
创建一个move_test.py程序：

#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import roslib;
import rospy  
import actionlib  
import os
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  

rospy.init_node('move_test', anonymous=True)  
   
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  

rospy.loginfo("Waiting for move_base action server...")  

while move_base.wait_for_server(rospy.Duration(5.0)) == 0:
    rospy.loginfo("Connected to move base server")  

target = Pose(Point(0.543, 0.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  
goal = MoveBaseGoal()  
goal.target_pose.pose = target  
goal.target_pose.header.frame_id = 'map'  
goal.target_pose.header.stamp = rospy.Time.now()  

rospy.loginfo("Going to: " + str(target))  

move_base.send_goal(goal)  

finished_within_time = move_base.wait_for_result(rospy.Duration(300))   

if not finished_within_time:  
    move_base.cancel_goal()  
    rospy.loginfo("Timed out achieving goal")  
else:  
    state = move_base.get_state()  
    if state == GoalStatus.SUCCEEDED:  
        rospy.loginfo("Goal succeeded!")
        os.system('rosrun mbot_gazebo moveit_ik.py')
    else:  
      rospy.loginfo("Goal failed！ ")  

该程序执行成功后会自动执行另一个机械臂正运动学规划程序moveit_ik.py,效果图如下：

至此，导航与机械臂规划完成．