使用MoveIt!配置软件包在RViz中进行机器人运动规划
MoveIt!为RViz提供了一个插件,可以建立新的规划场景(在该场景中,机器人运作、生成运动规划、添加新物体),
显示规划的输出结果,还可以直接与可视化机器人进行交互。
MoveIt!配置软件包由配置文件和启动文件组成,用于在RViz中启动运动规划,在软件包中有一个演示的启动文件,用于进一步了解该软件包的所有功能。
下面是运行演示启动文件的命令:
$ roslaunch seven_dof_arm_config demo.launch
我们将看到RViz加载了MoveIt!提供的MotionPlaning插件,如图所示:
MoveIt!-RViz插件
使用RViz运动规划插件
上图中,在屏幕的左边,我们可以看到RViz运动规划插件被加载了。在MotionPlanning窗口中有多个选项卡,例如Context、Planning等,默认的选项卡是Context。
我们看到默认的Planning Library为OMPL。这表明MoveIt!成功地加载了运动规划库,如果未加载的话,我们则无法执行运动规划。
如下图是Planning选项卡,这是一个经常使用的选项卡,在这个选项卡中可以设定起始状态(Start State)、目标状态(Goal State)、路径规划(Plan)和执行(Execute)规划的路径。
如图是MoveIt!-RViz的Planning选项卡
我们就可以在Ouery面板下指定机械臂的开始状态和目标状态,点击Plan按钮后,我们就可以规划从起始状态到目标状态的路径,
如果规划成功的话,我们就可以执行规划的路径,一般默认情况下,执行都是在伪控制器上完成的。我们可以将这些控制器更改为轨迹
控制器,以便在Gazebo或真实机器人中执行规划的轨迹。
我们可以使用机械臂夹爪的交互式标记来设置机器人末端执行器的起始位置和目标位置,我们可以拖动和旋转标记的姿态,并且如果
规划有解,我们将看到一个橙色的机械臂,在某些情况下,即使末端执行器的标记姿态移动了,机械臂也不移动,如果机械臂没有到
达标记位置,我们就可以假设该姿态中没有IK解,我们可能需要更多的自由度来到达哪里或者连杆之间可能存在一些碰撞。
我们也可以在起始状态和目标状态中使用random valid(随机有效)选项进行快速的运动规划,如果我们选择目标状态为random valid(即随机有效状态)
并按下Update按钮,那么将会生成随机有效的目标姿态,点击Plan按钮后,我们可以观察到运动规划的过程。
我们使用MoveIt!Planning插件中的多个选项自定义RViz的显示结果,如图所示:
第一个标记的区域是Scene Robot,它将显示机器人的模型:如果未选中,我们就看不到任何机器人模型。
第二个标记的区域是Trajectory Topic,RViz在这里获取可视化的轨迹,如果我们想要为运动规划设置动画并想要显示运动轨迹,那么我们就应该启用此选项。
在上图中,我们可以看到Query Start State和Query Goal State选项,这些选项可以显示机械臂的起始状态和目标姿态,
就如我们在图上看到的那样。Show Workspace将机器人周围的方形工作空间(所处世界的几何空间)可视化,可视化
可以帮助我们调试运动规划算法,并详细了解机器人运动行为的细节。
MoveIt!配置软件包与Gazebo的接口我们已经使用Gazebo仿真了机械臂及其控制器,如我们在MoveIt!架构中提到的那样,为了将MoveIt!中的机械臂连接到Gazebo,
我们还需要一个具有FollowJointTrajectory-Action接口的轨迹控制器。
下面是将MoveIt!连接到Gazebo的过程:
第一步:为MoveIt!编写控制器配置文件第一步是创建一个配置文件,用来与Gazebo中来自MoveIt!的轨迹控制器进行通信。
我们需要在seven_dof_arm_config软件包的config文件中创建名为controllers.yaml的控制器配置文件。
下面是controllers.yaml定义的一个示例:
1 controller_manager_ns: controller_manager 2 controller_list: 3 - name: seven_dof_arm/seven_dof_arm_joint_controller 4 action_ns: follow_joint_trajectory 5 type: FollowJointTrajectory 6 default: true 7 joints: 8 - shoulder_pan_joint 9 - shoulder_pitch_joint 10 - elbow_roll_joint 11 - elbow_pitch_joint 12 - wrist_roll_joint 13 - wrist_pitch_joint 14 - gripper_roll_joint 15 - name: seven_dof_arm/gripper_controller 16 action_ns: follow_joint_trajectory 17 type: FollowJointTrajectory 18 default: true 19 joints: 20 - finger_joint1 21 - finger_joint2
控制器配置文件包含两个控制器接口的定义,一个用于机械臂,另一个用于夹爪。控制器使用的动作类型为FollowJointTrajectory,
动作命名空间为follow_joint_trajectory。我们必须列出每组下的关节。default:ture表明它将使用默认控制器,它是MoveIt!中用来与
一组关节进行通信的主要控制器。
第二步:创建控制器启动文件接下来我们必须创建一个名为seven_dof_arm_moveit_controller_manager.launch的新启动文件,
它可以启动轨迹控制器,该文件名称是以机器人的名称开头,后面跟上_moveit_controller_manager结尾。
下面是启动文件:
1 <launch> 2 <!-- Set the param that trajectory_execution_manager needs to find the controll er plugin --> 3 <arg name="moveit_controller_manager" default="moveit_simple_controller_manager /MoveItSimpleControllerManager" /> 4 <param name="moveit_controller_manager" value="$(arg moveit_controller_manager) "/> 5 6 <!-- load controller_list --> 7 <arg name="use_controller_manager" default="true" /> 8 <param name="use_controller_manager" value="$(arg use_controller_manager)" /> 9 10 <!-- Load joint controller configurations from YAML file to parameter server -- > 11 <rosparam file="$(find seven_dof_arm_config)/config/controllers.yaml"/> 12 </launch>
该启动文件启动了MoveItSimpleControllerManager并加载了controller.yaml中定义的关节轨迹控制器。
第三步:为Gazebo创建控制器配置文件
创建MoveIt!文件后,我们必须创建Gazebo控制器配置文件和启动文件。
创建一个名为trajectory_control.yaml的新文件,该文件包含了需要与Gazebo一起加载的GazeboROS控制器列表。
我是从seven_dof_arm_gazebo软件包的/config文件夹下获取此文件。
以下是该文件的定义:
1 seven_dof_arm: 2 seven_dof_arm_joint_controller: 3 type: "position_controllers/JointTrajectoryController" 4 joints: 5 - shoulder_pan_joint 6 - shoulder_pitch_joint 7 - elbow_roll_joint 8 - elbow_pitch_joint 9 - wrist_roll_joint 10 - wrist_pitch_joint 11 - gripper_roll_joint 12 13 gains: 14 shoulder_pan_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 15 shoulder_pitch_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 16 elbow_roll_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 17 elbow_pitch_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 18 wrist_roll_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 19 wrist_pitch_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 20 gripper_roll_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 21 22 23 gripper_controller: 24 type: "position_controllers/JointTrajectoryController" 25 joints: 26 - finger_joint1 27 - finger_joint2 28 gains: 29 finger_joint1: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0} 30 finger_joint2: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}
在这里,我们创建了position_controllers/JointTrajectoryController,它具有跟arm和gripper通信的FollowJointTrajectory动作接口,
我们还定义了与每个关节相关的PID增益,这样就可以提供一个平滑的运动。
第四步:为Gazebo轨迹控制器创建启动文件
创建配置文件后,我们可以与Gazebo一起加载控制器,为此我们必须创建一个启动文件,这样就可以用单个命令同时启动Gazebo、轨迹控制器和MoveIt!的接口。
启动文件seven_dof_arm_bringun_moveIt.launch中包含了启动所有这些命令的定义:
1 <?xml version="1.0" ?> 2 3 <launch> 4 <!-- Launch Gazebo --> 5 <include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_world.launch" /> 6 7 <!-- ros_control seven dof arm launch file --> 8 <include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_gazebo_states.la unch" /> 9 10 <!-- ros_control trajectory control dof arm launch file --> 11 <include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_trajectory_contr oller.launch" /> 12 13 <!-- moveit launch file --> 14 <include file="$(find seven_dof_arm_config)/launch/moveit_planning_execution.laun ch" /> 15 16 <!-- publish joint states --> 17 <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_stat e_publisher"> 18 <param name="/use_gui" value="false"/> 19 <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</ rosparam> 20 </node> 21 </launch>
该启动文件在Gazebo中生成机器人模型,发布关节状态,连接位置控制器,连接轨迹控制器;
最后在MoveIt!软件包内启动moveit_planning_exection.launch文件,这样与RViz一起启动MoveIt!节点,
如果MotionPlanning插件没有默认加载的话,我们要在RViz中加载一下。
注意,在正常启动规划场景前,我们需要使用下面的命令来安装MoveIt!所需的一些软件包,我们就可以正常使用ROS控制器:
$ roslaunch apt install ros-melodic-joint-state-controller ros-melodic-position-controllers ros-melodic-joint-trajectory-controller
我们可以在RViz中启动运动规划,然后使用下面的命令在Gazebo中执行规划的路径:
$ roslaunch seven_dof_arm_gazebo seven_dof_arm_bringup_moveit.launch
我们就可以启动规划场景了,这将启动和Gazebo,我们可以在RViz中进行运动规划,运动规划后,
点击 Execute按钮将轨迹发送给Gazebo控制器。
第五步:调试Gaze-MoveIt!接口我们将讨论Gazebo-MoveIt!接口中的一些常见问题和调试技术。
如果在Gazebo中轨迹没有执行,首先列出所有的话题:
$ rostopic list