大家好，我是（月更侠）小明，很高兴我们又见面啦。相信大家从我之前的文章中能看出来，我所讲的内容基本都面向ROS新人，因为大家刚入门时，往往很难一窥程序的深层运行逻辑，使得许多问题迟迟无法解决，甚是耽误时间。例如在和小伙伴们日常交流时，就反复遇到过一些关于Gazebo仿真控制的“经典”问题：
A 我的模型一直抖动，怎么办？
B PID参数该怎么设置啊？我怎么调都没用！
C 我必须要设置PID参数吗？如果不设置会怎么样
D gazebo_ros_control插件可以完成计算力矩控制等更加复杂的控制策略吗？
E 我的并联机器人模型只能用sdf创建，可以使用gazebo_ros_control进行控制吗？
……
通过本篇文章的分析，我们将揭晓这些问题的答案。
在Gazebo仿真中，我们最常使用的控制插件就是是gazebo_ros_control，它是ros_control针对Gazebo仿真开发的插件。
图1 ros_control组成概览，其中左上角方框内就是gazebo_ros_control插件所实现的功能 图片来源
该插件的详细使用方法，可参考古月居ROS探索总结系列的相关文章。简单来说，就是进行了如下三个操作：
1 在机器人模型的URDF文件中，添加gazebo_ros_control插件，并添加transmission指明了各关节控制器的类型信息
2 建立一个yaml文件，用来定义各控制器的pid参数
3 建立一个roslaunch，用来加载模型、控制参数、控制器，并发布tf
1 文件结构
接下来我们来看看gazebo_ros_control功能包究竟对这些文件做了什么。需要注意，gazebo_ros_control源码已经集成到了github帐号ros-simulation下的gazebo_ros_pkgs项目中，而ros_controls账号下的gazebo_ros_pkgs是旧版本，现在已经不再使用了。源码地址：gazebo_ros_control
该功能包的核心是硬件接口文件src/default_robot_hw_sim.cpp，该文件定义了以下函数，用以实现相应功能：
clamp()：区间限定函数，用以规范某些变量的上下限
DefaultRobotHWSim::initSim()：初始化插件的各项属性
DefaultRobotHWSim::readSim()：从Gazebo仿真器中读取模型的信息和状态
DefaultRobotHWSim::writeSim()：将目标值写入Gazebo仿真器中
下面我们分析几个重点函数。
DefaultRobotHWSim::initSim()函数初始化了整个插件，首先处理了URDF文件中transmissions模块信息，主要是以下方面：
1 定义了机器人的自由度数
2 从transmissions中获取各个关节接口类型，名称
3 判断是否使用PID控制，如果没有设置PID参数，则视为不使用PID
并对模型进行了以下处理：
1 从gazebo模型中获取关节名称，并实例化各关节
2 为各个关节gazebo模型的行程设置上下限
读取函数readSim()和写入函数writeSim()，分别实现以下功能：
1 从gazebo仿真器中读取个关节的当前状态
2 根据控制器类型将控制量写入仿真器
2 控制代码
我们看看各类型的控制方法具体是如何实现的
1 力控制
力控制十分简单，直接将目标量写入了仿真器，其中SetForce()函数是Gazebo仿真器自带的关节力设置函数。
case EFFORT:
{
const double effort = e_stop_active_ ? 0 : joint_effort_command_[j];
sim_joints_[j]->SetForce(0, effort);
}
break;
2 PID位置控制
PID位置控制稍显复杂，首先计算当前状态下的位置误差error，其中对于REVOLUTE（转动副）类型的关节，还要考虑关节限位。再利用pid_controllers下的computeCommand()方法计算当前误差下应当施加到关节上的力，并使用区间限定函数clamp()来规范力的上下限。从此处可以看出，gazebo_ros_control插件实际上使用了输入力的PID位置控制，该方法的原理可见我在古月居发表的第一篇文章。最后，利用SetForce()函数将目标量写入了仿真器。
case POSITION_PID:
{
double error;
switch (joint_types_[j])
{
case urdf::Joint::REVOLUTE:
angles::shortest_angular_distance_with_limits(joint_position_[j],
joint_position_command_[j],
joint_lower_limits_[j],
joint_upper_limits_[j],
error);
break;
case urdf::Joint::CONTINUOUS:
error = angles::shortest_angular_distance(joint_position_[j],
joint_position_command_[j]);
break;
default:
error = joint_position_command_[j] - joint_position_[j];
}
const double effort_limit = joint_effort_limits_[j];
const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
-effort_limit, effort_limit);
sim_joints_[j]->SetForce(0, effort);
}
break;
3 PID速度控制
PID速度控制函数和PID位置控制函数的逻辑完全一致，唯一的区别是误差error取的是速度误差。
case VELOCITY_PID:
double error;
if (e_stop_active_)
error = -joint_velocity_[j];
else
error = joint_velocity_command_[j] - joint_velocity_[j];
const double effort_limit = joint_effort_limits_[j];
const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
-effort_limit, effort_limit);
sim_joints_[j]->SetForce(0, effort);
break;
4 非PID的位置控制和速度控制
如果不使用PID控制，那么硬件会直接调用Gazebo的SetPosition()和SetVelocity()接口，强行设置关节的位置和速度，而非通过输入力的方式进行控制。代码分别位于320行到330行，以及359行到372行。
3 答案揭晓
根据源码的分析，我们在Gazebo仿真中的很多“万年难题”就迎刃而解啦！例如：
A 我的模型在仿真时一直抖动，怎么办？
答：在保证模型惯量属性、关节摩擦、阻尼合理的前提下，调整PID参数。从源码看到，控制器的PID控制实际上是输入力的PID控制，模型物理属性不符合真实情况、PID参数不合理必然导致关节振荡。这也提醒我们，良好的Gazebo仿真，其前提是真实的物理模型和合理的理论推导。务必不要将其当做纯动画制作软件！
图2 图3 由于质量属性\PID参数不合理，机械臂发生了细微的或鬼畜般的抖动。 图片来源
B PID参数该怎么设置啊？我怎么调都没用！
答：自己努力吧孩子！还是如上一个问题所述，在仿真之前，先进行完整的控制理论推导，而非随便尝试。提供两个小技巧：动态配置插件Dynamic Reconfigure可以方便地动态调整PID参数，如图。另外尤其要注意一个问题：gazebo_ros_control对于PID中的积分增益i是默认锁定为0的，要想使用积分控制，需要在yaml文件中打开积分增益的上下限，否则无论怎么调积分增益都没有用的（坑…）。
# pid增益的正确配置姿势(如要使用积分控制，必须打开积分增益的上下限，否则默认均为0)
joint1_position_controller:
type: effort_controllers/JointPositionController
joint: joint_1
pid: {p: 100.0, i: 1.0, d: 10.0, ,i_clamp_max: 0.0, i_clamp_max: 1.0}
图4 PID参数动态配置插件Dynamic Reconfigure
C 我必须要设置PID参数吗？如果不设置会怎么样
答：不是必要的，但推荐在yaml中设置PID参数。从源码中可以看到，gazebo_ros_control插件会自动判断用户是否在yaml文件中设置了PID参数。如果没有，当用户使用位置控制和速度控制时，就会调用Gazebo默认的位置和速度设置接口。而根据注释信息可以看到，如果你的Gazebo版本小于9，这样做就是有坏处的：仿真器不会模拟模型的重力，会导致动力学仿真异常。因此，我们最好还是在yaml文件中设置合理的PID参数。
ROS_WARN_ONCE("The default_robot_hw_sim plugin is using the Joint::SetPosition method without preserving the link velocity.");
ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model.");
ROS_WARN_ONCE("Please set gazebo_pid parameters, switch to the VelocityJointInterface or EffortJointInterface, or upgrade to Gazebo 9.");
ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612");
图5 在gazebo9以前的版本中，一只手由于没有设置PID参数导致重力异常，悬浮在空中。图片来源
D gazebo_ros_control插件可以完成计算力矩控制等更加复杂的控制策略吗？
答：需要基于插件的自定义接口自己编写程序。从源码中看到，插件所提供的控制策略只有基本的PID控制，其输入量的计算只考虑了误差，并没有考虑任何动力学模型，详见control_toolbox项目下的src/pid.cpp文件。
E 我的并联机器人模型只能用sdf创建，可以使用gazebo_ros_control进行控制吗？
答：不可以。从源码看出，控制器类型等相关信息必须从urdf下的transmissions获取，甚至在注册关节限位时也使用了urdf类，而并没有支持sdf格式。不过，由于sdf是Gazebo默认的支持格式，有完整的官方文档提供，因此根据gazebo_ros_control的逻辑重新创建一个适用于sdf的控制插件其实并不困难。
图6 笔者利用sdf创建的六自由度并联机器人，使用自定义Gazebo控制插件
以上就是我们从源码中找到的答案，有没有解决你的一些疑惑呢？
如果你还想进一步交流，欢迎在这篇文章下给我留言。
