Moveit!入门——古月居机械臂开发笔记(一)
- 引言
- Moveit!与机械臂控制
- 1、创作机械臂模型
- 2、生成配置文件
- 3、如何使用Moveit!实现机械臂仿真(gazebo)
- 完善模型
- 在gazebo中加载机械臂模型
- ros_control
- 配置并加载控制器
- 最终使用Moveit!
- 4、通过Moveit!控制机械臂运动(用户接口:C++、python)
- 编程模式
- 几种空间运动
- 轨迹约束
- 轨迹修改
- 后记
引言
在上一篇完成场景搭建后觉得自己对Moveit!还不是很熟悉,找了b站上古月居的机械臂开发原理的视频进行学习。教程分为三部分:
- Moveit!和机械臂控制
- ROS机械臂开发_机器视觉与物体抓取
- ROS机械臂开发_机械臂开发实例
Moveit!与机械臂控制
1、创作机械臂模型
首先是写自己的urdf机械臂模型文件,主要包括<-link>连杆和连接两个连杆的<-joint>关节(有6中连接类型)。通常还是xacro格式,因为xacro中可以有一些编程语句,变量运算等。如果用ur等机械臂的话就可以使用其现成的模型文件。
有了模型文件后,可以通过rviz来查看该模型——将.xacro文件加载到robot_description的变量中(因为rviz中RobotModel插件订阅了robot_description),启动joint_state_publisher和robot_state_publisher节点(ROS提供)。
<launch>
<arg name="gui" default="true" />
<arg name="robot_desciption" default="$(find xacro)/xacro --inoder '$(find ur5_single_arm_turfs)/urdf/ur5_single_arm.urdf.xacro'"/>
<arg name="rvizconfig" default="$(find ur5_single_arm_turfs)/launch/ur5_single_arm_rviz.rviz" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
joint_state_publisher:关节状态发布器。如六轴机械臂,每一个轴有自己的一个位置角度,这六个角度就是关节状态。即将关节状态通过Topic发布出来。
robot_state_publisher:订阅发布的关节状态Topic,将六个角度通过TF发布出来。可以理解为正向运动学(已知六个轴的角度参数,求六个轴的空间位置)
其中rvizconfig中.rviz文件是rviz中保存的参数文件,可在第一次设置机械臂各种参数(如选择fix_frame、添加RobotModel插件等)
2、生成配置文件
同样,ur等机械臂已经完成了配置,在moveit_config文件夹中。如果是自己做的机械臂,通过以下步骤:
roslaunch moveit_setup_assistant setup_assistant.launch
主要步骤:
- 选择模型文件
- 自碰撞检测
- 虚拟关节,机械臂与世界坐标系的关系,主要用于移动机器人
- 设置机械臂规划组(PlanningGroups)。通常为两个部分:六轴、夹爪。还需要设置运动学求解器(Moveit默认的KDL,可以更换为效果更好的)
- 预定义机械臂位姿,便于编写程序直接调用
- 终端夹具配置,名称、坐标系
- Passive Joints,设置某个关节无需进行计算,一般不设置
- 输出配置文件(可在.srdf文件中查看配置结果)
可以运行文件夹中demo.launch尝试是否配置成功
3、如何使用Moveit!实现机械臂仿真(gazebo)
将创建的六轴机械臂放在gazebo环境中,通过Moveit!来控制机械臂做移动。我目前认知,只有在gazebo中仿真才需要完善模型等操作,而真实机械臂是不需要的。
完善模型
同样,ur等机械臂已经完成了该步骤,如果需要自己设置,则参考以下步骤:
- 在.xacro中完善一些描述信息——惯性参数<-inertial>和碰撞属性<-collision>(均为每一个link需要设置的)
- 为link添加gazebo标签——如设置link颜色,在.xacro中设置的颜色在rviz中能够看到,但在gazebo中显示不出来(gazebo有自己的颜色显示系统)。还可以设置摩擦系统等物理仿真引擎(如在gazebo中添加其他仿真模型物体需要设置摩擦参数,材质等等)。ur中文件位置为ur_description/urdf/ur.gazebo.xacro
<gazebo reference="base_link">
<material>Gazebo/White</material>
</gazebo>
- 为joint添加传动装置——可以简单理解为给机械臂装电机。(传动)Transmissions就是机器人的传动系统,机器人每个需要运动的关节都需要配置相应的Transmission,其通常在urdf文件中直接添加。如ur中在ur5.urdf.xacro文件中有:
<xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
该部分涉及ros_control的知识,可以参考https://www.guyuehome.com/890。如要自己写该部分,以elbow_joint为例,其他joint几乎一样:
<transmission name="${prefix}elbow_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}elbow_joint">
<hardware_interface>hardware_interface/PositionJointInterface</hardware_interface>
</joint>
<actuator name="${prefix}elbow_motor">
<machanicalReduction>1</machanicalReduction>
</actuator>
</transmission>
- 添加gazebo控制器插件。在ur中,ur_description/urdf/ur5_robot.urdf.xacro中有include进同目录下的common.gazebo.xacro。在common.gazebo.xacro有:
<gazebo>
<plugin name="ros_control" filename="libgazebo_ros_control.so">
</plugin>
</gazebo>
在gazebo中加载机械臂模型
<?xml version="1.0"?>
<launch>
<arg name="paused" default="true"/>
<arg name="gui" default="true"/>
<arg name="sim" default="true" />
<arg name="debug" default="false"/>
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!--arg name="world_name" default="worlds/empty.world"/-->
<arg name="world_name" default="$(find ur5_single_arm_tufts)/worlds/ur5_cubes.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
</include>
<!-- send robot urdf to param server -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur5_single_arm_tufts)/urdf/ur5_single_arm.urdf.xacro'"/>
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model robot -z 0.594
-J shoulder_lift_joint -2.0
-J elbow_joint 1.0"
output="screen" />
<!-- 同时设置了初始位姿 -->
</launch>
到这一步为止,运行上面代码的launch文件,可以在gazebo中看到机械臂模型,但是无法通过GUI rviz、C++、python等接口去发布信息控制机械臂运动。要控制机械臂运动还需要完善ros_control控制器。
ros_control
参考https://www.guyuehome.com/890
配置并加载控制器
在ur中,ur_gazebo/controller/arm_controller_ur5.yaml中有进行配置。
注:此时从步骤上分析/arm_controller_ur5.yaml中仅有joint_group_position_controller,并无arm_controller(其中的JointTrajectoryController为Moveit!和机械臂的接口,参考下面重要的图)
arm_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 10
joint_group_position_controller:
type: position_controllers/JointGroupPositionController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint`在这里插入代码片`
然后在launch文件中:
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
<rosparam file="$(find robotiq_85_gazebo)/controller/gripper_controller_robotiq.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller gripper" respawn="false" output="screen"/>
(此时还没有joint_state_controller[等价于joint_state_publisher]和robot_state_publisher),所以在最终的launch文件中有:
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<?xml version="1.0"?>
<launch>
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" type="string" value="" />
</node>
<!-- Fake Calibration -->
<node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
args="pub /calibrated std_msgs/Bool true" />
<!-- joint_state_controller -->
<rosparam file="$(find ur_gazebo)/controller/joint_state_controller.yaml" command="load"/>
<node name="joint_state_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>
</launch>
joint_state_controller.yaml文件中为:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
此时相当于有三个控制器——joint_state_controller、joint_group_position_controller:(包含六个关节的position)、gripper,还未包含arm_controller中的JointTrajectoryController。/joint_states话题可以查看机械臂状态。
最终使用Moveit!
以上虽然让机械臂在gazebo中动起来了,但是到刚才的运动为止,没有任何Moveit!的操作。那Moveit!给我们的究竟是什么呢?——关节轨迹(JointTrajectory)
刚才配置的PositionController为针对每一个关节的输入(应该是位置?),该输入并不是轨迹。
如何将机器人控制Moveit!连接在一起,即Moveit!的输出和机器人的输入之间接口是什么。通过Action通信机制——FollowJointTrajectory传递Trajectory数据P[] V[] A[] T(位置、速度、角度、时间)组合起来描述一系列轨迹点。follow_joint_trajectory是MoveIt!最终运动规划发布的action消息,由机器人控制器端接收该消息后控制机器人完成运动。在rostopic list中,可以找到follow_joint_trajectory,由仿真机器人的控制器插件订阅。
下图十分重要,解释了Moveit!最终输出的为轨迹点,以及我们使用Moveit!需要配置的controller
此时从步骤上来说,即完成了ur_gazebo/controller/arm_controller_ur5.yaml中有关arm_controller的配置。
总的来说,此时完成了上图中插座端的JointTrajectoryController和JointStateController。现在gazebo可以接收轨迹,反馈状态,接下来则需要完成插头端Moveit!的Controller配置FollowJointTrajectory(ros中的Action),以便将轨迹发布出去。
在ur中,ur5_moveit_config/config/controllers.yaml中即完成了该配置,同时在ur5_moveit_config/launch/ur5_moveit_controller_manager.launch.xml中加载了该参数包。
controller_list:
- name: ""
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
接下来即可定义最上层的launch文件,将各类参数加载进来:
这一部分的套路就是先写controller的yaml参数文件,再在launch文件中rosparam load,最后通过controller_manager pkg载入。
4、通过Moveit!控制机械臂运动(用户接口:C++、python)
通过编程控制机械臂做运动。
实际上通过编程接口控制机械臂运动经过了以下四步:
编程模式
Moveit!中编程控制机械臂的主要步骤:
- 连接控制需要的规划组(如arm组或gripper组)
- 设置目标位姿(关节空间或笛卡尔空间)
- 设置运动约束(可选,如夹水杯,保持水杯向上)
- 使用Moveit!规划一条到达目标的轨迹
- 修改轨迹(如速度等参数)
- 执行规划出的轨迹
主要通过学习Moveit!官方教程来学习相应的API功能。
几种空间运动
设置关节角度:主要API:
- 创建规划组的控制对象
- 设置关节空间运动的目标位姿
- 完成规划并控制机械臂完成运动
设置终端位姿:主要API:
- 创建规划组的控制对象
- 获取机器人的终端Link名称
- 设置目标位姿对应的参考坐标系和起始、终点
- 完成规划与运动
笛卡尔路径运动
plan:规划出来的运动轨迹
fraction:描述规划成功的路径在给定路点列表中的覆盖率,如果fraction小于1,说明给定的路点列表没办法完整规划路径。
轨迹约束
给轨迹加约束,如夹杯子,希望杯子保持朝上,则终端需要保持朝上。
Moveit!中运动规划的流程如下:
从左到右:
- 运动规划请求:让机械臂从A到B,B即目标值
- 适配器组:检查机械臂初始状态,设置一些约束条件
- 运动规划器:调用OMPL中算法,计算轨迹。此时得到的轨迹只有P[],没有V[],A[],t。
- 适配器组:添加速度、加速度、时间参数
- 运动规划应答:即为FollowJointTrajectory中要发布的轨迹
轨迹修改
修改已经plan好的轨迹,也可以理解为自己加一些“约束”,通过修改结构体(轨迹)中的数据。需要自己事先实现相应的API。
后记
以上即是博主结合课程和ur5机械臂的理解笔记,如有不当之处还望各位同行大佬多多指教。单Moveit!这一部分感觉还有运动学插件、规划场景没有讲到,接下来应该就是运动学插件、物体抓取与加入视觉了。
ps: 写了两天,第二天写完的时候都发布了,发现发布的只有第一天的内容,又要重新写,气死了