DefaultRobotHWSim::writeSim()详解

源码部分（已加注释）

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得控制啊

=========================================================================================================