KalmanFilter.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "KalmanFilter.h"
00011 #include "hrpsys/util/VectorConvert.h"
00012 #include <rtm/CorbaNaming.h>
00013 #include <hrpModel/ModelLoaderUtil.h>
00014 #include <math.h>
00015 #include <hrpModel/Link.h>
00016 #include <hrpModel/Sensor.h>
00017 
00018 //#define USE_EKF
00019 
00020 // Module specification
00021 // <rtc-template block="module_spec">
00022 static const char* kalmanfilter_spec[] =
00023   {
00024     "implementation_id", "KalmanFilter",
00025     "type_name",         "KalmanFilter",
00026     "description",       "kalman filter",
00027     "version",           HRPSYS_PACKAGE_VERSION,
00028     "vendor",            "AIST",
00029     "category",          "example",
00030     "activity_type",     "DataFlowComponent",
00031     "max_instance",      "10",
00032     "language",          "C++",
00033     "lang_type",         "compile",
00034     // Configuration variables
00035     "conf.default.debugLevel", "0",
00036     ""
00037   };
00038 // </rtc-template>
00039 
00040 KalmanFilter::KalmanFilter(RTC::Manager* manager)
00041   : RTC::DataFlowComponentBase(manager),
00042     // <rtc-template block="initializer">
00043     m_rateIn("rate", m_rate),
00044     m_accIn("acc", m_acc),
00045     m_accRefIn("accRef", m_accRef),
00046     m_rpyIn("rpyIn", m_rate),
00047     m_qCurrentIn("qCurrent", m_qCurrent),
00048     m_rpyOut("rpy", m_rpy),
00049     m_rpyRawOut("rpy_raw", m_rpyRaw),
00050     m_baseRpyCurrentOut("baseRpyCurrent", m_baseRpyCurrent),
00051     m_KalmanFilterServicePort("KalmanFilterService"),
00052     // </rtc-template>
00053     m_robot(hrp::BodyPtr()),
00054     m_debugLevel(0),
00055     dummy(0),
00056     loop(0)
00057 {
00058   m_service0.kalman(this);
00059 }
00060 
00061 KalmanFilter::~KalmanFilter()
00062 {
00063 }
00064 
00065 
00066 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00067 RTC::ReturnCode_t KalmanFilter::onInitialize()
00068 {
00069   std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00070   // <rtc-template block="bind_config">
00071   // Bind variables and configuration variable
00072   bindParameter("debugLevel", m_debugLevel, "0");
00073   
00074   // </rtc-template>
00075 
00076   // Registration: InPort/OutPort/Service
00077   // <rtc-template block="registration">
00078   // Set InPort buffers
00079   addInPort("rate", m_rateIn);
00080   addInPort("acc", m_accIn);
00081   addInPort("accRef", m_accRefIn);
00082   addInPort("rpyIn", m_rpyIn);
00083   addInPort("qCurrent", m_qCurrentIn);
00084 
00085   // Set OutPort buffer
00086   addOutPort("rpy", m_rpyOut);
00087   addOutPort("rpy_raw", m_rpyRawOut);
00088   addOutPort("baseRpyCurrent", m_baseRpyCurrentOut);
00089 
00090   // Set service provider to Ports
00091   m_KalmanFilterServicePort.registerProvider("service0", "KalmanFilterService", m_service0);
00092   
00093   // Set service consumers to Ports
00094   
00095   // Set CORBA Service Ports
00096   addPort(m_KalmanFilterServicePort);
00097   
00098   // </rtc-template>
00099 
00100   // Setup robot model
00101   RTC::Properties& prop = getProperties();
00102   if ( ! coil::stringTo(m_dt, prop["dt"].c_str()) ) {
00103     std::cerr << "[" << m_profile.instance_name << "]failed to get dt" << std::endl;
00104     return RTC::RTC_ERROR;
00105   }
00106 
00107   m_robot = hrp::BodyPtr(new hrp::Body());
00108 
00109   RTC::Manager& rtcManager = RTC::Manager::instance();
00110   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00111   int comPos = nameServer.find(",");
00112   if (comPos < 0){
00113     comPos = nameServer.length();
00114   }
00115   nameServer = nameServer.substr(0, comPos);
00116   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00117   if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
00118                                CosNaming::NamingContext::_duplicate(naming.getRootContext())
00119                                )){
00120     std::cerr << "[" << m_profile.instance_name << "]failed to load model[" << prop["model"] << "]" << std::endl;
00121   }
00122 
00123   m_rpy.data.r = 0;
00124   m_rpy.data.p = 0;
00125   m_rpy.data.y = 0;
00126 
00127   if (m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) {
00128     hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
00129     m_sensorR = sensor->link->R * sensor->localR;
00130   } else {
00131     m_sensorR = hrp::Matrix33::Identity();
00132   }
00133   rpy_kf.setParam(m_dt, 0.001, 0.003, 1000, std::string(m_profile.instance_name));
00134   rpy_kf.setSensorR(m_sensorR);
00135   ekf_filter.setdt(m_dt);
00136   kf_algorithm = OpenHRP::KalmanFilterService::RPYKalmanFilter;
00137   m_qCurrent.data.length(m_robot->numJoints());
00138   acc_offset = hrp::Vector3::Zero();
00139   sensorR_offset = hrp::Matrix33::Identity();
00140 
00141   return RTC::RTC_OK;
00142 }
00143 
00144 
00145 
00146 /*
00147   RTC::ReturnCode_t KalmanFilter::onFinalize()
00148   {
00149   return RTC::RTC_OK;
00150   }
00151 */
00152 
00153 /*
00154   RTC::ReturnCode_t KalmanFilter::onStartup(RTC::UniqueId ec_id)
00155   {
00156   return RTC::RTC_OK;
00157   }
00158 */
00159 
00160 /*
00161   RTC::ReturnCode_t KalmanFilter::onShutdown(RTC::UniqueId ec_id)
00162   {
00163   return RTC::RTC_OK;
00164   }
00165 */
00166 
00167 RTC::ReturnCode_t KalmanFilter::onActivated(RTC::UniqueId ec_id)
00168 {
00169   std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00170   return RTC::RTC_OK;
00171 }
00172 
00173 RTC::ReturnCode_t KalmanFilter::onDeactivated(RTC::UniqueId ec_id)
00174 {
00175   std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00176   return RTC::RTC_OK;
00177 }
00178 
00179 RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id)
00180 {
00181   loop++;
00182   static int initialize = 0;
00183   //std::cerr << m_profile.instance_name<< ": onExecute(" << ec_id << ") " << std::endl;
00184   if (m_rpyIn.isNew() ) {
00185     m_rpyIn.read();
00186     m_rpy.data.r = m_rate.data.avx;
00187     m_rpy.data.p = m_rate.data.avy;
00188     m_rpy.data.y = m_rate.data.avz;
00189     m_rpy.tm = m_rate.tm;
00190     m_rpyOut.write();
00191     return RTC::RTC_OK;
00192   }
00193   if (m_rateIn.isNew()){
00194     m_rateIn.read();
00195   }
00196   if (m_qCurrentIn.isNew()) {
00197       m_qCurrentIn.read();
00198       for ( int i = 0; i < m_robot->numJoints(); i++ ){
00199           m_robot->joint(i)->q = m_qCurrent.data[i];
00200       }
00201   }
00202   double sx_ref = 0.0, sy_ref = 0.0, sz_ref = 0.0;
00203   if (m_accRefIn.isNew()){
00204     m_accRefIn.read();
00205     sx_ref = m_accRef.data.ax, sy_ref = m_accRef.data.ay, sz_ref = m_accRef.data.az;
00206   }
00207   if (m_accIn.isNew()){
00208     m_accIn.read();
00209 
00210     Eigen::Vector3d acc = m_sensorR * hrp::Vector3(m_acc.data.ax-sx_ref+acc_offset(0), m_acc.data.ay-sy_ref+acc_offset(1), m_acc.data.az-sz_ref+acc_offset(2)); // transform to imaginary acc data
00211     acc = sensorR_offset * acc;
00212     Eigen::Vector3d gyro = m_sensorR * hrp::Vector3(m_rate.data.avx, m_rate.data.avy, m_rate.data.avz); // transform to imaginary rate data
00213     gyro = sensorR_offset * gyro;
00214     if (DEBUGP) {
00215         std::cerr << "[" << m_profile.instance_name << "] raw data acc : " << std::endl << acc << std::endl;
00216         std::cerr << "[" << m_profile.instance_name << "] raw data gyro : " << std::endl << gyro << std::endl;
00217     }
00218     hrp::Vector3 rpy, rpyRaw, baseRpyCurrent;
00219     if (kf_algorithm == OpenHRP::KalmanFilterService::QuaternionExtendedKalmanFilter) {
00220         ekf_filter.main_one(rpy, rpyRaw, acc, gyro);
00221     } else if (kf_algorithm == OpenHRP::KalmanFilterService::RPYKalmanFilter) {
00222         double sl_y;
00223         hrp::Matrix33 BtoS;
00224         m_robot->calcForwardKinematics();
00225         if (m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) {
00226             hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
00227             sl_y = hrp::rpyFromRot(sensor->link->R)[2];
00228             BtoS = (m_robot->rootLink()->R).transpose() * (sensor->link->R * sensor->localR);
00229         } else {
00230             sl_y = 0.0;
00231             BtoS = (m_robot->rootLink()->R).transpose();
00232         }
00233         rpy_kf.main_one(rpy, rpyRaw, baseRpyCurrent, acc, gyro, sl_y, BtoS);
00234     }
00235     m_rpyRaw.data.r = rpyRaw(0);
00236     m_rpyRaw.data.p = rpyRaw(1);
00237     m_rpyRaw.data.y = rpyRaw(2);
00238     m_rpy.data.r = rpy(0);
00239     m_rpy.data.p = rpy(1);
00240     m_rpy.data.y = rpy(2);
00241     m_baseRpyCurrent.data.r = baseRpyCurrent(0);
00242     m_baseRpyCurrent.data.p = baseRpyCurrent(1);
00243     m_baseRpyCurrent.data.y = baseRpyCurrent(2);
00244     // add time stamp
00245     m_rpyRaw.tm = m_acc.tm;
00246     m_rpy.tm = m_acc.tm;
00247     m_baseRpyCurrent.tm = m_acc.tm;
00248 
00249     m_rpyOut.write();
00250     m_rpyRawOut.write();
00251     m_baseRpyCurrentOut.write();
00252   }
00253   return RTC::RTC_OK;
00254 }
00255 
00256 /*
00257   RTC::ReturnCode_t KalmanFilter::onAborting(RTC::UniqueId ec_id)
00258   {
00259   return RTC::RTC_OK;
00260   }
00261 */
00262 
00263 /*
00264   RTC::ReturnCode_t KalmanFilter::onError(RTC::UniqueId ec_id)
00265   {
00266   return RTC::RTC_OK;
00267   }
00268 */
00269 
00270 /*
00271   RTC::ReturnCode_t KalmanFilter::onReset(RTC::UniqueId ec_id)
00272   {
00273   return RTC::RTC_OK;
00274   }
00275 */
00276 
00277 /*
00278   RTC::ReturnCode_t KalmanFilter::onStateUpdate(RTC::UniqueId ec_id)
00279   {
00280   return RTC::RTC_OK;
00281   }
00282 */
00283 
00284 /*
00285   RTC::ReturnCode_t KalmanFilter::onRateChanged(RTC::UniqueId ec_id)
00286   {
00287   return RTC::RTC_OK;
00288   }
00289 */
00290 
00291 bool KalmanFilter::setKalmanFilterParam(const OpenHRP::KalmanFilterService::KalmanFilterParam& i_param)
00292 {
00293     std::cerr << "[" << m_profile.instance_name << "] setKalmanFilterParam" << std::endl;
00294     rpy_kf.setParam(m_dt, i_param.Q_angle, i_param.Q_rate, i_param.R_angle, std::string(m_profile.instance_name));
00295     kf_algorithm = i_param.kf_algorithm;
00296     for (size_t i = 0; i < 3; i++) {
00297         acc_offset(i) = i_param.acc_offset[i];
00298     }
00299     hrp::Vector3 rpyoff;
00300     for (size_t i = 0; i < 3; i++) {
00301         rpyoff(i) = i_param.sensorRPY_offset[i];
00302     }
00303     sensorR_offset = hrp::rotFromRpy(rpyoff);
00304     std::cerr << "[" << m_profile.instance_name << "]   kf_algorithm=" << (kf_algorithm==OpenHRP::KalmanFilterService::RPYKalmanFilter?"RPYKalmanFilter":"QuaternionExtendedKalmanFilter") << std::endl;
00305     std::cerr << "[" << m_profile.instance_name << "]   acc_offset = " << acc_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
00306     std::cerr << "[" << m_profile.instance_name << "]   sensorRPY_offset = " << rpyoff.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
00307     return true;
00308 }
00309 
00310 bool KalmanFilter::resetKalmanFilterState()
00311 {
00312     rpy_kf.resetKalmanFilterState();
00313     ekf_filter.resetKalmanFilterState();
00314 };
00315 
00316 bool KalmanFilter::getKalmanFilterParam(OpenHRP::KalmanFilterService::KalmanFilterParam& i_param)
00317 {
00318   i_param.Q_angle = rpy_kf.getQangle();
00319   i_param.Q_rate = rpy_kf.getQrate();
00320   i_param.R_angle = rpy_kf.getRangle();
00321   i_param.kf_algorithm = kf_algorithm;
00322   for (size_t i = 0; i < 3; i++) {
00323       i_param.acc_offset[i] = acc_offset(i);
00324   }
00325   hrp::Vector3 rpyoff = hrp::rpyFromRot(sensorR_offset);
00326   for (size_t i = 0; i < 3; i++) {
00327       i_param.sensorRPY_offset[i] = rpyoff(i);
00328   }
00329   return true;
00330 }
00331 
00332 extern "C"
00333 {
00334 
00335   void KalmanFilterInit(RTC::Manager* manager)
00336   {
00337     RTC::Properties profile(kalmanfilter_spec);
00338     manager->registerFactory(profile,
00339                              RTC::Create<KalmanFilter>,
00340                              RTC::Delete<KalmanFilter>);
00341   }
00342 
00343 };
00344 
00345 


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