ros_controls/ros_controllers の制御の仕組み (position/effort/velocity_controllers の基礎)

# はじめに
`ros_control`の辺りって，チュートリアルを読みながら作業を進めれば何となく分かった気になるし、まぁ使えるっちゃ使えるんだけど，実際中身はどうなっているんだろう？と，気になったりはしませんか？

名前空間とかクラス名のあちこちに`effort`や`position`とか`velocity`とか入り込んできているし，容量の悪い私の頭は随分と混乱してしまいました．[ROS Wiki](http://wiki.ros.org/ros_control) を見ても，詳しい解説があるわけでもなく，あとはコードへのリンクがペトッと貼ってあるだけです．

じゃあ，コードを読んで自分で理解しろってことか！と解釈し，理解した内容をメモするに至りました．結論は，普通のPID制御をしているだけなのですが，どんなフローで指令が計算されているかを知った上で使えるようになれば，実装も変わってくるでしょうし，中身が分かれば自分好みに変えちゃうことだってできるわけです．

受け取る指令値と関節に送る指令値の組み合わせによって解釈が変わる部分も存在しており，それらの情報を文字だけで整理すると混乱を招くと判断したため，簡易的なブロック線図を示すことで視覚的にも仕様の理解を支援できるようにしました．

というわけで，コードをトレースしながら理解を深めていくことにします．

# 扱うコントローラの種類
`Controller`が受け取る指令の種類が`position`, `velocity`,  `effort`の3種類，ロボットの関節に送る指令値の種類(HardwareInterface)も同様に3種類ですが，その組み合わせは下記の6通りとなっています．

**position**_controllers (関節に位置指令を送る)
---
- Joint **Position** Controller (位置指令を受け取る)

**velocity**_controllers (関節に速度指令を送る)
---
- Joint **Velocity** Controller (速度指令を受け取る)
- Joint **Position** Controller (位置指令を受け取る)

**effort**_controllers (関節に力指令を送る)
---
- Joint **Effort** Controller (力指令を受け取る)
- Joint **Velocity** Controller (速度指令を受け取る)
- Joint **Position** Controller (位置指令を受け取る)

以上の名称から推察するに，下記のような命名ルールが成り立っていそうです．

- `JointHogeContoroller`の`Hoge`には，他ノードから受け取る指令値の種類が入る．
- `hoge_controllers`の`hoge`には，ロボットの関節に送る指令値の種類が入る．

この対応関係を表にすると，下記のとおりです．**太字で頭大文字**が受け取る指令値の種別，全小文字が関節に送る指令値の種別です．

|          | Position | Velocity | Effort |
|:--------:|:--------:|:--------:|:------:|
| position |     ○    |     ☓    |   ☓    |
| velocity |     ○    |     ○    |    ☓   |
|  effort  |     ○    |     ○    |    ○   |

# 基本ソフトウェア構成
すぐコードを見たい所ですが，まずは`gazebo`で`ros_control`を利用する場合を例に，基本構成をおさらいします．
詳細については，以前に小生が投稿した[Controller と HardwareInterface との間の処理の仕組み](http://qiita.com/MoriKen/items/613635b90f3a98042dc5)に譲らせて頂きます．

![basic_configuration.png](https://qiita-image-store.s3.amazonaws.com/0/110138/87f58600-6377-9fe1-abb1-a089133cab04.png)

一番大事なのは，`GazeboRosControlPlugin`が定周期で，

1. 状態読込コマンドを`RobotHWSim`に送信し，`gazebo`から関節の最新状態を取得する，
2. 取得した最新状態を基に，`Controller`に制御指令更新コマンドを送信し，指令値を更新する，
3. 更新された指令値を`gazebo`に送信する，

という処理を実行していることです．特に，2 では`Controller::update()`というメソッドが呼ばれますが，これは`ros_controls::ros_controllers`が提供するPID制御の動作を決定づけるメソッドなので，ここを中心に攻めることになります．

更に，実際にPID制御を司るクラスとして`ros-controls::control_toolbox::Pid`というものが存在しており，制御の実体はそちらにあります．本エントリでは，その中の計算方法まで紐解いていきます．以下の図をイメージしながら読み進めると，理解がしやすくなると思います．

![basic_configuration_pid.png](https://qiita-image-store.s3.amazonaws.com/0/110138/1640ad7d-7b22-6933-a577-3a0083ed6a14.png)

それでは，「扱うコントローラの種類」で挙げたパターンごとに順番に中身を見ていきます．

# `hardware_interface::PositionJointInterface`に対応する`Controller`

`hardware_interface::VelocityJointInterface`に対応する`Controller`は，次のとおりです．

- `position_controllers::JointPositionController`

はい，これだけです．
つまり，**位置指令**を受け取ったら，ロボットの関節に対しても**位置指令**でしかインターフェースしません．

さっそく，中を追ってみましょう．

## `position_controllers::JointPositionController`

[ros_controllers/position_controllers/include/position_controllers/joint_position_controller.h](https://github.com/ros-controls/ros_controllers/blob/jade-devel/position_controllers/include/position_controllers/joint_position_controller.h)

```cpp:joint_position_controller.h
// 省略
namespace position_controllers
{

/**
 * \brief Joint Position Controller (linear or angular)
 *
 * This class passes the commanded position down to the joint
 *
 * \section ROS interface
 *
 * \param type Must be "JointPositionController".
 * \param joint Name of the joint to control.
 *
 * Subscribes to:
 * - \b command (std_msgs::Float64) : The joint position to apply
 */
typedef forward_command_controller::ForwardCommandController<hardware_interface::PositionJointInterface>
        JointPositionController;
}
// 省略
```

`JointPositionController`の正体は，`ForwardCommandController`というクラスです．テンプレートクラスとなっており，ここでは`HardwareInterface`として`hardware_interface::PositionJointInterface`を指定しています．

[ros_controllers/forward_command_controller/include/forward_command_controller/forward_command_controller.h](https://github.com/ros-controls/ros_controllers/blob/jade-devel/forward_command_controller/include/forward_command_controller/forward_command_controller.h)

```cpp:forward_command_controller.h
template <class T>
class ForwardCommandController: public controller_interface::Controller<T>
{
public:
  ForwardCommandController() {}
  ~ForwardCommandController() {sub_command_.shutdown();}

  bool init(T* hw, ros::NodeHandle &n)
  {
    std::string joint_name;
    if (!n.getParam("joint", joint_name))
    {
      ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
      return false;
    }
    joint_ = hw->getHandle(joint_name);
    sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &ForwardCommandController::commandCB, this);
    return true;
  }

  void starting(const ros::Time& time);
  void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) {joint_.setCommand(*command_buffer_.readFromRT());}

  hardware_interface::JointHandle joint_;
  realtime_tools::RealtimeBuffer<double> command_buffer_;

private:
  ros::Subscriber sub_command_;
  void commandCB(const std_msgs::Float64ConstPtr& msg) {command_buffer_.writeFromNonRT(msg->data);}
};

}
```

他のノードが`"command"`というトピックに位置指令をパブリッシュしたら，`commandCB()`がコールされ，`command_buffer_`に位置指令が入力されます．

定周期で呼ばれる`update()`では`hardware_interface::PositionJointInterface`にマップされた`joint_`に，`command_buffer_`から取得した位置指令を代入します．

以上です．そうです，`JointPositionController`は，たったこれだけのことしかしていないのです．受け取った指令値を，そのまま`HardwareInterface`に横流ししているだけです．すなわち，**転送(Forward)**です．`ForwardCommandController`という名称が付いているのも，これで納得できます．

ブロック線図（？）も，下図のようにとてもシンプルなものになります．

![01_position_controller_PositionJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/51026f5e-1b22-2b81-e374-677dea02137d.png)

説明のため`gazebo`と記述していますが，実機に積まれたモータドライバ等でも構いません．実際には，低レイヤーで内部ループが回っているのですが，アプリケーションレイヤーの開発者としては知ったこっちゃないので（当然コードを読めば解析はできますが），**ブラックボックス**であるという意味を込めて，敢えて黒い箱で書いています．

なお，「これを**位置指令**としてロボットに送ることを，ロボット側が知っているかどうか」はまた別の話になります．
例えば，実際に指令値を`gazebo`側に送る`DefaultRobotHWSim`からしてみたら，この`command`が位置指令なのか，速度指令なのか，はたまた力指令なのかは，この司令だけをもらっても**知ったこっちゃありません**．
だから，事前に`URDF`の中で，どの`joint`がどの`HardwareInterface`を使うのかを，`<transmission>`タグで教えてあげる必要があります．

```xml:example
<robot xmlns:xacro="http://ros.org/wiki/xacro">
// 省略
    <transmission name="transmission1">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint1">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
    </transmission>
// 省略
</robot>
```
あとは，`DefaultRobotHWSim`が良きに計らって，指定された`HardwareInterface`に合わせて，自身の持つモデルに対して指令値を送ってくれます．

`RobotHWSim`や`RobotHW`のサブクラスを自分のロボット向けに設計するときは，仕様に合わせて作り込むことになります．


# `hardware_interface::VelocityJointInterface`に対応する`Controller`

`hardware_interface::VelocityJointInterface`に対応する`Controller`は，次のとおりです．

- `velocity_controllers::JointVelocityController`
 - 他ノードから**速度指令**を受け取り，ロボットの関節に対して**速度指令**を送信する
- `velocity_controllers::JointPositionController`
 - 他ノードから**位置指令**を受け取り，ロボットの関節に対して**速度指令**を送信する

`PositionJointInterface`では，自身のインターフェースの種別と同一である**位置指令**しか受け取ることができませんでしたが，`VelocityJointInterface`は**速度指令**と**位置指令**を受け取ることができます．どういう演算によってこれを実現しているのか，見てみましょう．

## `velocity_controllers::JointVelocityController`
まずは，単純な実装であるこちらから．

[ros_controllers/velocity_controllers/include/velocity_controllers/joint_velocity_controller.h](https://github.com/ros-controls/ros_controllers/blob/jade-devel/velocity_controllers/include/velocity_controllers/joint_velocity_controller.h)

```cpp:joint_position_controller.h
namespace velocity_controllers
{

/**
 * \brief Joint Velocity Controller (linear or angular)
 *
 * This class passes the commanded velocity down to the joint
 *
 * \section ROS interface
 *
 * \param type Must be "JointVelocityController".
 * \param joint Name of the joint to control.
 *
 * Subscribes to:
 * - \b command (std_msgs::Float64) : The joint velocity to apply
 */
typedef forward_command_controller::ForwardCommandController<hardware_interface::VelocityJointInterface>
        JointVelocityController;

}
```

ここでは`HardwareInterface`として`hardware_interface::VelocityJointInterface`を指定しています．あとは，`position_controllers::JointPositionController`と全く同様です．
実体のコードも[ros_controllers/forward_command_controller/include/forward_command_controller/forward_command_controller.h](https://github.com/ros-controls/ros_controllers/blob/jade-devel/forward_command_controller/include/forward_command_controller/forward_command_controller.h)で共通となります．

つまり，他ノードから**速度指令**を受け取ったら，そのまま**速度指令を**`HardwareInterface`に**転送(Forward)**するだけです．
先ほどと同じブロック線図になります．

![02_velocity_controller_VelocityJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/50703c38-f600-2f5d-6d04-87d710c416b7.png)

## `velocity_controllers::JointPositionController`
では，他ノードから**位置指令**を受け取り，**速度指令**に変換して`HardwareInterface`に送る処理はどうでしょうか．
本項は，以降全てに共通する内容を扱う出だしの項であり，ちょっと話が長くなるので，先に簡易的なブロック線図を載せておきます．

![03_velocity_controller_PositionJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/62bdfcff-98b2-6b95-9396-289c589ac91f.png)

今から対象とするのは，白い箱の`Controller`の部分です．ちょっと変形しているものも登場しますが，この基本形が分かれば問題ないと思います．では，コードを見てみます．

[ros_controllers/velocity_controllers/include/velocity_controllers/joint_position_controller.h](https://github.com/ros-controls/ros_controllers/blob/jade-devel/velocity_controllers/include/velocity_controllers/joint_position_controller.h)

```cpp
// 省略
namespace velocity_controllers
{

class JointPositionController: public controller_interface::Controller<hardware_interface::VelocityJointInterface>
{
// 省略
  bool init(hardware_interface::VelocityJointInterface *robot, ros::NodeHandle &n);
// 省略
  void update(const ros::Time& time, const ros::Duration& period);
// 省略
};
} // namespace
// 省略
```

こちらでは，`ForwardCommandController`を使用しません．`Controller`を直接継承し，テンプレートに`VelocityJointInterface`を指定しています．

気になるメソッドである`init()`と`update()`もオーバーライドされているようです．ただし，`velocity_controllers::JointPositionController`の場合については，`init()`の引数の型が`VelocityJointInterface`と明示的に指定された状態で，オーバーライドされています．

### 【寄り道】`init()`の引数の型を明示的に指定している件について
`init()`の引数の型が`VelocityJointInterface`と明示的に指定していることに矛盾はないか，気になりませんか？気になるので，ちょっとだけ寄り道します．
`controller_manager/spawner`で`Controller`をspawnする際のことを思い出しておきます．どの`Controller`とどの`HardwareInterface`を組み合わせて使うのか，その`HardwareInterface`が割り当てられる`joint`が何なのかを，事前に設定してきます．これを，`yaml`ファイルを使って設定する場合が多いです．

```yaml:example
hogehoge_controller:
  type: velocity_controllers/JointPositionController
  joints:
    - joint1
    - joint2
```

この情報があるから，`spawn`受け側であるサービスサーバにある`ControllerManager::loadController()`が呼ばれた際には，迷わず`initRequest`の引数の型として，`VelocityJointInterface`を指定できるのです．

具体的にコードを追います．以前投稿した[Controller と HardwareInterface との間の処理の仕組み（3. Controllerについて）](http://qiita.com/MoriKen/items/c29f653d03baffe5f0e2#controllermanager%E3%81%A8hardwareinterface%E3%81%A8%E3%81%AE%E3%83%9E%E3%83%83%E3%83%94%E3%83%B3%E3%82%B0)の中でも追ったことがあるのですが，とても重要な部分なので，ここで追います．
`ControllerManager::loadController()`が頭です．

[ros_control/controller_manager/src/controller_manager.cpp](https://github.com/ros-controls/ros_control/blob/jade-devel/controller_manager/src/controller_manager.cpp)

```cpp:controller_manager.cpp
// 省略
bool ControllerManager::loadController(const std::string& name)
{
// 省略
  try{
    initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
  }
// 省略
}
// 省略
```
`robot_hw_`は`RobotHW`型で，ロボットが持つ`joint`に関する`HardwareInterface`の情報を全て持っています．

[ros_control/controller_interface/include/controller_interface/controller.h](https://github.com/ros-controls/ros_control/blob/jade-devel/controller_interface/include/controller_interface/controller.h)

```cpp:controller.h
//省略
template <class T>
class Controller: public ControllerBase
{
  virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
                           ros::NodeHandle&             root_nh,
                           ros::NodeHandle&             controller_nh,
                           ClaimedResources&            claimed_resources)
  {
//省略
    // get a pointer to the hardware interface
    T* hw = robot_hw->get<T>();
//省略
    if (!init(hw, controller_nh) || !init(hw, root_nh, controller_nh))
//省略
  }
};
//省略
```

先ほどの宣言で確認したとおり，`velocity_controllers::JointPositionController`が呼ばれた場合には，テンプレートには`VelocityJointInterface`が必ず指定され，`RobotHW`インスタンスからそのポインタが返されます．それを`init()`の引数に取っているので，ここまで議論した内容の筋が通ります．

###`init()`の処理
本題に戻り，`init()`の中身を解析します．

[ros_controllers/velocity_controllers/src/joint_position_controller.cpp](https://github.com/ros-controls/ros_controllers/blob/jade-devel/velocity_controllers/src/joint_position_controller.cpp)

```cpp:joint_position_controller.cpp
// 省略
bool JointPositionController::init(hardware_interface::VelocityJointInterface *robot, ros::NodeHandle &n)
{
// 省略
  // Load PID Controller using gains set on parameter server
  if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
    return false;

  // Start command subscriber
  sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);

  // Get joint handle from hardware interface
  joint_ = robot->getHandle(joint_name);

  // Get URDF info about joint
  urdf::Model urdf;
  if (!urdf.initParam("robot_description"))
  {
    ROS_ERROR("Failed to parse urdf file");
    return false;
  }
  joint_urdf_ = urdf.getJoint(joint_name);
  if (!joint_urdf_)
  {
    ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
    return false;
  }

  return true;
}
```

PIDコントローラの初期化，指令値のサブスクライバ，`joint`のハンドルの初期化，`urdf`のパラメータの取得，と言った流れです．
`pid_controller_`の正体を見ておきます．

[ros_controllers/velocity_controllers/include/velocity_controllers/joint_position_controller.h](https://github.com/ros-controls/ros_controllers/blob/jade-devel/velocity_controllers/include/velocity_controllers/joint_position_controller.h)

```cpp:joint_position_controller.h
// 省略
class JointPositionController: public controller_interface::Controller<hardware_interface::VelocityJointInterface>
{
// 省略
  control_toolbox::Pid pid_controller_;       /**< Internal PID controller. */
// 省略
}
// 省略
```
このクラスの`init()`を見ます．

[control_toolbox/src/pid.cpp](https://github.com/ros-controls/control_toolbox/blob/indigo-devel/src/pid.cpp)

```cpp:pid.cpp
// 省略
bool Pid::init(const ros::NodeHandle &node, const bool quiet)
{
  ros::NodeHandle nh(node);

  Gains gains;

  // Load PID gains from parameter server
  if (!nh.getParam("p", gains.p_gain_))
  {
    if (!quiet) {
      ROS_ERROR("No p gain specified for pid.  Namespace: %s", nh.getNamespace().c_str());
    }
    return false;
  }
  // Only the P gain is required, the I and D gains are optional and default to 0:
  nh.param("i", gains.i_gain_, 0.0);
  nh.param("d", gains.d_gain_, 0.0);

  // Load integral clamp from param server or default to 0
  double i_clamp;
  nh.param("i_clamp", i_clamp, 0.0);
  gains.i_max_ = std::abs(i_clamp);
  gains.i_min_ = -std::abs(i_clamp);
  if(nh.hasParam("i_clamp_min"))
  {
    nh.param("i_clamp_min", gains.i_min_, gains.i_min_); // use i_clamp_min parameter, otherwise keep -i_clamp
    gains.i_min_ = -std::abs(gains.i_min_); // make sure the value is <= 0
  }
  if(nh.hasParam("i_clamp_max"))
  {
    nh.param("i_clamp_max", gains.i_max_, gains.i_max_); // use i_clamp_max parameter, otherwise keep i_clamp
    gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0
  }

  setGains(gains);

  reset();
  initDynamicReconfig(nh);

  return true;
}
// 省略
```

パラメータサーバから，PIDゲインを取得しています．例えば，`yaml`ファイルで設定したゲインが，ここで反映されます．

###`update()`の処理

[ros_controllers/velocity_controllers/src/joint_position_controller.cpp](https://github.com/ros-controls/ros_controllers/blob/jade-devel/velocity_controllers/src/joint_position_controller.cpp)

```cpp: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_velocity;

  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_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_velocity = pid_controller_.computeCommand(error, vel_error, period);
  }
  else
  {
    // Set the PID error and compute the PID command with nonuniform
    // time step size.
    commanded_velocity = pid_controller_.computeCommand(error, period);
  }

  joint_.setCommand(commanded_velocity);

  // 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_velocity;

      double dummy;
      getGains(controller_state_publisher_->msg_.p,
        controller_state_publisher_->msg_.i,
        controller_state_publisher_->msg_.d,
        controller_state_publisher_->msg_.i_clamp,
        dummy);
      controller_state_publisher_->unlockAndPublish();
    }
  }
  loop_count_++;
}
// 省略
```

大きな流れとしては，次のとおりです．

1. 位置の現在値を取得する
2. 位置の目標値と現在値との差分を算出する
3. 差分から速度指令値を算出する
4. 算出された速度指令値を`HardwareInterface`に反映させる
5. 現在の状態をパブリッシュする

3.について，少し深掘りします．これは，条件によって分岐がされています．

```cpp:joint_position_controller.cpp(速度指令値を算出部再掲)
// 省略
  // 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_velocity = pid_controller_.computeCommand(error, vel_error, period);
  }
  else
  {
    // Set the PID error and compute the PID command with nonuniform
    // time step size.
    commanded_velocity = pid_controller_.computeCommand(error, period);
  }
// 省略
```

**位置指令**に加え，**速度指令**も与えられているかどうかを判定しています．**速度指令**値が与えられているなら，速度偏差を計算し，**速度指令**値である`commanded_velocity`を更新しています．一方，**速度指令**値が与えられていない場合は，内部で速度偏差を計算することになります．

まずは，前者の`computeCommand()`を確認してみます．
見づらくて恐縮ですが，ブロック線図はこのようになります．

![04_velocity_controller_PositionJointInterface_has_vel.png](https://qiita-image-store.s3.amazonaws.com/0/110138/ed2dd223-581f-0d66-8f62-30d68fb6f0a7.png)

これを実現するコードを見てみましょう．

[control_toolbox/src/pid.cpp](https://github.com/ros-controls/control_toolbox/blob/indigo-devel/src/pid.cpp)

```cpp:pid.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_;

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

  // Limit i_term so that the limit is meaningful in the output
  i_term = std::max( gains.i_min_, std::min( i_term, 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;

  return cmd_;
}
```

PID制御器により，指令値を計算しています．ここでは，下記の一般的な制御則を離散計算したものが採用されています．

```math
\text{command} = K_pe + K_i \int e \; dt + K_d \frac{de}{dt}
```
微分項の差分の微分値が，速度の差分となっていました．簡単ですが，ここを一応追うと下記のとおりです．

```math
v_{curr} - v_{prev} = \frac{d}{dt}(x_{curr} - x_{prev}) = \frac{de}{dt}
```

また，積分項についてはバンド幅が定めることができ，それを超えないように閾値処理がされています．積分項が過剰に作用することで，オーバーシュートやハンチングが発生することを抑制するためです．この閾値は，例えばゲイン用の`yaml`ファイルにおいて，`i_clamp`(最大が正号付き値，最小が負号付き値)，または`i_clamp_min`と`i_clamp_max`を設定することで反映されます．

次に，後者の`computeCommand()`，すなわち偏差の微分値をPID制御器の内部で計算する場合の処理を確認します．
ブロック線図，コードの順に示します．

![03_velocity_controller_PositionJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/62bdfcff-98b2-6b95-9396-289c589ac91f.png)

[control_toolbox/src/pid.cpp](https://github.com/ros-controls/control_toolbox/blob/indigo-devel/src/pid.cpp)

```cpp:pid.cpp
// 省略
double Pid::computeCommand(double error, ros::Duration dt)
{

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

  double error_dot = d_error_;

  // Calculate the derivative error
  if (dt.toSec() > 0.0)
  {
    error_dot = (error - p_error_last_) / dt.toSec();
    p_error_last_ = error;
  }

  return computeCommand(error, error_dot, dt);
}
// 省略
```
後退差分によって，誤差の微分値を計算し，その後は前述した`computeCommand()`でPID制御器による指令値の計算を行います．

ここで，誤差の微分値を計算する際に，時間`dt.toSec()`による除算が入る点に注意が必要です．この時間は`ROS`の制御周期に依存するもので，必ずしも一定ではありません．たまたま周期が短いステップでこの計算がされた場合，速度微分値が極端に大きくなったり，その逆の現象が発生する可能性があります．真面目に制御をしたい場合は，この制御器がそのような特性を持ちうることを良く理解した上で，採用の判断をする必要がありそうです．

`hardware_interface::VelocityJointInterface`に対応する`Controller`については，以上です．

# `hardware_interface::EffortJointInterface`に対応する`Controller`
最後に，力指令の`EffortJointInterface`について述べます．これに対応する`Controller`は，次のとおりです．

- `effort_controllers::JointEffortController`
 - 他ノードから**力指令**を受け取り，ロボットの関節に対して**力指令**を送信する
- `effort_controllers::JointVelocityController`
 - 他ノードから**速度指令**を受け取り，ロボットの関節に対して**力指令**を送信する
- `effort_controllers::JointPositionController`
 - 他ノードから**位置指令**を受け取り，ロボットの関節に対して**力指令**を送信する

## `effort_controllers::JointEffortController`

[ros_controllers/effort_controllers/include/effort_controllers/joint_effort_controller.h](https://github.com/ros-controls/ros_controllers/blob/jade-devel/effort_controllers/include/effort_controllers/joint_effort_controller.h)

```cpp:joint_effort_controller.h
// 省略
typedef forward_command_controller::ForwardCommandController<hardware_interface::EffortJointInterface>
        JointEffortController;
// 省略
```
これについて，もう説明は不要です．**力指令値**を**転送(Forward)**しています．

![05_effort_controller_EffortJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/420bfa2e-3aeb-5769-6844-99c331c0e003.png)

## `effort_controllers::JointVelocityController`
`init()`は前述したものとほとんど同じなので，割愛します．
`update()`を見ます．
[ros_controllers/effort_controllers/src/joint_velocity_controller.cpp](https://github.com/ros-controls/ros_controllers/blob/jade-devel/effort_controllers/src/joint_velocity_controller.cpp)

```cpp:joint_velocity_controller.cpp
// 省略
void JointVelocityController::update(const ros::Time& time, const ros::Duration& period)
{
  double error = command_ - joint_.getVelocity();

  // Set the PID error and compute the PID command with nonuniform time
  // step size. The derivative error is computed from the change in the error
  // and the timestep dt.
  double commanded_effort = pid_controller_.computeCommand(error, period);

  joint_.setCommand(commanded_effort);

  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_;
      controller_state_publisher_->msg_.process_value = 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;
      getGains(controller_state_publisher_->msg_.p,
               controller_state_publisher_->msg_.i,
               controller_state_publisher_->msg_.d,
               controller_state_publisher_->msg_.i_clamp,
               dummy);
      controller_state_publisher_->unlockAndPublish();
    }
  }
  loop_count_++;
}
// 省略
```
速度に関する目標との差分を計算して，すぐに`computeCommand()`を実行しています．この場合，PID制御の微分項は加速度を扱うことになります．もし，`command_accelaration`などという属性があれば，`has_accelaration`というフラグで微分項の差分計算を行うことになりそうですが，現状の仕様ではそうはなっていません．`effort`はあると思うので，同様の思想で拡張はできると思いますが，当方の知識ではこの点を理論的に展開できないので，深追いはしません．
とにかく，ここは普通に後退差分により微分項の近似を行うことになります．

![06_effort_controller_PositionJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/46b1a50e-437a-0796-b62f-b24d349afb87.png)

## `effort_controllers::JointPositionController`
[ros_controllers/effort_controllers/src/joint_position_controller.cpp](https://github.com/ros-controls/ros_controllers/blob/jade-devel/effort_controllers/src/joint_position_controller.cpp)

```cpp: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_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;
      getGains(controller_state_publisher_->msg_.p,
        controller_state_publisher_->msg_.i,
        controller_state_publisher_->msg_.d,
        controller_state_publisher_->msg_.i_clamp,
        dummy);
      controller_state_publisher_->unlockAndPublish();
    }
  }
  loop_count_++;
}
// 省略
```
なんと，`velocity_controllers::JointPositionController::update()`と同一です．**速度指令**値が与えられているかどうかで，処理を切り替えています．

- **速度指令**値が**与えられている**場合
![08_effort_controller_VelocityJointInterface_has_vel.png](https://qiita-image-store.s3.amazonaws.com/0/110138/bdddf3ff-0ae3-18ab-9dc2-ac61a79f558c.png)
- **速度指令**値が**与えられていない**場合
![07_effort_controller_VelocityJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/3ec3703b-9484-b118-d208-f730bdaa58d9.png)

# 考察
以上の`Controller`は，ブロック線図が全て共通の構成をとっています．つまり，`Controller`で閉じた世界だけで考えた場合は，その出力が力か速度かなんて，**全く知ったこっちゃない**ということです．PID制御とは，そういうものです．

どんな指令値を扱いたいかは，ロボットの設計者の思想と，ハードウェアの制約で決めてください，と．実際の挙動の安定性は，`gazebo`なり実機なりで動かしながらゲイン調整しながら決めてください，ということなのです．
図で表すと，このような感じです．

もちろん，制御対象のブロックを何らかの手法で同定して，コテコテのゲイン調整をしたって構わないわけで，というか本気でやるならそうするべきなのですが，`pr2`等の既存のソースを参考にした後は，取り敢えず安定したらその値を使っちゃえばいいや，というケースが多いのではないでしょうか？（あれ，私だけ？）

## 終わりに
既存の`ros_controllers`の制御方法について深掘りをしました．`Controller`が受け取る指令値と，`HardwareInterface`が受け取る指令値によって，それぞれパターンが存在することを理解しました．

最後に，以上で得られた内容から抽出される事項をまとめます．

### 各`Controller`のパターンに対する命名ルール

- `JointHogeContoroller`の`Hoge`には，他ノードから受け取る指令値の種類が入ります．
- `hoge_controllers`の`hoge`には，ロボットの関節に送る指令値の種類が入ります．
- `JointHogeContoroller`で扱う`HogeJointInterface`の`Hoge`にも，ロボットの関節に送る指令値の種類が入ります．

### 各`Controller`のパターンに対するブロック線図
**position**_controllers
---
- Joint **Position** Controller

![01_position_controller_PositionJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/51026f5e-1b22-2b81-e374-677dea02137d.png)

**velocity**_controllers
---
- Joint **Velocity** Controller
![02_velocity_controller_VelocityJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/50703c38-f600-2f5d-6d04-87d710c416b7.png)

- Joint **Position** Controller
 - 速度指令値も**与えられた**場合
![04_velocity_controller_PositionJointInterface_has_vel.png](https://qiita-image-store.s3.amazonaws.com/0/110138/ed2dd223-581f-0d66-8f62-30d68fb6f0a7.png)
 - 速度指令値が**与えられない**場合
![03_velocity_controller_PositionJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/62bdfcff-98b2-6b95-9396-289c589ac91f.png)

**effort**_controllers
---
- Joint **Effort** Controller
![05_effort_controller_EffortJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/420bfa2e-3aeb-5769-6844-99c331c0e003.png)

- Joint **Velocity** Controller
![06_effort_controller_PositionJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/46b1a50e-437a-0796-b62f-b24d349afb87.png)

- Joint **Position** Controller
 - 速度指令値も**与えられている**場合
![08_effort_controller_VelocityJointInterface_has_vel.png](https://qiita-image-store.s3.amazonaws.com/0/110138/bdddf3ff-0ae3-18ab-9dc2-ac61a79f558c.png)
 - 速度指令値が**与えられていない**場合
![07_effort_controller_VelocityJointInterface.png](https://qiita-image-store.s3.amazonaws.com/0/110138/3ec3703b-9484-b118-d208-f730bdaa58d9.png)

上述しましたが，実機の場合には，`gazebo`がモータドライバ等に置き換わるだけで，構成自体の本質的な違いはありません．ゲインを変える必要はありますが．

さて，実際に使う場合，上図の黒いブロックは基本的にいじることが出来ないので，`Controller`のブロックを調整する必要があります．その調整するパラメータはPIDのゲインになるということです．モデルもよくわかっていないのに，制御量と操作量の次元も違うとなると，調整が大変そうだなぁ，ということは，容易に想像できます．

ですので，PIDベースで高尚な制御をしたければ，黒い部分もシステム同定して，先人の知恵をお借りして丁寧にゲイン設定をしましょう，というお話になるわけです．

まぁ，当方自身が古典制御理論に明るくなくこれ以上深追いできないという事情もありますが（！），ひとまずここまで追えれえば使う分には十分であるとは思います．まずは，色々試して慣れてみようと思います．