GraspController.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "GraspController.h"
00011 #include "hrpsys/util/VectorConvert.h"
00012 #include <rtm/CorbaNaming.h>
00013 #include <hrpModel/ModelLoaderUtil.h>
00014 #include "hrpsys/idl/RobotHardwareService.hh"
00015 
00016 #include <hrpModel/Link.h>
00017 
00018 #include <math.h>
00019 #define deg2rad(x)((x)*M_PI/180)
00020 #define min(a,b) ((a)<(b)?(a):(b))
00021 #define max(a,b) ((a)>(b)?(a):(b))
00022 
00023 // Module specification
00024 // <rtc-template block="module_spec">
00025 static const char* softerrorlimiter_spec[] =
00026   {
00027     "implementation_id", "GraspController",
00028     "type_name",         "GraspController",
00029     "description",       "soft error limiter",
00030     "version",           HRPSYS_PACKAGE_VERSION,
00031     "vendor",            "AIST",
00032     "category",          "example",
00033     "activity_type",     "DataFlowComponent",
00034     "max_instance",      "10",
00035     "language",          "C++",
00036     "lang_type",         "compile",
00037     // Configuration variables
00038     "conf.default.debugLevel", "0",
00039     ""
00040   };
00041 // </rtc-template>
00042 
00043 GraspController::GraspController(RTC::Manager* manager)
00044   : RTC::DataFlowComponentBase(manager),
00045     // <rtc-template block="initializer">
00046     m_qRefIn("qRef", m_qRef),
00047     m_qCurrentIn("qCurrent", m_qCurrent),
00048     m_qIn("qIn", m_q),
00049     m_qOut("q", m_q),
00050     m_GraspControllerServicePort("GraspControllerService"),
00051     // </rtc-template>
00052     m_debugLevel(0),
00053     dummy(0)
00054 {
00055   m_service0.grasp(this);
00056 }
00057 
00058 GraspController::~GraspController()
00059 {
00060 }
00061 
00062 
00063 
00064 RTC::ReturnCode_t GraspController::onInitialize()
00065 {
00066   std::cout << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl;
00067   // <rtc-template block="bind_config">
00068   // Bind variables and configuration variable
00069   bindParameter("debugLevel", m_debugLevel, "0");
00070   
00071   // </rtc-template>
00072 
00073   // Registration: InPort/OutPort/Service
00074   // <rtc-template block="registration">
00075   // Set InPort buffers
00076   addInPort("qRef", m_qRefIn); // for naming rule of hrpsys_config.py
00077   addInPort("qCurrent", m_qCurrentIn);
00078   addInPort("qIn", m_qIn);
00079   
00080   // Set OutPort buffer
00081   addOutPort("q", m_qOut); // for naming rule of hrpsys_config.py
00082   
00083   // Set service provider to Ports
00084   m_GraspControllerServicePort.registerProvider("service0", "GraspControllerService", m_service0);
00085   
00086   // Set service consumers to Ports
00087   
00088   // Set CORBA Service Ports
00089   addPort(m_GraspControllerServicePort);
00090   
00091   // </rtc-template>
00092 
00093   m_robot = hrp::BodyPtr(new hrp::Body());
00094   RTC::Properties& prop = getProperties();
00095   coil::stringTo(m_dt, prop["dt"].c_str());
00096 
00097   RTC::Manager& rtcManager = RTC::Manager::instance();
00098   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00099   int comPos = nameServer.find(",");
00100   if (comPos < 0){
00101       comPos = nameServer.length();
00102   }
00103   nameServer = nameServer.substr(0, comPos);
00104   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00105   if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
00106                                CosNaming::NamingContext::_duplicate(naming.getRootContext())
00107           )){
00108       std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" 
00109                 << std::endl;
00110       return RTC::RTC_ERROR;
00111   }
00112   // <name> : <joint1>, <direction1>, <joint2>, <direction2>, <joint1>, <direction2>, <name> : <joint1>, <direction1>
00113   // check num of grasp
00114   coil::vstring grasp_joint_params = coil::split(prop["grasp_joint_groups"], ",");
00115   std::string grasp_name;
00116   GraspJoint grasp_joint;
00117   std::vector<GraspJoint> grasp_joints;
00118   std::cerr << "[" << m_profile.instance_name << "] Parse joint group setting..." << std::endl;
00119   for(unsigned int i = 0, f = 0; i < grasp_joint_params.size(); i++ ){
00120     coil::vstring grasp_joint_group_names = coil::split(grasp_joint_params[i], ":");
00121     if ( grasp_joint_group_names.size() > 1 ) {
00122       if ( grasp_name != "" ) {
00123         GraspParam grasp_param;
00124         grasp_param.time = 0;
00125         grasp_param.joints = grasp_joints;
00126         grasp_param.time = 1; // stop
00127         m_grasp_param[grasp_name] = grasp_param;
00128         grasp_joints.clear();
00129       }
00130       // initialize
00131       grasp_name = grasp_joint_group_names[0];
00132       if ( !! m_robot->link(grasp_joint_group_names[1]) ) {
00133         grasp_joint.id = m_robot->link(std::string(grasp_joint_group_names[1].c_str()))->jointId;
00134       } else {
00135         std::cerr << "[" << m_profile.instance_name << "]   No such grasp joint name " << grasp_joint_group_names[1] << std::endl;
00136       }
00137       f = 0;
00138       i++;
00139     } else {
00140         std::cerr << "[" << m_profile.instance_name << "]   Invalid joint group setting (length " << grasp_joint_group_names.size() << " should be > 1" << std::endl;
00141     }
00142     if ( f == 0 ) {
00143       coil::stringTo(grasp_joint.dir,grasp_joint_params[i].c_str());
00144       grasp_joints.push_back(grasp_joint);
00145       f = 1 ;
00146     } else {
00147       if ( !! m_robot->link(grasp_joint_params[i]) ) {
00148         grasp_joint.id = m_robot->link(grasp_joint_params[i])->jointId;
00149       } else {
00150         std::cerr << "[" << m_profile.instance_name << "]   No such grasp joint name " << grasp_joint_params[i] << std::endl;
00151       }
00152       f = 0 ;
00153     }
00154   }
00155   // finalize
00156   if ( grasp_name != "" ) {
00157     GraspParam grasp_param;
00158     grasp_param.time = 0;
00159     grasp_param.joints = grasp_joints;
00160     grasp_param.time = 1; // stop
00161     m_grasp_param[grasp_name] = grasp_param;
00162   }
00163   //
00164   std::cerr << "[" << m_profile.instance_name << "] Joint group setting results." << std::endl;
00165   std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin();
00166   while ( it != m_grasp_param.end() ) {
00167       std::cerr << "[" << m_profile.instance_name << "]   " << it->first << " : ";
00168       for ( unsigned int i = 0 ; i < it->second.joints.size(); i++ ) {
00169         std::cerr << "id = " << it->second.joints[i].id << ", dir = " << it->second.joints[i].dir << ", ";
00170       }
00171       std::cerr << std::endl;
00172       it++;
00173   }
00174 
00175 
00176   return RTC::RTC_OK;
00177 }
00178 
00179 /*
00180 RTC::ReturnCode_t GraspController::onFinalize()
00181 {
00182   return RTC::RTC_OK;
00183 }
00184 */
00185 
00186 /*
00187 RTC::ReturnCode_t GraspController::onStartup(RTC::UniqueId ec_id)
00188 {
00189   return RTC::RTC_OK;
00190 }
00191 */
00192 
00193 /*
00194 RTC::ReturnCode_t GraspController::onShutdown(RTC::UniqueId ec_id)
00195 {
00196   return RTC::RTC_OK;
00197 }
00198 */
00199 
00200 RTC::ReturnCode_t GraspController::onActivated(RTC::UniqueId ec_id)
00201 {
00202   std::cout << "[" << m_profile.instance_name<< "] : onActivated(" << ec_id << ")" << std::endl;
00203   return RTC::RTC_OK;
00204 }
00205 
00206 RTC::ReturnCode_t GraspController::onDeactivated(RTC::UniqueId ec_id)
00207 {
00208   std::cout << "[" << m_profile.instance_name<< "] : onDeactivated(" << ec_id << ")" << std::endl;
00209   for (std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin(); it != m_grasp_param.end(); it++ ) {
00210     it->second.time = 2; // count down to 1
00211     it->second.target_error = 0;
00212   }
00213   return RTC::RTC_OK;
00214 }
00215 
00216 RTC::ReturnCode_t GraspController::onExecute(RTC::UniqueId ec_id)
00217 {
00218   //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00219   if (m_qRefIn.isNew()) {
00220     m_qRefIn.read();
00221   }
00222   if (m_qCurrentIn.isNew()) {
00223     m_qCurrentIn.read();
00224   }
00225   if (m_qIn.isNew()) {
00226     m_qIn.read();
00227   }
00228 
00229   if ( m_qRef.data.length() == m_qCurrent.data.length() &&
00230        m_qRef.data.length() == m_q.data.length() ) {
00231 
00232     std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin();
00233     while ( it != m_grasp_param.end() ) {
00234       GraspParam& grasp_param = it->second;
00235       if ( grasp_param.time < 0 ) { // staring
00236         grasp_param.time++;
00237       } else if ( grasp_param.time == 0 ) {// working
00238         //std::cerr << "grasp mode " << std::endl;
00239         for ( unsigned int j= 0; j < grasp_param.joints.size(); j++ ) {
00240           int i = grasp_param.joints[j].id;
00241           if ( 0 <= i && (unsigned int)i < m_qRef.data.length() ) {
00242             double error = (m_qCurrent.data[i] - m_qRef.data[i]) + grasp_param.target_error * grasp_param.joints[j].dir;
00243             double diff  = fabs(error);
00244             if ( error > 0 ) m_q.data[i] = m_qRef.data[i] + diff;
00245             if ( error < 0 ) m_q.data[i] = m_qRef.data[i] - diff;
00246             //std::cerr << "id = " << i << ", ref = " << m_qRef.data[i] << ", cur =" << m_qCurrent.data[i] << " error = " << error << ", diff = " << diff << ", q = " << m_q.data[i] << " (target = " << grasp_param.target_error << ", dir=" << grasp_param.joints[j].dir << ")" << std::endl;
00247           } else {
00248             if (m_debugLevel==1) std::cerr << "GraspController is not working..., id = " << i << std::endl;
00249           }
00250         }
00251       } else if ( grasp_param.time > 1 ) { // stopping 
00252         grasp_param.time--;
00253         for ( unsigned int j= 0; j < grasp_param.joints.size(); j++ ) {
00254           int i = grasp_param.joints[j].id;
00255           if ( 0 <= i && (unsigned int)i < m_qRef.data.length() ) {
00256             m_qRef.data[i] = (m_qRef.data[i] - m_q.data[i] ) * grasp_param.time/1000 + m_q.data[i];
00257             double diff = m_qRef.data[i] - m_qCurrent.data[i];
00258             if ( diff > 0 ) diff = min(diff, 0.034907); // 2 [deg]
00259             if ( diff < 0 ) diff = max(diff,-0.034907); // 2 [deg]
00260             m_q.data[i] = diff + m_qCurrent.data[i];
00261             //std::cerr << "id = " << i << ", ref = " << m_qRef.data[i] << ", cur =" << m_qCurrent.data[i] << " q = " << m_q.data[i] << std::endl;
00262           }
00263         }
00264       } else if ( grasp_param.time == 1 ) {// stop
00265       }
00266       it++;
00267     }
00268 
00269     m_qOut.write();
00270   }else if ( m_qCurrent.data.length() == m_q.data.length() ) {
00271     if (m_debugLevel==1) std::cerr << "GraspController in pass through mode..." << std::endl;
00272     m_qOut.write();
00273   } else {
00274     std::cerr << "GraspController is not working..." << std::endl;
00275     std::cerr << "          m_qIn " << m_q.data.length() << std::endl;
00276     std::cerr << "         m_qRef " << m_qRef.data.length() << std::endl;
00277     std::cerr << "     m_qCurrent " << m_qCurrent.data.length() << std::endl;
00278   }
00279 
00280   return RTC::RTC_OK;
00281 }
00282 
00283 /*
00284 RTC::ReturnCode_t GraspController::onAborting(RTC::UniqueId ec_id)
00285 {
00286   return RTC::RTC_OK;
00287 }
00288 */
00289 
00290 /*
00291 RTC::ReturnCode_t GraspController::onError(RTC::UniqueId ec_id)
00292 {
00293   return RTC::RTC_OK;
00294 }
00295 */
00296 
00297 /*
00298 RTC::ReturnCode_t GraspController::onReset(RTC::UniqueId ec_id)
00299 {
00300   return RTC::RTC_OK;
00301 }
00302 */
00303 
00304 /*
00305 RTC::ReturnCode_t GraspController::onStateUpdate(RTC::UniqueId ec_id)
00306 {
00307   return RTC::RTC_OK;
00308 }
00309 */
00310 
00311 /*
00312 RTC::ReturnCode_t GraspController::onRateChanged(RTC::UniqueId ec_id)
00313 {
00314   return RTC::RTC_OK;
00315 }
00316 */
00317 
00318  // m_grasp_param.time
00319  // > 1  : stopping
00320  //   1  : stopped
00321  //   0  : working
00322  // < 0  : starting
00323 bool GraspController::startGrasp(const char *name, double target_error) {
00324   if ( m_grasp_param.find( name ) == m_grasp_param.end() ) {
00325     std::cerr << "[" << m_profile.instance_name << "] Could not found grasp controller " << name << std::endl;
00326     return false;
00327   }
00328   std::cerr << "[" << m_profile.instance_name << "] Start Grasp " << name << std::endl;
00329   m_grasp_param[name].time = -10; // count up to 0
00330   m_grasp_param[name].target_error = fabs(target_error);
00331   return true;
00332 }
00333 
00334 bool GraspController::stopGrasp(const char *name) {
00335   if ( m_grasp_param.find( name ) == m_grasp_param.end() ) {
00336     std::cerr << "[" << m_profile.instance_name << "] Could not found grasp controller " << name << std::endl;
00337     return false;
00338   }
00339   std::cerr << "[" << m_profile.instance_name << "] Stop Grasp " << name << std::endl;
00340   m_grasp_param[name].time = 1000; // count down to 1
00341   m_grasp_param[name].target_error = 0;
00342   return true;
00343 }
00344 
00345 
00346 extern "C"
00347 {
00348 
00349   void GraspControllerInit(RTC::Manager* manager)
00350   {
00351     RTC::Properties profile(softerrorlimiter_spec);
00352     manager->registerFactory(profile,
00353                              RTC::Create<GraspController>,
00354                              RTC::Delete<GraspController>);
00355   }
00356 
00357 };
00358 
00359 


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