Note: This tutorial assumes that you have completed the previous tutorials: Communicating with a realtime joint controller.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Adding a PID to a realtime joint controller

Description: このチュートリアルはあなたにリアルタイムJointコントローラーにPIDオブジェクトを加える方法を教えます

Tutorial Level: INTERMEDIATE

Next Tutorial: Capturing data from a controller

Contents

  1. Introduction
  2. Adding the PID
    1. The code
    2. Configuration
  3. Running the controller

Introduction

このチュートリアルは我々が前のチュートリアルに作ったコントローラーをもとに作り上げます。ここで我々はコントローラーの中にPIDオブジェクトを使いスタートするでしょう。PIDコントローラーはcontrol_toolboxパッケージの一部です。

Adding the PID

The code

今我々は我々のコントローラーコードにpidコントローラーを加える必要があります。 結果のコードは下に示されます：

include/my_controller_pkg/my_controller_file.h 

   1 #include <pr2_controller_interface/controller.h>
   2 #include <pr2_mechanism_model/joint.h>
   3 #include <ros/ros.h>
   4 #include <my_controller_pkg/SetAmplitude.h>
   5 #include <control_toolbox/pid.h>
   6 
   7 
   8 namespace my_controller_ns{
   9 
  10 class MyControllerClass: public pr2_controller_interface::Controller
  11 {
  12 private:
  13   bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
  14                     my_controller_pkg::SetAmplitude::Response& resp);
  15 
  16   pr2_mechanism_model::JointState* joint_state_;
  17   double init_pos_;
  18   double amplitude_;
  19   ros::ServiceServer srv_;
  20   control_toolbox::Pid pid_controller_;
  21   pr2_mechanism_model::RobotState *robot_;
  22   ros::Time time_of_last_cycle_;
  23 
  24 public:
  25   virtual bool init(pr2_mechanism_model::RobotState *robot,
  26                     ros::NodeHandle &n);
  27   virtual void starting();
  28   virtual void update();
  29   virtual void stopping();
  30 };
  31 }

オリジナルのコードと比較して、ヘッダーは次の場所において変えました： 

   5 #include <control_toolbox/pid.h>
   6

ここで我々はpidコントローラーと我々にサービスコールを使ってその場その場でコントローラーゲインに変わることを許すであろうゲインセッターを含みます。我々がその場その場でシヌソイドの振幅を変えるために加えたサービスに非常に類似しています。 

  20   control_toolbox::Pid pid_controller_;
  21   pr2_mechanism_model::RobotState *robot_;
  22   ros::Time time_of_last_cycle_;

ここで我々はストアするべきクラス変数を加えます

  • pidオブジェクト、
  • ロボットモデルへのポインタ（これは、init(..)メソッドで、我々に与えられるポインタです。そして
  • 最後にコントローラーアップデートループが実行しました。今回、我々はアップデートループの2連続実行の間のデルタ時間を計算することができます。

次に、ここにcppファイルの新しいコードがあります： src/my_controller_file.cpp 

   1 #include "my_controller_pkg/my_controller_file.h"
   2 #include <pluginlib/class_list_macros.h>
   3 
   4 using namespace my_controller_ns;
   5 
   6 
   7 /// Controller initialization in non-realtime
   8 bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
   9                              ros::NodeHandle &n)
  10 {
  11   // get joint name
  12   std::string joint_name;
  13   if (!n.getParam("joint_name", joint_name))
  14   {
  15     ROS_ERROR("No joint given in namespace: '%s')",
  16               n.getNamespace().c_str());
  17     return false;
  18   }
  19 
  20   // get pointer to joint state
  21   joint_state_ = robot->getJointState(joint_name);
  22   if (!joint_state_)
  23   {
  24     ROS_ERROR("MyController could not find joint named '%s'",
  25               joint_name.c_str());
  26     return false;
  27   }
  28 
  29   // advertise service to set amplitude
  30   amplitude_ = 0.5;
  31   srv_ = n.advertiseService("set_amplitude",
  32                             &MyControllerClass::setAmplitude, this);
  33 
  34 
  35   // copy robot pointer so we can access time
  36   robot_ = robot;
  37 
  38   // construct pid controller
  39   if (!pid_controller_.init(ros::NodeHandle(n, "pid_parameters"))){
  40     ROS_ERROR("MyController could not construct PID controller for joint '%s'",
  41               joint_name.c_str());
  42     return false;
  43   }
  44 
  45   return true;
  46 }
  47 
  48 
  49 /// Controller startup in realtime
  50 void MyControllerClass::starting()
  51 {
  52   init_pos_ = joint_state_->position_;
  53   time_of_last_cycle_ = robot_->getTime();
  54   pid_controller_.reset();
  55 }
  56 
  57 
  58 /// Controller update loop in realtime
  59 void MyControllerClass::update()
  60 {
  61   double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
  62   double current_pos = joint_state_->position_;
  63 
  64   ros::Duration dt = robot_->getTime() - time_of_last_cycle_;
  65   time_of_last_cycle_ = robot_->getTime();
  66   joint_state_->commanded_effort_ = pid_controller_.updatePid(current_pos-desired_pos, dt);
  67 }
  68 
  69 
  70 /// Controller stopping in realtime
  71 void MyControllerClass::stopping()
  72 {}
  73 
  74 
  75 /// Service call to set amplitude of sin
  76 bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
  77                                      my_controller_pkg::SetAmplitude::Response& resp)
  78 {
  79   if (fabs(req.amplitude) < 2.0){
  80     amplitude_ = req.amplitude;
  81     ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
  82   }
  83   else
  84     ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);
  85 
  86   resp.amplitude = amplitude_;
  87   return true;
  88 }
  89 
  90 
  91 /// Register controller to pluginlib
  92 PLUGINLIB_REGISTER_CLASS(MyControllerPlugin,
  93                          my_controller_ns::MyControllerClass,
  94                          pr2_controller_interface::Controller)

cppファイルの変更は： 

  35   // copy robot pointer so we can access time
  36   robot_ = robot;

ここで我々はコピーをロボットにストアします。ロボットオブジェクトはコントローラーがトリガされる時間を得るために使われます。この時間はros::Time::now()!と異なります。ロボットモデルによって提供された時間はすべてのコントローラーのために同じであって、そしてコントローラーマネージャーがすべてのコントローラーをトリガしてスタートした時間です。 

  38   // construct pid controller
  39   if (!pid_controller_.init(ros::NodeHandle(n, "pid_parameters"))){
  40     ROS_ERROR("MyController could not construct PID controller for joint '%s'",
  41               joint_name.c_str());
  42     return false;
  43   }

ここで我々は新しいpidコントローラーを初期化します。initメソッドはpidパラメータが指定されるnamespaceを指定するNodeHandleをとります。この場合、namespaceはコントローラー名プラス文字列"pid_parameters"です。このnamespaceは我々のlaunchファイルでpid値を設定するために重要です。 

  53   time_of_last_cycle_ = robot_->getTime();
  54   pid_controller_.reset();

コントローラーのスタートのメソッドで我々はコントローラー時間をストアします、それで我々は後に2のアップデートループ間の時間における差を計算することができます。 

  60 {
  61   double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
  62   double current_pos = joint_state_->position_;
  63 
  64   ros::Duration dt = robot_->getTime() - time_of_last_cycle_;
  65   time_of_last_cycle_ = robot_->getTime();
  66   joint_state_->commanded_effort_ = pid_controller_.updatePid(current_pos-desired_pos, dt);
  67 }

これは我々が実際にpidコントローラーを使う場所です。pidはエラーシグナル、プラス時間間隔を必要とします。最初に我々は間隔"dt"、そして次に我々は実測と望ましい位置の間の差であるエラーシグナルを計算します。

/!\ pidコントローラーが負帰還を使うことに注意を払ってください (error = actual - desired)

Configuration

この新しいコードを編集するために、あなたは依存としてあなたのパッケージにcontrol_toolboxパッケージを加えます。あなたのmanifest.xmlに次のラインを加えてください： 

 <depend package="control_toolbox"/>

それで今我々はコントローラーを作る準備ができています。なぜなら我々はただ依存を加えました、そして我々はこの依存が同じくビルドを得ることを確認することを望みますから、我々はただ"make"の代わりに"rosmake"を使います 

  $ rosmake

次に、我々はpidに対してyamlファイルmy_controller.yamlにパラメータを加えます： 

  my_controller_name:
    type: MyControllerPlugin
    joint_name: r_shoulder_pan_joint
    pid_parameters:
      p: 10.0
      i: 0.0
      d: 0.0
      i_clamp: 0.0

Running the controller

前と同じように、新しいコードはリアルタイムプロセスにロードされなければなりません。あなたは同様に、gazeboのすべてをリスタートしてください： 

  $ roslaunch gazebo_worlds empty_world.launch
  $ roslaunch pr2_gazebo pr2.launch

あるいはもしgazeboがまだ走っているなら、あなたはpr2_controller_managerをライブラリのリロードを求めるために使うことができます： 

  $ rosrun pr2_controller_manager pr2_controller_manager reload-libraries

そして今、我々の新しいコントローラーをスタートさせてください： 

  $ roslaunch my_controller.launch

Wiki: ja/pr2_mechanism/Tutorials/Adding a PID to a realtime joint controller (last edited 2016-08-16 03:10:06 by Crescent)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0