【ROS】机器人的速度/角度/力矩控制是如何实现的

作者写在前面的话，我是希望通过寥寥几篇博客可以把一个复杂的代码核心问题讲清楚的，比如ROS 1/2对外骨骼机器人的控制这件事。之前的【FileZilla】系列博客通过五六篇把如何实现文件传输的核心功能给讲明白了，这篇一点点字多多的代码截图读者能看到机器人的电机是如何被控制的，甚至会发现并没有书中提到的复杂的控制技术，而仅仅是一个工具包里的PID算法。

首先是看一个惯常的控制接口的使用流程：

cpp 复制代码

// (1) 通过Launch启动Node
ros::init(argc, argv, "drv_hw");

// (2) 初始化ROS的Control Manager
controller_manager::ControllerManager cm(&drv_hw, nh);

//（3）读取电机数据
drv_hw.read();
drv_hw.validate();

//（4）控制算法控制
cm.update(now, elapsed_time, drv_hw.get_reset_update());

//（5）发送控制指令
drv_hw.write(elapsed_time);

那么问题是ROS的Control Manager是如何读到电机数据的结构 ，然后通过什么样的计算 ，然后写出电机数据的新控制数据的结构 呢？从下面一步一步的截图，我们可以看到首先ROS使用了接口注册机制 ，然后是通过了PID算法对力进行控制。

cpp 复制代码

// https://github.com/ros-controls/ros_controllers
// Inside ros_controllers/effort_controllers/src/joint_position_controller.cpp
//
void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
{
  command_struct_ = *(command_.readFromRT());
  double command_position = command_struct_.position_;
  double command_velocity = command_struct_.velocity_;
  bool has_velocity_ =  command_struct_.has_velocity_;

  double error, vel_error;
  double commanded_effort;

  double current_position = joint_.getPosition();

  // Make sure joint is within limits if applicable
  enforceJointLimits(command_position);

  // Compute position error
  if (joint_urdf_->type == urdf::Joint::REVOLUTE)
  {
    angles::shortest_angular_distance_with_large_limits(
      current_position,
      command_position,
      joint_urdf_->limits->lower,
      joint_urdf_->limits->upper,
      error);
  }
  else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
  {
    error = angles::shortest_angular_distance(current_position, command_position);
  }
  else //prismatic
  {
    error = command_position - current_position;
  }

  // Decide which of the two PID computeCommand() methods to call
  if (has_velocity_)
  {
    // Compute velocity error if a non-zero velocity command was given
    vel_error = command_velocity - joint_.getVelocity();

    // Set the PID error and compute the PID command with nonuniform
    // time step size. This also allows the user to pass in a precomputed derivative error.
    commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
  }
  else
  {
    // Set the PID error and compute the PID command with nonuniform
    // time step size.
    commanded_effort = pid_controller_.computeCommand(error, period);
  }

  joint_.setCommand(commanded_effort);

  // publish state
  if (loop_count_ % 10 == 0)
  {
    if(controller_state_publisher_ && controller_state_publisher_->trylock())
    {
      controller_state_publisher_->msg_.header.stamp = time;
      controller_state_publisher_->msg_.set_point = command_position;
      controller_state_publisher_->msg_.process_value = current_position;
      controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
      controller_state_publisher_->msg_.error = error;
      controller_state_publisher_->msg_.time_step = period.toSec();
      controller_state_publisher_->msg_.command = commanded_effort;

      double dummy;
      bool antiwindup;
      getGains(controller_state_publisher_->msg_.p,
        controller_state_publisher_->msg_.i,
        controller_state_publisher_->msg_.d,
        controller_state_publisher_->msg_.i_clamp,
        dummy,
        antiwindup);
      controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
      controller_state_publisher_->unlockAndPublish();
    }
  }
  loop_count_++;
}

/***注意到这段代码：***/
// Decide which of the two PID computeCommand() methods to call
if (has_velocity_)
{
  // Compute velocity error if a non-zero velocity command was given
  vel_error = command_velocity - joint_.getVelocity();

  // Set the PID error and compute the PID command with nonuniform
  // time step size. This also allows the user to pass in a precomputed derivative error.
  commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
}
else
{
  // Set the PID error and compute the PID command with nonuniform
  // time step size.
  commanded_effort = pid_controller_.computeCommand(error, period);
}

  joint_.setCommand(commanded_effort);

cpp 复制代码

double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
{
  // Get the gain parameters from the realtime buffer
  Gains gains = *gains_buffer_.readFromRT();

  double p_term, d_term, i_term;
  p_error_ = error; // this is error = target - state
  d_error_ = error_dot;

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = gains.p_gain_ * p_error_;

  // Calculate the integral of the position error
  i_error_ += dt.toSec() * p_error_;

  if(gains.antiwindup_ && gains.i_gain_!=0)
  {
    // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
    boost::tuple<double, double> bounds = boost::minmax<double>(gains.i_min_ / gains.i_gain_, gains.i_max_ / gains.i_gain_);
    i_error_ = boost::algorithm::clamp(i_error_, bounds.get<0>(), bounds.get<1>());
  }

  // Calculate integral contribution to command
  i_term = gains.i_gain_ * i_error_;

  if(!gains.antiwindup_)
  {
    // Limit i_term so that the limit is meaningful in the output
    i_term = boost::algorithm::clamp(i_term, gains.i_min_, gains.i_max_);
  }

  // Calculate derivative contribution to command
  d_term = gains.d_gain_ * d_error_;

  // Compute the command
  cmd_ = p_term + i_term + d_term;

  // Publish controller state if configured
  if (publish_state_ && state_publisher_)
  {
    if (state_publisher_->trylock())
    {
      state_publisher_->msg_.header.stamp = ros::Time::now();
      state_publisher_->msg_.timestep = dt;
      state_publisher_->msg_.error = error;
      state_publisher_->msg_.error_dot = error_dot;
      state_publisher_->msg_.p_error = p_error_;
      state_publisher_->msg_.i_error = i_error_;
      state_publisher_->msg_.d_error = d_error_;
      state_publisher_->msg_.p_term = p_term;
      state_publisher_->msg_.i_term = i_term;
      state_publisher_->msg_.d_term = d_term;
      state_publisher_->msg_.i_max = gains.i_max_;
      state_publisher_->msg_.i_min = gains.i_min_;
      state_publisher_->msg_.output = cmd_;
      state_publisher_->unlockAndPublish();
    }
  }

  return cmd_;
}

这样我们就找到了ROS控制器的核心控制代码！