源码部分（已加注释）
void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period)
{
// If the E-stop is active, joints controlled by position commands will maintain their positions.
// 紧急停止部分内容
if (e_stop_active_)
{
if (!last_e_stop_active_)//保存当前停止位置指令
{
last_joint_position_command_ = joint_position_;//以当前位置 作为位置控制指令
last_e_stop_active_ = true; // 同一时间断内得急停指令不更新
}
joint_position_command_ = last_joint_position_command_;// 急停时得位置作为位置控制指令
}
else
{
last_e_stop_active_ = false;//急停时位置可被更新
}
// joint 限制相关内容
ej_sat_interface_.enforceLimits(period);
ej_limits_interface_.enforceLimits(period);
pj_sat_interface_.enforceLimits(period);
pj_limits_interface_.enforceLimits(period);
vj_sat_interface_.enforceLimits(period);
vj_limits_interface_.enforceLimits(period);
//遍历每个joint
for(unsigned int j=0; j < n_dof_; j++)
{
switch (joint_control_methods_[j]) // 根据不同joint 得控制接口 进行不同处理
{
case EFFORT: // joint 为力控制接口
{
// 判断是否是急停 是至0 不是 赋 joint_effort_command_ ， ？？？此命令如何更新 应该是 ros control里发布得
const double effort = e_stop_active_ ? 0 : joint_effort_command_[j];
// 设置 gazebo 里 joint 受到得力
sim_joints_[j]->SetForce(0, effort);
}
break;
case POSITION: // 由上面看应该不灰进才对
#if GAZEBO_MAJOR_VERSION >= 9
sim_joints_[j]->SetPosition(0, joint_position_command_[j], true);
#else
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");
sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
#endif
break;
case POSITION_PID: // 位置控制接口应该进这个地方
{
double error;
switch (joint_types_[j]) // 根据joint 得类型 在urdf里设置得
{
case urdf::Joint::REVOLUTE: // joint 旋转 带限位得
// 由当前位置 和目前位置指令 和限位信息 综合计算 更新位置偏差 error（最短角距离）
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:// joint 旋转 无限转
// 由当前位置 和目前位置指令 更新位置偏差 error （最短角距离）
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]; // joint得 effort 幅值限制
// pid_controllers_[j].computeCommand(error, period) 反馈得pid 计算结果 即joint应受得力
//clamp() 对pid 计算 joint应受得力 进行幅值限制
const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
-effort_limit, effort_limit);
// 和力控制接口一样，最终都归结到 力
// 设置 gazebo 里 joint 受到得力
sim_joints_[j]->SetForce(0, effort);
// ？？？ 既然都最终归结到力 那控制器选择 位置控制时， joint接口 为力接口， 控制器得输出是力指令吗
// ？？？ 需要看 位置控制器得输出究竟是什么
// ？？？ joint_position_command_ 位置得指令是怎么发布得 那应该可以直接发布一个joint_position_command_期望位置来完成joint得控制啊
}
break;
case VELOCITY://应该不会进来
#if GAZEBO_MAJOR_VERSION > 2
//ODE(Open Dynamics Engine) gazebo 得模式 可以先不管 应该是 ode模式
if (physics_type_.compare("ode") == 0)
{
sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
}
else
{
sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
}
#else
sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
#endif
break;
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];
// pid_controllers_[j].computeCommand(error, period) 反馈得pid 计算结果 即joint应受得力
//clamp() 对pid 计算 joint应受得力 进行幅值限制
const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
-effort_limit, effort_limit);//与位置控制同理
// 和力控制接口一样，最终都归结到 力
// 设置 gazebo 里 joint 受到得力
sim_joints_[j]->SetForce(0, effort);
break;
// ？？？ 既然都最终归结到力 那控制器选择 速度控制时， joint接口 为力接口， 控制器得输出是力指令吗
// ？？？ 需要看 速度控制器得输出究竟是什么
// ？？？ joint_velocity_command_ 速度得指令是怎么发布得 那应该可以直接发布一个joint_velocity_command_期望速度来完成joint得控制啊
}
}
}对源码进行功能分块解读
由上至下
=========================================================================================================
紧急停止部分内容
同一时间断内得急停指令不更新
触发急停时得位置作为位置控制指令
=========================================================================================================
joint 限制相关内容
=========================================================================================================
遍历每个joint
根据不同joint 得控制接口 进行不同处理 下面不再赘述
是什么控制接口 在初始化函数 DefaultRobotHWSim::initSim() 进行了设置 详看此链接
joint 为力控制接口时
判断是否是急停 是至0 不是 赋 joint_effort_command_
- [ ] 此命令如何更新 应该是 ros control里发布得 要看ros controller 相关内容
sim_joints_[j]->SetForce(0, effort)此函数设置 gazebo 里 joint 受到得力
下面位置控制接口与速度控制接口最后都归到调用此函数 就是用pid进行计算，得到joint应受得力
在用ros控制器时 设置得控制器为位置控制器
如这篇博客
得控制器配置时 用得
应该就是直接输出了 joint_effort_command_ 指令 ，效果类似直接用位置控制接口
- [ ] 具体应该看 jointpositioncontroller得源码
=========================================================================================================
位置控制接口应该进这个地方
根据joint 得类型 在urdf里设置得 进行不同的处理
设置如这篇博客
- [ ] joint的类型可以再写篇博客介绍
REVOLUTE： joint 旋转 带限位得
处理此种joint类型时
由当前位置 和目前位置指令 和限位信息 综合计算 更新位置偏差 error（最短角距离）
CONTINUOUS： joint 旋转 无限转
由当前位置 和目前位置指令 更新位置偏差 error （最短角距离）
不是上面那两种
直接做差得到error
pid_controllers_[j].computeCommand(error, period) 反馈得pid 计算结果 即joint应受得力
clamp() 对pid 计算 joint应受得力 进行幅值限制
和力控制接口一样，最终都归结到 力
设置 gazebo 里 joint 受到得力
- [ ] 这里存在一个疑问
既然都最终归结到力 那控制器选择 位置控制时， joint接口 为力接口， 控制器得输出是力指令吗
需要看 位置控制器得输出究竟是什么
joint_position_command_ 位置得指令是怎么发布得 那应该可以直接发布一个joint_position_command_期望位置来完成joint得控制啊
=========================================================================================================
速度控制接口应该进这个地方
pid_controllers_[j].computeCommand(error, period) 反馈得pid 计算结果 即joint应受得力
clamp() 对pid 计算 joint应受得力 进行幅值限制
和力控制接口一样，最终都归结到 力
设置 gazebo 里 joint 受到得力
- [ ] 这里存在一个疑问
既然都最终归结到力 那控制器选择 位置控制时， joint接口 为力接口， 控制器得输出是力指令吗
需要看 速度控制器得输出究竟是什么
joint_velocity_command_速度得指令是怎么发布得 那应该可以直接发布一个joint_velocity_command_期望位置来完成joint得控制啊
=========================================================================================================
