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


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19