TwoDofControllerDynamicsModel.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00002 
00011 #include "TwoDofControllerDynamicsModel.h"
00012 #include <iostream>
00013 #include <cmath>
00014 
00015 #define NUM_CONVOLUTION_TERM 3
00016 
00017 TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModel() {
00018   param = TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam(); // use default constructor
00019   current_time = 0;
00020   convolutions.clear();
00021   exp_sinh.clear();
00022   for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
00023     convolutions.push_back(Convolution(0.0, 0.0));
00024   }
00025   integrate_exp_sinh_current.setup(0.0, 0.0);
00026   error_prefix = ""; // inheritted from TwoDofControllerInterface
00027 }
00028 
00029 TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModel(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param, unsigned int _range) {
00030   param.alpha = _param.alpha; param.beta = _param.beta; param.ki = _param.ki; param.tc = _param.tc; param.dt = _param.dt;
00031   current_time = 0;
00032   convolutions.clear();
00033   exp_sinh.clear();
00034   for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
00035     convolutions.push_back(Convolution(_param.dt, _range));
00036   }
00037   integrate_exp_sinh_current.setup(_param.dt, _range);
00038   error_prefix = ""; // inheritted from TwoDofControllerInterface  
00039 }
00040 
00041 TwoDofControllerDynamicsModel::~TwoDofControllerDynamicsModel() {
00042 }
00043 
00044 void TwoDofControllerDynamicsModel::setup() {
00045   param.alpha = 0; param.beta = 0; param.ki = 0; param.tc = 0; param.dt = 0;
00046   convolutions.clear();
00047   exp_sinh.clear();
00048   integrate_exp_sinh_current.reset();
00049   reset();
00050 }
00051 
00052 void TwoDofControllerDynamicsModel::setup(TwoDofControllerDynamicsModel::TwoDofControllerDynamicsModelParam &_param, unsigned int _range) {
00053   param.alpha = _param.alpha; param.beta = _param.beta; param.ki = _param.ki; param.tc = _param.tc; param.dt = _param.dt;
00054   convolutions.clear();
00055   for (int i = 0; i < NUM_CONVOLUTION_TERM; i++) {
00056     convolutions.push_back(Convolution(_param.dt, _range));
00057   }
00058   integrate_exp_sinh_current.setup(_param.dt, _range);
00059   reset();
00060 }
00061 
00062 void TwoDofControllerDynamicsModel::reset() {
00063   current_time = 0;
00064   exp_sinh.clear();
00065   for (std::vector<Convolution>::iterator itr = convolutions.begin(); itr != convolutions.end(); ++itr) {
00066     (*itr).reset();
00067   }
00068   integrate_exp_sinh_current.reset();
00069 }
00070 
00071 bool TwoDofControllerDynamicsModel::getParameter() {
00072   return false;
00073 }
00074   
00075 bool TwoDofControllerDynamicsModel::getParameter(TwoDofControllerDynamicsModelParam &_p) {
00076   _p.alpha = param.alpha;
00077   _p.beta = param.beta;
00078   _p.ki = param.ki;
00079   _p.tc = param.tc;
00080   _p.dt = param.dt;
00081   return true;
00082 }
00083 
00084 double TwoDofControllerDynamicsModel::update (double _x, double _xd) {
00085   // motor model: P = -ke / s + kd + ki * s
00086   // completing the square: s^2 + (kd/ki)*s - (ke/ki) = (s+alpha)^2-beta^2
00087   
00088   double velocity; // velocity calcurated by 2 dof controller
00089 
00090   // check parameters
00091   if (!param.alpha || !param.beta || !param.tc || !param.dt) {
00092     std::cerr << "[" << error_prefix << "]" << "TwoDofControllerDynamicsModel parameters are not set." << std::endl;
00093     std::cerr << "[" << error_prefix << "]" << "alpha: " << param.alpha << ", beta: " << param.beta << ", tc: " << param.tc << ", dt: " << param.dt << std::endl;
00094     return 0;
00095   }
00096   
00097   // update exp(-a*t)*sinh(b*t) buffer
00098   double exp_sinh_current = std::exp(-param.alpha * current_time) * std::sinh(param.beta * current_time);
00099   exp_sinh.push_back(exp_sinh_current);
00100   integrate_exp_sinh_current.update(exp_sinh_current);
00101 
00102   // update convolution
00103   convolutions[0].update(exp_sinh_current, _x);
00104   convolutions[1].update(exp_sinh_current, _xd - _x);
00105   convolutions[2].update(integrate_exp_sinh_current.calculate(), _xd - _x);
00106 
00107   // 2 dof controller
00108   velocity = (1 / (param.tc * param.ki * param.beta)) * (-convolutions[0].calculate() + convolutions[1].calculate())
00109     + (1 / (param.tc * param.tc * param.ki * param.beta)) * convolutions[2].calculate();
00110 
00111   current_time += param.dt;
00112   
00113   return velocity * param.dt;
00114   
00115 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56