AccelerationFilter.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00007 #include "AccelerationFilter.h"
00008 
00009 typedef coil::Guard<coil::Mutex> Guard;
00010 // Module specification
00011 // <rtc-template block="module_spec">
00012 static const char* accelerationfilter_spec[] =
00013   {
00014     "implementation_id", "AccelerationFilter",
00015     "type_name",         "AccelerationFilter",
00016     "description",       "Acceleration Filter component",
00017     "version",           "1.0",
00018     "vendor",            "JSK",
00019     "category",          "example",
00020     "activity_type",     "SPORADIC",
00021     "kind",              "DataFlowComponent",
00022     "max_instance",      "10",
00023     "language",          "C++",
00024     "lang_type",         "compile",
00025     // Configuration variables
00026     ""
00027   };
00028 // </rtc-template>
00029 
00030 AccelerationFilter::AccelerationFilter(RTC::Manager* manager)
00031     // <rtc-template block="initializer">
00032     : RTC::DataFlowComponentBase(manager),
00033       m_accInIn("accIn", m_accIn),
00034       m_rateInIn("rateIn", m_rateIn),
00035       m_rpyInIn("rpyIn", m_rpyIn),
00036       m_posInIn("posIn", m_posIn),
00037       m_velOutOut("velOut", m_velOut),
00038       //m_posOutOut("posOut", m_posOut),
00039       m_AccelerationFilterServicePort("AccelerationFilterService"),
00040     // </rtc-template>
00041       m_use_filter_bool(false)
00042 {
00043 }
00044 
00045 AccelerationFilter::~AccelerationFilter()
00046 {
00047 }
00048 
00049 
00050 RTC::ReturnCode_t AccelerationFilter::onInitialize()
00051 {
00052     // Registration: InPort/OutPort/Service
00053     // <rtc-template block="registration">
00054     // Set InPort buffers
00055     addInPort("accIn", m_accInIn);
00056     addInPort("rateIn", m_rateInIn);
00057     addInPort("rpyIn", m_rpyInIn);
00058     addInPort("posIn", m_posInIn);
00059 
00060     // Set OutPort buffer
00061     addOutPort("velOut", m_velOutOut);
00062     //addOutPort("posOut", m_posOutOut);
00063 
00064     // Set service provider to Ports
00065     m_AccelerationFilterServicePort.registerProvider("service0", "AccelerationFilterService", m_service0);
00066     m_service0.setInstance(this);
00067 
00068     // Set service consumers to Ports
00069 
00070     // Set CORBA Service Ports
00071     addPort(m_AccelerationFilterServicePort);
00072 
00073     // </rtc-template>
00074 
00075     // <rtc-template block="bind_config">
00076     // Bind variables and configuration variable
00077     RTC::Properties& prop = getProperties();
00078     if ( ! coil::stringTo(m_dt, prop["dt"].c_str()) ) {
00079         std::cerr << "[" << m_profile.instance_name << "] failed to get dt" << std::endl;
00080         return RTC::RTC_ERROR;
00081     }
00082 
00083     // read gravity param
00084     double param_gravity = 9.80665;
00085     if ( ! coil::stringTo(m_gravity, prop["gravity"].c_str()) ) {
00086         param_gravity = m_gravity = 9.80665;
00087     }
00088     std::cerr << "[" << m_profile.instance_name << "] gravity : " << m_gravity << std::endl;
00089 
00090     // read filter param
00091     {
00092         coil::vstring filter_str = coil::split(prop["iir_filter_setting"], ",");
00093         if (filter_str.size() > 2) {
00094             int dim = (filter_str.size() - 1)/2;
00095             std::vector<double> bb;
00096             std::vector<double> aa;
00097             for(int i = 0; i < dim + 1; i++) {
00098                 double val = -1;
00099                 coil::stringTo(val, filter_str[i].c_str());
00100                 bb.push_back(val);
00101             }
00102             for(int i = 0; i < filter_str.size() - dim - 1; i++) {
00103                 double val = -1;
00104                 coil::stringTo(val, filter_str[dim+1+i].c_str());
00105                 aa.push_back(val);
00106             }
00107             if (aa.size() > 0 && bb.size() > 0) {
00108                 m_use_filter_bool = true;
00109                 std::cerr << "[" << m_profile.instance_name << "] pass filter_param : " << std::endl;
00110                 std::cerr << "B = [";
00111                 for(int i = 0; i < bb.size(); i++) {
00112                     std::cerr << " " << bb[i];
00113                 }
00114                 std::cerr << "]" << std::endl;
00115                 std::cerr << "A = [";
00116                 for(int i = 0; i < aa.size(); i++) {
00117                     std::cerr << " " << aa[i];
00118                 }
00119                 std::cerr << "]" << std::endl;
00120                 for (int i = 0; i < 3; i++) {
00121                     IIRFilterPtr fl(new IIRFilter(std::string(m_profile.instance_name)));
00122                     fl->setParameter(dim, aa, bb);
00123                     fl->reset(param_gravity);
00124                     m_filters.push_back(fl);
00125                 }
00126             }
00127         }
00128     }
00129 
00130     // </rtc-template>
00131     return RTC::RTC_OK;
00132 }
00133 
00134 
00135 /*
00136 RTC::ReturnCode_t AccelerationFilter::onFinalize()
00137 {
00138   return RTC::RTC_OK;
00139 }
00140 */
00141 /*
00142 RTC::ReturnCode_t AccelerationFilter::onStartup(RTC::UniqueId ec_id)
00143 {
00144   return RTC::RTC_OK;
00145 }
00146 */
00147 /*
00148 RTC::ReturnCode_t AccelerationFilter::onShutdown(RTC::UniqueId ec_id)
00149 {
00150   return RTC::RTC_OK;
00151 }
00152 */
00153 
00154 RTC::ReturnCode_t AccelerationFilter::onActivated(RTC::UniqueId ec_id)
00155 {
00156     std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00157     return RTC::RTC_OK;
00158 }
00159 RTC::ReturnCode_t AccelerationFilter::onDeactivated(RTC::UniqueId ec_id)
00160 {
00161     std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00162     // reset filter
00163     return RTC::RTC_OK;
00164 }
00165 
00166 
00167 RTC::ReturnCode_t AccelerationFilter::onExecute(RTC::UniqueId ec_id)
00168 {
00169     if (m_rpyInIn.isNew()) {
00170         m_rpyInIn.read();
00171     }
00172     if (m_rateInIn.isNew()) {
00173         m_rateInIn.read();
00174     }
00175     // calc expected velocity from AutoBalancer
00176     hrp::Vector3 expected_vel;
00177     if (m_posInIn.isNew()) {
00178         m_posInIn.read();
00179         hrp::Vector3 pos(m_posIn.data.x, m_posIn.data.y, m_posIn.data.z);
00180         expected_vel = pos - m_previous_pos;
00181         expected_vel /= m_dt;
00182         m_previous_pos = pos;
00183     }
00184 
00185     //
00186     if (m_accInIn.isNew()) {
00187         Guard guard(m_mutex);
00188 
00189         m_accInIn.read();
00190         hrp::Vector3 acc(m_accIn.data.ax, m_accIn.data.ay, m_accIn.data.az);
00191         hrp::Matrix33 imuR = hrp::rotFromRpy(m_rpyIn.data.r,
00192                                              m_rpyIn.data.p,
00193                                              m_rpyIn.data.y);
00194         hrp::Vector3 gravity(0, 0, - m_gravity);
00195         hrp::Vector3 acc_wo_g = imuR * acc + gravity;
00196 
00197         for (int i = 0; i < 3; i++) {
00198             if (m_use_filter_bool) {
00199                 double filtered_acc =  m_filters[i]->passFilter(acc_wo_g[i]);
00200                 m_global_vel[i] += filtered_acc * m_dt;
00201             } else {
00202                 m_global_vel[i] += acc_wo_g[i] * m_dt;
00203             }
00204         }
00205 
00206         hrp::Vector3 _result_vel = imuR.inverse() * m_global_vel; // result should be described in sensor coords
00207 
00208         m_velOut.data.x  = _result_vel[0];
00209         m_velOut.data.y  = _result_vel[1];
00210         m_velOut.data.z  = _result_vel[2];
00211         m_velOutOut.write();
00212     }
00213 
00214     return RTC::RTC_OK;
00215 }
00216 
00217 /*
00218 RTC::ReturnCode_t AccelerationFilter::onAborting(RTC::UniqueId ec_id)
00219 {
00220   return RTC::RTC_OK;
00221 }
00222 */
00223 /*
00224 RTC::ReturnCode_t AccelerationFilter::onError(RTC::UniqueId ec_id)
00225 {
00226   return RTC::RTC_OK;
00227 }
00228 */
00229 /*
00230 RTC::ReturnCode_t AccelerationFilter::onReset(RTC::UniqueId ec_id)
00231 {
00232   return RTC::RTC_OK;
00233 }
00234 */
00235 /*
00236 RTC::ReturnCode_t AccelerationFilter::onStateUpdate(RTC::UniqueId ec_id)
00237 {
00238   return RTC::RTC_OK;
00239 }
00240 */
00241 /*
00242 RTC::ReturnCode_t AccelerationFilter::onRateChanged(RTC::UniqueId ec_id)
00243 {
00244   return RTC::RTC_OK;
00245 }
00246 */
00247 
00248 bool AccelerationFilter::resetFilter(const OpenHRP::AccelerationFilterService::ControlMode &mode,
00249                                      const double *vel)
00250 {
00251     Guard guard(m_mutex);
00252     switch(mode) {
00253     case OpenHRP::AccelerationFilterService::MODE_ZERO_VELOCITY:
00254         m_global_vel[0] = 0;
00255         m_global_vel[1] = 0;
00256         m_global_vel[2] = 0;
00257         break;
00258     case OpenHRP::AccelerationFilterService::MODE_RELATIVE_GLOBAL_VELOCITY:
00259         m_global_vel[0] += vel[0];
00260         m_global_vel[1] += vel[1];
00261         m_global_vel[2] += vel[2];
00262         break;
00263     case OpenHRP::AccelerationFilterService::MODE_ABSOLUTE_GLOBAL_VELOCITY:
00264         m_global_vel[0] = vel[0];
00265         m_global_vel[1] = vel[1];
00266         m_global_vel[2] = vel[2];
00267         break;
00268     case OpenHRP::AccelerationFilterService::MODE_RELATIVE_LOCAL_VELOCITY:
00269         {
00270             hrp::Matrix33 imuR = hrp::rotFromRpy(m_rpyIn.data.r,
00271                                                  m_rpyIn.data.p,
00272                                                  m_rpyIn.data.y);
00273             hrp::Vector3 in_vel(vel[0], vel[1], vel[2]);
00274             hrp::Vector3 g_vel = imuR * in_vel;
00275             m_global_vel += g_vel;
00276         }
00277         break;
00278     case OpenHRP::AccelerationFilterService::MODE_ABSOLUTE_LOCAL_VELOCITY:
00279         {
00280             hrp::Matrix33 imuR = hrp::rotFromRpy(m_rpyIn.data.r,
00281                                                  m_rpyIn.data.p,
00282                                                  m_rpyIn.data.y);
00283             hrp::Vector3 in_vel(vel[0], vel[1], vel[2]);
00284             hrp::Vector3 g_vel = imuR * in_vel;
00285             m_global_vel = g_vel;
00286         }
00287         break;
00288     default:
00289         break;
00290     }
00291     return true;
00292 }
00293 
00294 bool AccelerationFilter::setParam(const ::OpenHRP::AccelerationFilterService::AccelerationFilterParam& i_param)
00295 {
00296     Guard guard(m_mutex);
00297     m_gravity = i_param.gravity;
00298 
00299     if(i_param.filter_param.length() > 1) {
00300         int dim;
00301         std::vector<double> A;
00302         std::vector<double> B;
00303         dim = (i_param.filter_param.length() - 1)/2;
00304         for(int i = 0; i < dim + 1; i++) {
00305             B.push_back(i_param.filter_param[i]);
00306         }
00307         for(int i = 0; i < i_param.filter_param.length() - dim - 1; i++) {
00308             A.push_back(i_param.filter_param[dim+1+i]);
00309         }
00310         m_filters.resize(0);
00311         for(int i = 0; i < 3; i++) {
00312             IIRFilterPtr fl(new IIRFilter);
00313             fl->setParameter(dim, A, B);
00314             m_filters.push_back(fl);
00315         }
00316         m_use_filter_bool = i_param.use_filter;
00317     }
00318     return true;
00319 }
00320 
00321 bool AccelerationFilter::getParam(::OpenHRP::AccelerationFilterService::AccelerationFilterParam &i_param)
00322 {
00323     i_param.gravity = m_gravity;
00324     i_param.use_filter = m_use_filter_bool;
00325     if(m_filters.size() > 0) {
00326         int dim;
00327         std::vector<double> A;
00328         std::vector<double> B;
00329         m_filters[0]->getParameter(dim, A, B);
00330         i_param.filter_param.length(2*(dim+1));
00331         for(int i = 0; i < dim+1; i++) {
00332             i_param.filter_param[i] = B[i];
00333             i_param.filter_param[i + dim + 1] = A[i];
00334         }
00335     }
00336     return true;
00337 }
00338 
00339 extern "C"
00340 {
00341   void AccelerationFilterInit(RTC::Manager* manager)
00342   {
00343     coil::Properties profile(accelerationfilter_spec);
00344     manager->registerFactory(profile,
00345                              RTC::Create<AccelerationFilter>,
00346                              RTC::Delete<AccelerationFilter>);
00347   }
00348 };
00349 
00350 
00351 


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