00001
00010 #include "TorqueFilter.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/ModelLoaderUtil.h>
00013 #include <hrpUtil/MatrixSolvers.h>
00014
00015 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00016
00017
00018
00019 static const char* torquefilter_spec[] =
00020 {
00021 "implementation_id", "TorqueFilter",
00022 "type_name", "TorqueFilter",
00023 "description", "null component",
00024 "version", HRPSYS_PACKAGE_VERSION,
00025 "vendor", "AIST",
00026 "category", "example",
00027 "activity_type", "DataFlowComponent",
00028 "max_instance", "10",
00029 "language", "C++",
00030 "lang_type", "compile",
00031
00032 "conf.default.debugLevel", "0",
00033 ""
00034 };
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 TorqueFilter::TorqueFilter(RTC::Manager* manager)
00052 : RTC::DataFlowComponentBase(manager),
00053
00054 m_qCurrentIn("qCurrent", m_qCurrent),
00055 m_tauInIn("tauIn", m_tauIn),
00056 m_tauOutOut("tauOut", m_tauOut),
00057
00058 m_debugLevel(0),
00059 m_is_gravity_compensation(false)
00060 {
00061 }
00062
00063 TorqueFilter::~TorqueFilter()
00064 {
00065 }
00066
00067 RTC::ReturnCode_t TorqueFilter::onInitialize()
00068 {
00069 std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00070
00071
00072 bindParameter("debugLevel", m_debugLevel, "0");
00073
00074
00075
00076
00077
00078
00079 addInPort("qCurrent", m_qCurrentIn);
00080 addInPort("tauIn", m_tauInIn);
00081
00082
00083 addOutPort("tauOut", m_tauOutOut);
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093 RTC::Properties& prop = getProperties();
00094 coil::stringTo(m_dt, prop["dt"].c_str());
00095
00096 m_robot = hrp::BodyPtr(new hrp::Body());
00097
00098 RTC::Manager& rtcManager = RTC::Manager::instance();
00099 std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00100 int comPos = nameServer.find(",");
00101 if (comPos < 0){
00102 comPos = nameServer.length();
00103 }
00104 nameServer = nameServer.substr(0, comPos);
00105 RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00106 if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
00107 CosNaming::NamingContext::_duplicate(naming.getRootContext())
00108 )){
00109 std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
00110 << m_profile.instance_name << std::endl;
00111 return RTC::RTC_ERROR;
00112 }
00113
00114
00115 m_tauOut.data.length(m_robot->numJoints());
00116
00117
00118 coil::stringTo(m_is_gravity_compensation, prop["gravity_compensation"].c_str());
00119 if (m_debugLevel > 0) {
00120 std::cerr << "[" << m_profile.instance_name << "] : gravity compensation flag: " << m_is_gravity_compensation << std::endl;
00121 }
00122
00123
00124
00125 m_torque_offset.resize(m_robot->numJoints());
00126 coil::vstring torque_offset = coil::split(prop["torque_offset"], ",");
00127 if ( m_torque_offset.size() == torque_offset.size() && m_debugLevel > 0) {
00128 for(unsigned int i = 0; i < m_robot->numJoints(); i++){
00129 coil::stringTo(m_torque_offset[i], torque_offset[i].c_str());
00130 std::cerr << "[" << m_profile.instance_name << "]" << "offset[" << m_robot->joint(i)->name << "]: " << m_torque_offset[i] << std::endl;
00131 }
00132 } else {
00133 if (m_debugLevel > 0) {
00134 std::cerr << "[" << m_profile.instance_name << "]" << "Size of torque_offset is not correct." << std::endl;
00135 std::cerr << "[" << m_profile.instance_name << "]" << "joints: " << m_robot->numJoints() << std::endl;
00136 std::cerr << "[" << m_profile.instance_name << "]" << "offsets: " << torque_offset.size() << std::endl;
00137 }
00138 }
00139
00140
00141
00142 coil::vstring torque_filter_params = coil::split(prop["torque_filter_params"], ",");
00143 int filter_dim = 0;
00144 std::vector<double> fb_coeffs, ff_coeffs;
00145 bool use_default_flag = false;
00146
00147 if ( torque_filter_params.size() > 0 ) {
00148 coil::stringTo(filter_dim, torque_filter_params[0].c_str());
00149 if (m_debugLevel > 0) {
00150 std::cerr << "[" << m_profile.instance_name << "]" << "filter dim: " << filter_dim << std::endl;
00151 std::cerr << "[" << m_profile.instance_name << "]" << "torque filter param size: " << torque_filter_params.size() << std::endl;
00152 }
00153 } else {
00154 use_default_flag = true;
00155 if (m_debugLevel > 0) {
00156 std::cerr<< "[" << m_profile.instance_name << "]" << "There is no torque_filter_params. Use default values." << std::endl;
00157 }
00158 }
00159 if (!use_default_flag && ((filter_dim + 1) * 2 + 1 != (int)torque_filter_params.size()) ) {
00160 if (m_debugLevel > 0) {
00161 std::cerr<< "[" << m_profile.instance_name << "]" << "Size of torque_filter_params is not correct. Use default values." << std::endl;
00162 }
00163 use_default_flag = true;
00164 }
00165
00166 if (use_default_flag) {
00167
00168
00169
00170
00171
00172
00173
00174
00175 filter_dim = 2;
00176 fb_coeffs.resize(filter_dim+1);
00177 fb_coeffs[0] = 1.00000;
00178 fb_coeffs[1] = 1.88903;
00179 fb_coeffs[2] =-0.89487;
00180 ff_coeffs.resize(filter_dim+1);
00181 ff_coeffs[0] = 0.0014603;
00182 ff_coeffs[1] = 0.0029206;
00183 ff_coeffs[2] = 0.0014603;
00184 } else {
00185 fb_coeffs.resize(filter_dim + 1);
00186 ff_coeffs.resize(filter_dim + 1);
00187 for (int i = 0; i < filter_dim + 1; i++) {
00188 coil::stringTo(fb_coeffs[i], torque_filter_params[i + 1].c_str());
00189 coil::stringTo(ff_coeffs[i], torque_filter_params[i + (filter_dim + 2)].c_str());
00190 }
00191 }
00192
00193 if (m_debugLevel > 0) {
00194 for (int i = 0; i < filter_dim + 1; i++) {
00195 std::cerr << "[" << m_profile.instance_name << "]" << "fb[" << i << "]: " << fb_coeffs[i] << std::endl;
00196 std::cerr << "[" << m_profile.instance_name << "]" << "ff[" << i << "]: " << ff_coeffs[i] << std::endl;
00197 }
00198 }
00199
00200
00201 for(unsigned int i = 0; i < m_robot->numJoints(); i++){
00202 m_filters.push_back(IIRFilter(filter_dim, fb_coeffs, ff_coeffs, std::string(m_profile.instance_name)));
00203 }
00204
00205 return RTC::RTC_OK;
00206 }
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231 RTC::ReturnCode_t TorqueFilter::onActivated(RTC::UniqueId ec_id)
00232 {
00233 std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00234 return RTC::RTC_OK;
00235 }
00236
00237 RTC::ReturnCode_t TorqueFilter::onDeactivated(RTC::UniqueId ec_id)
00238 {
00239 std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00240 return RTC::RTC_OK;
00241 }
00242
00243 RTC::ReturnCode_t TorqueFilter::onExecute(RTC::UniqueId ec_id)
00244 {
00245
00246 static int loop = 0;
00247 loop ++;
00248
00249 coil::TimeValue coiltm(coil::gettimeofday());
00250 RTC::Time tm;
00251 tm.sec = coiltm.sec();
00252 tm.nsec = coiltm.usec()*1000;
00253
00254 if (m_qCurrentIn.isNew()) {
00255 m_qCurrentIn.read();
00256 }
00257 if (m_tauInIn.isNew()) {
00258 m_tauInIn.read();
00259 }
00260
00261 if (m_tauIn.data.length() == m_robot->numJoints()) {
00262 int num_joints = m_robot->numJoints();
00263 hrp::dvector g_joint_torque(num_joints);
00264 hrp::dvector torque(num_joints);
00265
00266 if (m_qCurrent.data.length() == m_robot->numJoints()) {
00267
00268 for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00269 m_robot->joint(i)->q = m_qCurrent.data[i];
00270 }
00271 m_robot->calcForwardKinematics();
00272 m_robot->calcCM();
00273 m_robot->rootLink()->calcSubMassCM();
00274
00275
00276 hrp::Vector3 g(0, 0, 9.8);
00277 for (int i = 0; i < num_joints; i++) {
00278
00279 g_joint_torque[i] = (m_robot->joint(i)->subm*g).cross(m_robot->joint(i)->submwc / m_robot->joint(i)->subm - m_robot->joint(i)->p).dot(m_robot->joint(i)->R * m_robot->joint(i)->a);
00280 }
00281 } else {
00282 if (DEBUGP) {
00283 std::cerr << "[" << m_profile.instance_name << "]" << "input is not correct" << std::endl;
00284 std::cerr << "[" << m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
00285 std::cerr << "[" << m_profile.instance_name << "]" << " qCurrent: " << m_qCurrent.data.length() << std::endl;
00286 std::cerr << std::endl;
00287 }
00288 for (int i = 0; i < num_joints; i++) {
00289 g_joint_torque[i] = 0.0;
00290 }
00291 }
00292
00293 if (DEBUGP) {
00294 std::cerr << "[" << m_profile.instance_name << "]" << "raw torque: ";
00295 for (int i = 0; i < num_joints; i++) {
00296 std::cerr << " " << m_tauIn.data[i] ;
00297 }
00298 std::cerr << std::endl;
00299 std::cerr << "[" << m_profile.instance_name << "]" << "gravity compensation: ";
00300 for (int i = 0; i < num_joints; i++) {
00301 std::cerr << " " << g_joint_torque[i];
00302 }
00303 std::cerr << std::endl;
00304 }
00305
00306 for (int i = 0; i < num_joints; i++) {
00307
00308
00309
00310 torque[i] = m_filters[i].executeFilter(m_tauIn.data[i]) - m_torque_offset[i];
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322 if(m_is_gravity_compensation){
00323 m_tauOut.data[i] = torque[i] + g_joint_torque[i];
00324 } else {
00325 m_tauOut.data[i] = torque[i];
00326 }
00327 }
00328 if (DEBUGP) {
00329 std::cerr << "[" << m_profile.instance_name << "]" << "filtered torque: ";
00330 for (int i = 0; i < num_joints; i++) {
00331 std::cerr << " " << torque[i];
00332 }
00333 std::cerr << std::endl;
00334 }
00335 m_tauOut.tm = tm;
00336 m_tauOutOut.write();
00337 } else {
00338 if (DEBUGP) {
00339 std::cerr << "[" << m_profile.instance_name << "]" << "input is not correct" << std::endl;
00340 std::cerr << "[" << m_profile.instance_name << "]" << " numJoints: " << m_robot->numJoints() << std::endl;
00341 std::cerr << "[" << m_profile.instance_name << "]" << " tauIn: " << m_tauIn.data.length() << std::endl;
00342 std::cerr << std::endl;
00343 }
00344 }
00345
00346 return RTC::RTC_OK;
00347 }
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386 extern "C"
00387 {
00388
00389 void TorqueFilterInit(RTC::Manager* manager)
00390 {
00391 RTC::Properties profile(torquefilter_spec);
00392 manager->registerFactory(profile,
00393 RTC::Create<TorqueFilter>,
00394 RTC::Delete<TorqueFilter>);
00395 }
00396
00397 };
00398
00399