作者-杭州电子科技大学智能机器人实验室
ROS下移动机械臂导航与Moveit!抓取仿真实现
功能实现:将六自由度机械臂与移动底盘组合,然后实现在Gazebo里面导航与机械臂抓取动作
URDF模型链接:
评论有人询问模型文件,现将链接贴上:
https://pan.baidu.com/s/1Tg7IqzRFK2D87AwNLoHnEw
提取码:372f
(一)URDF模型组合
第一步主要是将单独的移动底盘以及机械臂雷达等URDF模型进行组合,由于Gazebo具有一定的物理属性,再进行模型组合时不能直接将几个模型直接连贯起来,否则会出现底盘无法移动或者直接倾倒的现象,而是要根据各模型的惯性矩阵以及质量合理设置.这里参考师弟的修改模型,他将机械臂的惯性矩阵inertial matrix以及质量mass进行修改,改成和移动底盘同一数量级的属性,结果可以运行,模型效果图如下(模型参考胡春旭教学资料):
其中,上方为机械臂,下方为移动底盘,底盘中间放置雷达.
终端会报No p gain specified for pid这种错,可以忽略.
(二)导航配置
参考胡春旭资料即可,这里不做过多赘述.
(三)机械臂配置
1.Moveit! assistant 配置参数
对上述组合的模型进行机械臂参数配置
rosrun moveit_setup_assistant moveit_setup_assistant
1.加载模型:
2.碰撞点配置:
3.规划组设置:
4.初始位姿:
5.末端执行:
6.多余关节设置:
这里要注意,因为移动底盘的左右轮是不纳入机械臂规划范围内的,所以要将这两个关节剔除.
其他设置保持默认即可,最后会生成一个config包.
运行里面的demo.launch文件,会出现机械臂仿真界面,可自行在里面进行简单的仿真操作,如下图所示.
roslaunch arm_with_mbot_moveit_config demo.launch
2.文件修改
第一步我们只是在rviz里面进行了简单的仿真,没有真实机械臂,我们可以使用gazebo进行仿真,ros为我们提供了action接口,因此我们需要打通moveit与gazebo之间的通信,具体方法如下:
(1)创建关节轨迹文件
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:
<launch>
<rosparam file="$(find mbot_gazebo)/config/trajectory_control.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/arm" args="arm_joint_controller gripper_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
(4)修改Moveit配置的config包里的launch文件
找到Moveit配置的config包里面的launch包,然后里面有一个mrobot_moveit_controller_manager.launch.xml文件:注意,这个文件名字引人而异,务必找到与下面内容相符的文件,而且千万不要自己创建一个文件,否则没用:
将最后一行的yaml名字改成步骤(3)中的文件名.
<launch>
<!-- loads moveit_controller_manager on the parameter server which is taken as argument
if no argument is passed, moveit_simple_controller_manager will be set -->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- loads ros_controllers to the param server -->
<rosparam file="$(find arm_with_mbot_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>
(5)创建关节状态控制文件
创建一个arm_gazebo_joint_states.yaml文件:
主要目的是发布机械臂关节状态和tf变换
arm:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
创建一个arm_gazebo_states.launch文件:
<launch>
<rosparam file="$(find mbot_gazebo)/config/arm_gazebo_joint_states.yaml" command="load"/>
<node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/arm" args="joint_state_controller" />
</launch>
胡春旭这里还有个发布tf的命令,我这里并没有加是因为我的启动仿真环境文件里已经添加了,不需要重复发布.
(6)创建机械臂控制启动文件
创建一个arm_bringup_moveit.launch文件:
第一个启动是我的仿真环境
<!-- Launch Gazebo -->
<include file="$(find mbot_gazebo)/launch/mbot_laser_nav_gazebo.launch" />
<!-- ros_control arm launch file -->
<include file="$(find mbot_gazebo)/launch/arm_gazebo_states.launch" />
<!-- ros_control trajectory control dof arm launch file -->
<include file="$(find mbot_gazebo)/launch/arm_trajectory_controller.launch" />
<!-- moveit launch file -->
<include file="$(find arm_with_mbot_moveit_config)/launch/moveit_planning_execution.launch" />
moveit_planning_execution.launch文件内容:
主要是用来启动move_group
<launch>
# 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
<include file="$(find arm_with_mbot_moveit_config)/launch/move_group.launch">
<arg name="publish_monitored_planning_scene" value="true" />
</include>
# The visualization component of MoveIt!
<include file="$(find arm_with_mbot_moveit_config)/launch/moveit_rviz.launch"/>
<!-- 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="false"/>
<rosparam param="/source_list">[/arm/joint_states]</rosparam>
</node>
</launch>
(四)仿真实现
依次启动下面文件:
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,效果图如下:
至此,导航与机械臂规划完成.