Added TwoDofControllerDynamicsModel, using dynamic model based on equ…

…ation of motion

rtc/TorqueController/MotorTorqueController.cpp

@@ -36,6 +36,12 @@ MotorTorqueController::MotorTorqueController(std::string _jname, double _ke, dou
  setupControllerCommon(_jname, _dt);
 }
 
+MotorTorqueController::MotorTorqueController(std::string _jname, double _alpha, double _beta, double _ki, double _tc, double _dt)
+{
+ setupController(_alpha, _beta, _ki, _dt);
+ setupControllerCommon(_jname, _dt);
+}
+
 MotorTorqueController::~MotorTorqueController(void)
 {
 }
@@ -52,6 +58,12 @@ void MotorTorqueController::setupController(double _ke, double _kd, double _tc,
  m_emergencyController.setupTwoDofControllerPDModel(_ke, _kd, _tc, _dt);
 }
 
+void MotorTorqueController::setupController(double _alpha, double _beta, double _ki, double _tc, double _dt)
+{
+ m_normalController.setupTwoDofControllerDynamicsModel(_alpha, _beta, _ki, _tc, _dt);
+ m_emergencyController.setupTwoDofControllerDynamicsModel(_alpha, _beta, _ki, _tc, _dt);
+}
+
 bool MotorTorqueController::activate(void)
 {
  bool retval = false;
@@ -235,6 +247,12 @@ void MotorTorqueController::MotorController::setupTwoDofControllerPDModel(double
  controller->reset();
 }
 
+void MotorTorqueController::MotorController::setupTwoDofControllerDynamicsModel(double _alpha, double _beta, double _ki, double _tc, double _dt)
+{
+ controller.reset(new TwoDofControllerDynamicsModel(_alpha, _beta, _ki, _tc, _dt));
+ controller->reset();
+}
+
 double MotorTorqueController::MotorController::getMotorControllerDq(void)
 {
  double ret_dq;

rtc/TorqueController/MotorTorqueController.h

@@ -14,6 +14,7 @@
 #include <boost/shared_ptr.hpp>
 #include "../Stabilizer/TwoDofController.h"
 #include "TwoDofControllerPDModel.h"
+#include "TwoDofControllerDynamicsModel.h"
 
 // </rtc-template>
 
@@ -28,10 +29,12 @@ class MotorTorqueController {
  MotorTorqueController();
  MotorTorqueController(std::string _jname, double ke, double tc, double dt);
  MotorTorqueController(std::string _jname, double ke, double kd, double tc, double dt);
+ MotorTorqueController(std::string _jname, double _alpha, double _beta, double _ki, double _tc, double _dt);
  ~MotorTorqueController(void);
 
  void setupController(double _ke, double _tc, double _dt);
  void setupController(double _ke, double _kd, double _tc, double _dt);
+ void setupController(double _alpha, double _beta, double _ki, double _tc, double _dt);
  bool activate(void); // set state of torque controller to ACTIVE
  bool deactivate(void); // set state of torque controller to STOP -> INACTIVE
  bool setReferenceTorque(double _tauRef); // set reference torque (does not activate controller)
@@ -58,6 +61,7 @@ class MotorTorqueController {
  double recovery_dq; // last difference of joint angle from qRef (dq + transition_dq) when state was changed to STOP
  void setupTwoDofController(double _ke, double _tc, double _dt);
  void setupTwoDofControllerPDModel(double _ke, double _kd, double _tc, double _dt);
+ void setupTwoDofControllerDynamicsModel(double _alpha, double _beta, double _ki, double _tc, double _dt);
  double getMotorControllerDq(void); // get according dq according to state
  };
 

rtc/TorqueController/TwoDofControllerDynamicsModel.cpp

@@ -0,0 +1,88 @@
+// -*- C++ -*-
+
+/*!
+ * @file TwoDofControllerPDModel.cpp
+ * @brief Feedback and Feedforward Controller which use PDModel as motor model
+ * @date $Date$
+ *
+ * $Id$
+ */
+
+#include "TwoDofControllerDynamicsModel.h"
+#include <iostream>
+#include <cmath>
+
+#define NUM_CONVOLUTION_TERM 3
+
+TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModel(double _alpha, double _beta, double _ki, double _tc, double _dt, unsigned int _range) {
+ alpha = _alpha; beta = _beta; ki = _ki; tc = _tc; dt = _dt;
+ current_time = 0;
+ convolutions.clear();
+ exp_sin.clear();
+ for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
+ convolutions.push_back(Convolution(_dt, _range));
+ }
+ integrate_exp_sin_current.setup(_dt, _range);
+}
+
+TwoDofControllerDynamicsModel::~TwoDofControllerDynamicsModel() {
+}
+
+void TwoDofControllerDynamicsModel::setup() {
+ alpha = 0; beta = 0; ki = 0; tc = 0; dt = 0;
+ convolutions.clear();
+ exp_sin.clear();
+ integrate_exp_sin_current.reset();
+}
+
+void TwoDofControllerDynamicsModel::setup(double _alpha, double _beta, double _ki, double _tc, double _dt, unsigned int _range) {
+ alpha = _alpha; beta = _beta; ki = _ki; tc = _tc; dt = _dt;
+ convolutions.clear();
+ for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
+ convolutions.push_back(Convolution(_dt, _range));
+ }
+ integrate_exp_sin_current.setup(_dt, _range);
+ reset();
+}
+
+void TwoDofControllerDynamicsModel::reset() {
+ current_time = 0;
+ exp_sin.clear();
+ for (std::vector<Convolution>::iterator itr = convolutions.begin(); itr != convolutions.end(); ++itr) {
+ (*itr).reset();
+ }
+ integrate_exp_sin_current.reset();
+}
+
+double TwoDofControllerDynamicsModel::update (double _x, double _xd) {
+ // motor model: P = -ke / s + kd + ki * s
+ // completing the square: s^2 + (kd/ki)*s - (ke/ki) = (s+alpha)^2-beta^2
+
+ double velocity; // velocity calcurated by 2 dof controller
+
+ // check parameters
+ if (!alpha || !beta || !tc || !dt) {
+ std::cerr << "ERROR: parameters are not set." << std::endl;
+ std::cerr << "alpha: " << alpha << ", beta: " << beta << ", tc: " << tc << ", dt: " << dt << std::endl;
+ return 0;
+ }
+
+ // update exp(-a*t)*sin(b*t) buffer
+ double exp_sin_current = std::exp(-alpha * current_time) * std::sinh(beta * current_time);
+ exp_sin.push_back(exp_sin_current);
+ integrate_exp_sin_current.update(exp_sin_current);
+
+ // update convolution
+ convolutions[0].update(exp_sin_current, _x);
+ convolutions[1].update(exp_sin_current, _xd - _x);
+ convolutions[2].update(integrate_exp_sin_current.calculate(), _xd - _x);
+
+ // 2 dof controller
+ velocity = (1 / (tc * ki * beta)) * (-convolutions[0].calculate() + convolutions[1].calculate())
+ + (1 / (tc * tc * ki * beta)) * convolutions[2].calculate();
+
+ current_time += dt;
+
+ return velocity * dt;
+
+}

rtc/TorqueController/TwoDofControllerDynamicsModel.h

@@ -0,0 +1,35 @@
+// -*- C++ -*-
+/*!
+ * @file TwoDofControllerPDModel.h
+ * @brief Feedback and Feedforward Controller which use PDModel as motor model
+ * @date $Date$
+ *
+ * $Id$
+ */
+
+#ifndef TWO_DOF_CONTROLLER_DYNAMICS_MODEL_H
+#define TWO_DOF_CONTROLLER_DYNAMICS_MODEL_H
+
+// </rtc-template>
+
+#include "../Stabilizer/TwoDofController.h"
+#include "Convolution.h"
+#include <vector>
+
+class TwoDofControllerDynamicsModel : public TwoDofControllerInterface {
+public:
+ TwoDofControllerDynamicsModel(double _alpha = 0, double _beta = 0, double _ki = 0, double _tc = 0, double _dt = 0, unsigned int _range = 0);
+ ~TwoDofControllerDynamicsModel();
+ void setup();
+ void setup(double _alpha, double _beta, double _ki, double _tc, double _dt, unsigned int _range = 0);
+ void reset();
+ double update(double _x, double _xd);
+private:
+ double alpha, beta, ki, tc, dt; // alpha, beta: completing square, ki: Inertia, tc: time constant, dt: control cycle
+ double current_time;
+ Integrator integrate_exp_sin_current;
+ std::vector<double> exp_sin;
+ std::vector<Convolution> convolutions;
+};
+
+#endif // TWO_DOF_CONTROLLER_WITH_DAMPER_H