ImpedanceController.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/CorbaNaming.h>
00011 #include <hrpModel/Link.h>
00012 #include <hrpModel/Sensor.h>
00013 #include <hrpModel/ModelLoaderUtil.h>
00014 #include "ImpedanceController.h"
00015 #include "JointPathEx.h"
00016 #include <hrpModel/JointPath.h>
00017 #include <hrpUtil/MatrixSolvers.h>
00018 #include "hrpsys/util/Hrpsys.h"
00019 #include <boost/assign.hpp>
00020 
00021 #define MAX_TRANSITION_COUNT (static_cast<int>(2/m_dt))
00022 typedef coil::Guard<coil::Mutex> Guard;
00023 
00024 // Module specification
00025 // <rtc-template block="module_spec">
00026 static const char* impedancecontroller_spec[] =
00027     {
00028         "implementation_id", "ImpedanceController",
00029         "type_name",         "ImpedanceController",
00030         "description",       "impedance controller component",
00031         "version",           HRPSYS_PACKAGE_VERSION,
00032         "vendor",            "AIST",
00033         "category",          "example",
00034         "activity_type",     "DataFlowComponent",
00035         "max_instance",      "10",
00036         "language",          "C++",
00037         "lang_type",         "compile",
00038         // Configuration variables
00039         "conf.default.debugLevel", "0",
00040         ""
00041     };
00042 // </rtc-template>
00043 
00044 ImpedanceController::ImpedanceController(RTC::Manager* manager)
00045     : RTC::DataFlowComponentBase(manager),
00046       // <rtc-template block="initializer">
00047       m_qCurrentIn("qCurrent", m_qCurrent),
00048       m_qRefIn("qRef", m_qRef),
00049       m_basePosIn("basePosIn", m_basePos),
00050       m_baseRpyIn("baseRpyIn", m_baseRpy),
00051       m_rpyIn("rpy", m_rpy),
00052       m_qOut("q", m_q),
00053       m_ImpedanceControllerServicePort("ImpedanceControllerService"),
00054       // </rtc-template>
00055       m_robot(hrp::BodyPtr()),
00056       m_debugLevel(0),
00057       dummy(0),
00058       use_sh_base_pos_rpy(false)
00059 {
00060     m_service0.impedance(this);
00061 }
00062 
00063 ImpedanceController::~ImpedanceController()
00064 {
00065 }
00066 
00067 
00068 RTC::ReturnCode_t ImpedanceController::onInitialize()
00069 {
00070     std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00071     bindParameter("debugLevel", m_debugLevel, "0");
00072 
00073     // Registration: InPort/OutPort/Service
00074     // <rtc-template block="registration">
00075     // Set InPort buffers
00076     addInPort("qCurrent", m_qCurrentIn);
00077     addInPort("qRef", m_qRefIn);
00078     addInPort("basePosIn", m_basePosIn);
00079     addInPort("baseRpyIn", m_baseRpyIn);
00080     addInPort("rpy", m_rpyIn);
00081 
00082     // Set OutPort buffer
00083     addOutPort("q", m_qOut);
00084   
00085     // Set service provider to Ports
00086     m_ImpedanceControllerServicePort.registerProvider("service0", "ImpedanceControllerService", m_service0);
00087   
00088     // Set service consumers to Ports
00089   
00090     // Set CORBA Service Ports
00091     addPort(m_ImpedanceControllerServicePort);
00092   
00093     // </rtc-template>
00094     // <rtc-template block="bind_config">
00095     // Bind variables and configuration variable
00096   
00097     // </rtc-template>
00098 
00099     RTC::Properties& prop = getProperties();
00100     coil::stringTo(m_dt, prop["dt"].c_str());
00101 
00102     m_robot = hrp::BodyPtr(new hrp::Body());
00103 
00104     RTC::Manager& rtcManager = RTC::Manager::instance();
00105     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00106     int comPos = nameServer.find(",");
00107     if (comPos < 0){
00108         comPos = nameServer.length();
00109     }
00110     nameServer = nameServer.substr(0, comPos);
00111     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00112     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
00113                                  CosNaming::NamingContext::_duplicate(naming.getRootContext())
00114                                  )){
00115       std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
00116       return RTC::RTC_ERROR;
00117     }
00118 
00119 
00120     // Setting for wrench data ports (real + virtual)
00121     std::vector<std::string> fsensor_names;
00122     //   find names for real force sensors
00123     unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
00124     for (unsigned int i=0; i<npforce; i++){
00125         fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
00126     }
00127     // load virtual force sensors
00128     readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
00129     unsigned int nvforce = m_vfs.size();
00130     for (unsigned int i=0; i<nvforce; i++){
00131         for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
00132             if (it->second.id == (int)i) fsensor_names.push_back(it->first);
00133         }
00134     }
00135     //   add ports for all force sensors
00136     unsigned int nforce  = npforce + nvforce;
00137     m_force.resize(nforce);
00138     m_forceIn.resize(nforce);
00139     m_ref_force.resize(nforce);
00140     m_ref_forceIn.resize(nforce);
00141     std::cerr << "[" << m_profile.instance_name << "] force sensor ports" << std::endl;
00142     for (unsigned int i=0; i<nforce; i++){
00143         // actual inport
00144         m_forceIn[i] = new InPort<TimedDoubleSeq>(fsensor_names[i].c_str(), m_force[i]);
00145         m_force[i].data.length(6);
00146         registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]);
00147         // ref inport
00148         m_ref_force[i].data.length(6);
00149         for (unsigned int j=0; j<6; j++) m_ref_force[i].data[j] = 0.0;
00150         m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+fsensor_names[i]+"In").c_str(), m_ref_force[i]);
00151         registerInPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), *m_ref_forceIn[i]);
00152         std::cerr << "[" << m_profile.instance_name << "]   name = " << fsensor_names[i] << std::endl;
00153     }
00154 
00155     for (unsigned int i=0; i<m_forceIn.size(); i++){
00156       abs_forces.insert(std::pair<std::string, hrp::Vector3>(m_forceIn[i]->name(), hrp::Vector3::Zero()));
00157       abs_moments.insert(std::pair<std::string, hrp::Vector3>(m_forceIn[i]->name(), hrp::Vector3::Zero()));
00158     }
00159 
00160     unsigned int dof = m_robot->numJoints();
00161     for ( unsigned int i = 0 ; i < dof; i++ ){
00162       if ( (int)i != m_robot->joint(i)->jointId ) {
00163         std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl;
00164         return RTC::RTC_ERROR;
00165       }
00166     }
00167 
00168     // setting from conf file
00169     // rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle)
00170     coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
00171     std::map<std::string, std::string> base_name_map;
00172     if (end_effectors_str.size() > 0) {
00173         size_t prop_num = 10;
00174         size_t num = end_effectors_str.size()/prop_num;
00175         for (size_t i = 0; i < num; i++) {
00176             std::string ee_name, ee_target, ee_base;
00177             coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
00178             coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
00179             coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
00180             ee_trans eet;
00181             for (size_t j = 0; j < 3; j++) {
00182                 coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
00183             }
00184             double tmpv[4];
00185             for (int j = 0; j < 4; j++ ) {
00186                 coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
00187             }
00188             eet.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
00189             eet.target_name = ee_target;
00190             ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));
00191             base_name_map.insert(std::pair<std::string, std::string>(ee_name, ee_base));
00192             std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl;
00193             std::cerr << "[" << m_profile.instance_name << "]   target = " << ee_target << ", base = " << ee_base << std::endl;
00194             std::cerr << "[" << m_profile.instance_name << "]   localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << "[m]" << std::endl;
00195             std::cerr << "[" << m_profile.instance_name << "]   localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
00196         }
00197     }
00198 
00199     // initialize impedance params
00200     std::cerr << "[" << m_profile.instance_name << "] Add impedance params" << std::endl;
00201     for (unsigned int i=0; i<m_forceIn.size(); i++){
00202         std::string sensor_name = m_forceIn[i]->name();
00203         hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_name);
00204         std::string sensor_link_name;
00205         if ( sensor ) {
00206             // real force sensor
00207             sensor_link_name = sensor->link->name;
00208         } else if ( m_vfs.find(sensor_name) !=  m_vfs.end()) {
00209             // virtual force sensor
00210             sensor_link_name = m_vfs[sensor_name].link->name;
00211         } else {
00212             std::cerr << "[" << m_profile.instance_name << "]   unknown force param" << std::endl;
00213             continue;
00214         }
00215         // 1. Check whether adequate ee_map exists for the sensor.
00216         std::string ee_name;
00217         bool is_ee_exists = false;
00218         for ( std::map<std::string, ee_trans>::iterator it = ee_map.begin(); it != ee_map.end(); it++ ) {
00219             hrp::Link* alink = m_robot->link(it->second.target_name);
00220             std::string tmp_base_name = base_name_map[it->first];
00221             while (alink != NULL && alink->name != tmp_base_name && !is_ee_exists) {
00222                 if ( alink->name == sensor_link_name ) {
00223                     is_ee_exists = true;
00224                     ee_name = it->first;
00225                 }
00226                 alink = alink->parent;
00227             }
00228         }
00229         if (!is_ee_exists) {
00230             std::cerr << "[" << m_profile.instance_name << "]   No such ee setting for " << sensor_name << " and " << sensor_link_name << "!!. Impedance param for " << sensor_name << " cannot be added!!" << std::endl;
00231             continue;
00232         }
00233         // 2. Check whether already impedance param exists, which has the same target link as the sensor.
00234         if (m_impedance_param.find(ee_name) != m_impedance_param.end()) {
00235             std::cerr << "[" << m_profile.instance_name << "]   Already impedance param (target_name=" << sensor_link_name << ", ee_name=" << ee_name << ") exists!!. Impedance param for " << sensor_name << " cannot be added!!" << std::endl;
00236             continue;
00237         }
00238         // 3. Check whether joint path is adequate.
00239         hrp::Link* target_link = m_robot->link(ee_map[ee_name].target_name);
00240         ImpedanceParam p;
00241         p.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(base_name_map[ee_name]), target_link, m_dt, false, std::string(m_profile.instance_name)));
00242         if ( ! p.manip ) {
00243             std::cerr << "[" << m_profile.instance_name << "]   Invalid joint path from " << base_name_map[ee_name] << " to " << target_link->name << "!! Impedance param for " << sensor_name << " cannot be added!!" << std::endl;
00244             continue;
00245         }
00246         // 4. Set impedance param
00247         p.transition_joint_q.resize(m_robot->numJoints());
00248         p.sensor_name = sensor_name;
00249         m_impedance_param[ee_name] = p;
00250         std::cerr << "[" << m_profile.instance_name << "]   sensor = " << sensor_name << ", sensor-link = " << sensor_link_name << ", ee_name = " << ee_name << ", ee-link = " << target_link->name << std::endl;
00251     }
00252 
00253     std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
00254     readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
00255     if (interlocking_joints.size() > 0) {
00256         for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
00257             std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl;
00258             it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
00259         }
00260     }
00261 
00262     // allocate memory for outPorts
00263     m_q.data.length(dof);
00264     qrefv.resize(dof);
00265     loop = 0;
00266 
00267     return RTC::RTC_OK;
00268 }
00269 
00270 
00271 
00272 RTC::ReturnCode_t ImpedanceController::onFinalize()
00273 {
00274     return RTC::RTC_OK;
00275 }
00276 
00277 /*
00278   RTC::ReturnCode_t ImpedanceController::onStartup(RTC::UniqueId ec_id)
00279   {
00280   return RTC::RTC_OK;
00281   }
00282 */
00283 
00284 /*
00285   RTC::ReturnCode_t ImpedanceController::onShutdown(RTC::UniqueId ec_id)
00286   {
00287   return RTC::RTC_OK;
00288   }
00289 */
00290 
00291 RTC::ReturnCode_t ImpedanceController::onActivated(RTC::UniqueId ec_id)
00292 {
00293     std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00294     
00295     return RTC::RTC_OK;
00296 }
00297 
00298 RTC::ReturnCode_t ImpedanceController::onDeactivated(RTC::UniqueId ec_id)
00299 {
00300   std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00301   for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
00302       if (it->second.is_active) {
00303           stopImpedanceControllerNoWait(it->first);
00304           it->second.transition_count = 1;
00305       }
00306   }
00307   return RTC::RTC_OK;
00308 }
00309 
00310 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00311 RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
00312 {
00313     //std::cout << "ImpedanceController::onExecute(" << ec_id << ")" << std::endl;
00314     loop ++;
00315 
00316     // check dataport input
00317     for (unsigned int i=0; i<m_forceIn.size(); i++){
00318         if ( m_forceIn[i]->isNew() ) {
00319             m_forceIn[i]->read();
00320         }
00321         if ( m_ref_forceIn[i]->isNew() ) {
00322             m_ref_forceIn[i]->read();
00323         }
00324     }
00325     if (m_basePosIn.isNew()) {
00326       m_basePosIn.read();
00327     }
00328     if (m_baseRpyIn.isNew()) {
00329       m_baseRpyIn.read();
00330     }
00331     if (m_rpyIn.isNew()) {
00332       m_rpyIn.read();
00333     }
00334     if (m_qCurrentIn.isNew()) {
00335       m_qCurrentIn.read();
00336     }
00337     if (m_qRefIn.isNew()) {
00338         m_qRefIn.read();
00339         m_q.tm = m_qRef.tm;
00340     }
00341     if ( m_qRef.data.length() ==  m_robot->numJoints() &&
00342          m_qCurrent.data.length() ==  m_robot->numJoints() ) {
00343 
00344         if ( DEBUGP ) {
00345           std::cerr << "[" << m_profile.instance_name << "] qRef = ";
00346             for ( unsigned int i = 0; i <  m_qRef.data.length(); i++ ){
00347                 std::cerr << " " << m_qRef.data[i];
00348             }
00349             std::cerr << std::endl;
00350         }
00351 
00352         Guard guard(m_mutex);
00353 
00354         {
00355           // Store current robot state
00356           hrp::dvector qorg(m_robot->numJoints());
00357           for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00358             qorg[i] = m_robot->joint(i)->q;
00359           }
00360           // Get target parameters mainly from SequencePlayer
00361           getTargetParameters();
00362           // Calculate act/ref absolute force/moment
00363           calcForceMoment();
00364           // back to impedance robot model (only for controlled joint)
00365           for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
00366             ImpedanceParam& param = it->second;
00367             if (param.is_active) {
00368                 for ( unsigned int j = 0; j < param.manip->numJoints(); j++ ){
00369                     int i = param.manip->joint(j)->jointId;
00370                     m_robot->joint(i)->q = qorg[i];
00371                 }
00372             }
00373           }
00374           m_robot->calcForwardKinematics();
00375 
00376         }
00377 
00378         // Check if all limb is non-is_active mode.
00379         bool is_active = false;
00380         for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
00381             is_active = is_active || it->second.is_active;
00382         }
00383         if ( !is_active ) {
00384           for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){
00385             m_q.data[i] = m_qRef.data[i];
00386             m_robot->joint(i)->q = m_qRef.data[i];
00387           }
00388           m_qOut.write();
00389           return RTC_OK;
00390         }
00391 
00392         // Caculate ImpedanceControl and solve IK
00393         calcImpedanceControl();
00394 
00395         // Output
00396         if ( m_q.data.length() != 0 ) { // initialized
00397             for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00398                 m_q.data[i] = m_robot->joint(i)->q;
00399             }
00400             m_qOut.write();
00401             if ( DEBUGP ) {
00402                 std::cerr << "[" << m_profile.instance_name << "] q = ";
00403                 for ( unsigned int i = 0; i < m_q.data.length(); i++ ){
00404                     std::cerr << " " << m_q.data[i];
00405                 }
00406                 std::cerr << std::endl;
00407             }
00408         }
00409     } else {
00410         if ( DEBUGP || loop % 100 == 0 ) {
00411             std::cerr << "ImpedanceController is not working..." << std::endl;
00412             std::cerr << "         m_qRef " << m_qRef.data.length() << std::endl;
00413             std::cerr << "     m_qCurrent " << m_qCurrent.data.length() << std::endl;
00414         }
00415     }
00416     return RTC::RTC_OK;
00417 }
00418 
00419 /*
00420   RTC::ReturnCode_t ImpedanceController::onAborting(RTC::UniqueId ec_id)
00421   {
00422   return RTC::RTC_OK;
00423   }
00424 */
00425 
00426 /*
00427   RTC::ReturnCode_t ImpedanceController::onError(RTC::UniqueId ec_id)
00428   {
00429   return RTC::RTC_OK;
00430   }
00431 */
00432 
00433 /*
00434   RTC::ReturnCode_t ImpedanceController::onReset(RTC::UniqueId ec_id)
00435   {
00436   return RTC::RTC_OK;
00437   }
00438 */
00439 
00440 /*
00441   RTC::ReturnCode_t ImpedanceController::onStateUpdate(RTC::UniqueId ec_id)
00442   {
00443   return RTC::RTC_OK;
00444   }
00445 */
00446 
00447 /*
00448   RTC::ReturnCode_t ImpedanceController::onRateChanged(RTC::UniqueId ec_id)
00449   {
00450   return RTC::RTC_OK;
00451   }
00452 */
00453 
00454 void ImpedanceController::getTargetParameters ()
00455 {
00456     // Get reference robot state
00457     for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00458         m_robot->joint(i)->q = m_qRef.data[i];
00459         qrefv[i] = m_qRef.data[i];
00460     }
00461     m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
00462     m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
00463     m_robot->calcForwardKinematics();
00464     // Fix leg for legged robot
00465     if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot
00466          && !use_sh_base_pos_rpy ) {
00467         // TODO
00468         //  Tempolarily modify root coords to fix foot pos rot
00469         //  This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
00470 
00471         // get current foot mid pos + rot
00472         std::vector<hrp::Vector3> foot_pos;
00473         std::vector<hrp::Matrix33> foot_rot;
00474         std::vector<std::string> leg_names;
00475         leg_names.push_back("rleg");
00476         leg_names.push_back("lleg");
00477         for (size_t i = 0; i < leg_names.size(); i++) {
00478             hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
00479             foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos);
00480             foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
00481         }
00482         hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0);
00483         hrp::Matrix33 current_foot_mid_rot;
00484         rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
00485         // calculate fix pos + rot
00486         hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos);
00487         hrp::Matrix33 new_foot_mid_rot;
00488         {
00489             hrp::Vector3 ex = hrp::Vector3::UnitX();
00490             hrp::Vector3 ez = hrp::Vector3::UnitZ();
00491             hrp::Vector3 xv1 (current_foot_mid_rot * ex);
00492             xv1(2) = 0.0;
00493             xv1.normalize();
00494             hrp::Vector3 yv1(ez.cross(xv1));
00495             new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2);
00496             new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2);
00497             new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2);
00498         }
00499         // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
00500         hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose());
00501         m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
00502         rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
00503         m_robot->calcForwardKinematics();
00504     }
00505 
00506     // Set sequencer position and orientation to target_p0 and target_r0
00507     for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
00508         ImpedanceParam& param = it->second;
00509         std::string target_name = ee_map[it->first].target_name;
00510         param.target_p0 = m_robot->link(target_name)->p + m_robot->link(target_name)->R * ee_map[it->first].localPos;
00511         param.target_r0 = m_robot->link(target_name)->R * ee_map[it->first].localR;
00512         if (param.transition_count == -MAX_TRANSITION_COUNT) param.resetPreviousTargetParam();
00513     }
00514 };
00515 
00516 void ImpedanceController::calcForceMoment ()
00517 {
00518       for (unsigned int i=0; i<m_forceIn.size(); i++){
00519         if ( m_force[i].data.length()==6 ) {
00520           std::string sensor_name = m_forceIn[i]->name();
00521           hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_name);
00522           hrp::Vector3 data_p(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]);
00523           hrp::Vector3 data_r(m_force[i].data[3], m_force[i].data[4], m_force[i].data[5]);
00524           hrp::Vector3 ref_data_p(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
00525           hrp::Vector3 ref_data_r(m_ref_force[i].data[3], m_ref_force[i].data[4], m_ref_force[i].data[5]);
00526           if ( DEBUGP ) {
00527             std::cerr << "[" << m_profile.instance_name << "] force and moment [" << sensor_name << "]" << std::endl;
00528             std::cerr << "[" << m_profile.instance_name << "]   sensor force  = " << data_p.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00529             std::cerr << "[" << m_profile.instance_name << "]   sensor moment = " << data_r.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00530             std::cerr << "[" << m_profile.instance_name << "]   reference force  = " << ref_data_p.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00531             std::cerr << "[" << m_profile.instance_name << "]   reference moment = " << ref_data_r.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00532           }
00533           hrp::Matrix33 sensorR;
00534           hrp::Vector3 sensorPos, eePos;
00535           if ( sensor ) {
00536             // real force sensore
00537             sensorR = sensor->link->R * sensor->localR;
00538             sensorPos = sensor->link->p + sensorR * sensor->localPos;
00539           } else if ( m_vfs.find(sensor_name) !=  m_vfs.end()) {
00540             // virtual force sensor
00541             if ( DEBUGP ) {
00542               std::cerr << "[" << m_profile.instance_name << "]   sensorR = " << m_vfs[sensor_name].localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
00543             }
00544             sensorR = m_vfs[sensor_name].link->R * m_vfs[sensor_name].localR;
00545             sensorPos = m_vfs[sensor_name].link->p + m_vfs[sensor_name].link->R * m_vfs[sensor_name].localPos;
00546           } else {
00547             std::cerr << "[" << m_profile.instance_name << "]   unknown force param" << std::endl;
00548           }
00549           abs_forces[sensor_name] = sensorR * data_p;
00550           for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
00551               if ( it->second.sensor_name == sensor_name ) eePos = it->second.target_p0;
00552           }
00553           abs_moments[sensor_name] = sensorR * data_r + (sensorPos - eePos).cross(abs_forces[sensor_name]);
00554           // End effector local frame
00555           // hrp::Matrix33 eeR (parentlink->R * ee_map[parentlink->name].localR);
00556           // abs_ref_forces[sensor_name] = eeR * ref_data_p;
00557           // abs_ref_moments[sensor_name] = eeR * ref_data_r;
00558           // World frame
00559           abs_ref_forces[sensor_name] = ref_data_p;
00560           abs_ref_moments[sensor_name] = ref_data_r;
00561           if ( DEBUGP ) {
00562             std::cerr << "[" << m_profile.instance_name << "]   abs force  = " << abs_forces[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00563             std::cerr << "[" << m_profile.instance_name << "]   abs moment = " << abs_moments[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00564             std::cerr << "[" << m_profile.instance_name << "]   abs ref force  = " << abs_ref_forces[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00565             std::cerr << "[" << m_profile.instance_name << "]   abs ref moment = " << abs_ref_moments[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00566           }
00567         }
00568       }
00569 };
00570 
00571 void ImpedanceController::calcImpedanceControl ()
00572 {
00573     std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin();
00574     while(it != m_impedance_param.end()){
00575         ImpedanceParam& param = it->second;
00576         if (param.is_active) {
00577             if (DEBUGP) {
00578                 std::cerr << "[" << m_profile.instance_name << "] impedance mode " << it->first << " transition count = " << param.transition_count << ", ";
00579                 std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << ", ";
00580                 std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << ", ";
00581                 std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << ", ";
00582                 std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl;
00583             }
00584             if ( param.transition_count > 0 ) {
00585                 // set m_robot to qRef when deleting status
00586                 hrp::JointPathExPtr manip = param.manip;
00587                 // transition_smooth_gain moves from 0 to 1
00588                 // (/ (log (/ (- 1 0.99) 0.99)) 0.5)
00589                 double transition_smooth_gain = 1/(1+exp(-9.19*((static_cast<double>(MAX_TRANSITION_COUNT - param.transition_count) / MAX_TRANSITION_COUNT) - 0.5)));
00590                 for ( unsigned int j = 0; j < manip->numJoints(); j++ ) {
00591                     int i = manip->joint(j)->jointId; // index in robot model
00592                     hrp::Link* joint =  m_robot->joint(i);
00593                     joint->q = ( m_qRef.data[i] - param.transition_joint_q[i] ) * transition_smooth_gain + param.transition_joint_q[i];
00594                 }
00595                 // transition_count update
00596                 param.transition_count--;
00597                 if(param.transition_count <= 0){ // erase impedance param
00598                     std::cerr << "[" << m_profile.instance_name << "] Finished cleanup and erase impedance param " << it->first << std::endl;
00599                     param.is_active = false;
00600                 }
00601             } else {
00602                 // use impedance model
00603 
00604                 hrp::Link* target = m_robot->link(ee_map[it->first].target_name);
00605                 assert(target);
00606                 param.current_p1 = target->p + target->R * ee_map[it->first].localPos;
00607                 param.current_r1 = target->R * ee_map[it->first].localR;
00608                 if (param.transition_count == -MAX_TRANSITION_COUNT) param.resetPreviousCurrentParam();
00609 
00610                 hrp::Vector3 vel_p, vel_r;
00611                 //std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << std::endl;
00612                 //std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << std::endl;
00613                 // std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << std::endl;
00614                 // std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl;
00615 
00616                 // ref_force/ref_moment and force_gain/moment_gain are expressed in global coordinates. 
00617                 hrp::Matrix33 eeR = target->R * ee_map[it->first].localR;
00618                 hrp::Vector3 force_diff = abs_forces[it->second.sensor_name] - abs_ref_forces[it->second.sensor_name];
00619                 hrp::Vector3 moment_diff = abs_moments[it->second.sensor_name] - abs_ref_moments[it->second.sensor_name];
00620                 param.calcTargetVelocity(vel_p, vel_r, eeR, force_diff, moment_diff, m_dt,
00621                                          DEBUGP, std::string(m_profile.instance_name), it->first);
00622 
00623                 // Solve ik
00624                 hrp::JointPathExPtr manip = param.manip;
00625                 assert(manip);
00626                 //const int n = manip->numJoints();
00627                 manip->calcInverseKinematics2Loop(param.getOutputPos(), param.getOutputRot(), 1.0, param.avoid_gain, param.reference_gain, &qrefv, 1.0,
00628                                                   ee_map[it->first].localPos, ee_map[it->first].localR);
00629                 // transition_count update
00630                 if ( param.transition_count < 0 ) {
00631                     param.transition_count++;
00632                 }
00633             } // else
00634         }
00635         it++;
00636     } // while
00637 };
00638 
00639 //
00640 bool ImpedanceController::startImpedanceControllerNoWait(const std::string& i_name_)
00641 {
00642     // Lock Mutex
00643     {
00644         Guard guard(m_mutex);
00645         if ( m_impedance_param.find(i_name_) == m_impedance_param.end() ) {
00646             std::cerr << "[" << m_profile.instance_name << "] Could not found impedance controller param [" << i_name_ << "]" << std::endl;
00647             return false;
00648         }
00649         if ( m_impedance_param[i_name_].is_active ) {
00650             std::cerr << "[" << m_profile.instance_name << "] Impedance control [" << i_name_ << "] is already started" << std::endl;
00651             return false;
00652         }
00653         std::cerr << "[" << m_profile.instance_name << "] Start impedance control [" << i_name_ << "]" << std::endl;
00654         m_impedance_param[i_name_].is_active = true;
00655         m_impedance_param[i_name_].transition_count = -MAX_TRANSITION_COUNT; // when start impedance, count up to 0
00656     }
00657     return true;
00658 }
00659 
00660  bool ImpedanceController::startImpedanceController(const std::string& i_name_)
00661  {
00662      bool ret = startImpedanceControllerNoWait(i_name_);
00663      waitImpedanceControllerTransition(i_name_);
00664      return ret;
00665  }
00666 
00667 bool ImpedanceController::stopImpedanceControllerNoWait(const std::string& i_name_)
00668 {
00669     // Lock Mutex
00670     {
00671         Guard guard(m_mutex);
00672         if ( m_impedance_param.find(i_name_) == m_impedance_param.end() ) {
00673             std::cerr << "[" << m_profile.instance_name << "] Could not found impedance controller param [" << i_name_ << "]" << std::endl;
00674             return false;
00675         }
00676         if ( !m_impedance_param[i_name_].is_active ) {
00677             std::cerr << "[" << m_profile.instance_name << "] Impedance control [" << i_name_ << "] is already stopped" << std::endl;
00678             return false;
00679         }
00680         std::cerr << "[" << m_profile.instance_name << "] Stop impedance control [" << i_name_ << "]" << std::endl;
00681         for (unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
00682             m_impedance_param[i_name_].transition_joint_q[i] = m_robot->joint(i)->q;
00683         }
00684         m_impedance_param[i_name_].transition_count = MAX_TRANSITION_COUNT; // when stop impedance, count down to 0
00685     }
00686     return true;
00687 }
00688 
00689 bool ImpedanceController::stopImpedanceController(const std::string& i_name_)
00690 {
00691     bool ret = stopImpedanceControllerNoWait(i_name_);
00692     waitImpedanceControllerTransition(i_name_);
00693     return ret;
00694 }
00695 
00696 bool ImpedanceController::setImpedanceControllerParam(const std::string& i_name_, OpenHRP::ImpedanceControllerService::impedanceParam i_param_)
00697 {
00698     // Lock Mutex
00699     {
00700         Guard guard(m_mutex);
00701         std::string name = std::string(i_name_);
00702         if ( m_impedance_param.find(name) == m_impedance_param.end() ) {
00703             std::cerr << "[" << m_profile.instance_name << "] Could not found impedance controller param [" << name << "]" << std::endl;
00704             return false;
00705         }
00706 
00707         std::cerr << "[" << m_profile.instance_name << "] Update impedance parameters" << std::endl;
00708 
00709         m_impedance_param[name].sr_gain    = i_param_.sr_gain;
00710         m_impedance_param[name].avoid_gain = i_param_.avoid_gain;
00711         m_impedance_param[name].reference_gain = i_param_.reference_gain;
00712         m_impedance_param[name].manipulability_limit = i_param_.manipulability_limit;
00713         m_impedance_param[name].manip->setSRGain(m_impedance_param[name].sr_gain);
00714         m_impedance_param[name].manip->setManipulabilityLimit(m_impedance_param[name].manipulability_limit);
00715 
00716         m_impedance_param[name].M_p = i_param_.M_p;
00717         m_impedance_param[name].D_p = i_param_.D_p;
00718         m_impedance_param[name].K_p = i_param_.K_p;
00719         m_impedance_param[name].M_r = i_param_.M_r;
00720         m_impedance_param[name].D_r = i_param_.D_r;
00721         m_impedance_param[name].K_r = i_param_.K_r;
00722 
00723         m_impedance_param[name].force_gain = hrp::Vector3(i_param_.force_gain[0], i_param_.force_gain[1], i_param_.force_gain[2]).asDiagonal();
00724         m_impedance_param[name].moment_gain = hrp::Vector3(i_param_.moment_gain[0], i_param_.moment_gain[1], i_param_.moment_gain[2]).asDiagonal();
00725 
00726         std::vector<double> ov;
00727         ov.resize(m_impedance_param[name].manip->numJoints());
00728         for (size_t i = 0; i < m_impedance_param[name].manip->numJoints(); i++) {
00729             ov[i] = i_param_.ik_optional_weight_vector[i];
00730         }
00731         m_impedance_param[name].manip->setOptionalWeightVector(ov);
00732         use_sh_base_pos_rpy = i_param_.use_sh_base_pos_rpy;
00733 
00734         std::cerr << "[" << m_profile.instance_name << "] set parameters" << std::endl;
00735         std::cerr << "[" << m_profile.instance_name << "]             name : " << name << std::endl;
00736         std::cerr << "[" << m_profile.instance_name << "]    M, D, K (pos) : " << m_impedance_param[name].M_p << " " << m_impedance_param[name].D_p << " " << m_impedance_param[name].K_p << std::endl;
00737         std::cerr << "[" << m_profile.instance_name << "]    M, D, K (rot) : " << m_impedance_param[name].M_r << " " << m_impedance_param[name].D_r << " " << m_impedance_param[name].K_r << std::endl;
00738         std::cerr << "[" << m_profile.instance_name << "]       force_gain : " << m_impedance_param[name].force_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
00739         std::cerr << "[" << m_profile.instance_name << "]      moment_gain : " << m_impedance_param[name].moment_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "    [", "]")) << std::endl;
00740         std::cerr << "[" << m_profile.instance_name << "]      manip_limit : " << m_impedance_param[name].manipulability_limit << std::endl;
00741         std::cerr << "[" << m_profile.instance_name << "]          sr_gain : " << m_impedance_param[name].sr_gain << std::endl;
00742         std::cerr << "[" << m_profile.instance_name << "]       avoid_gain : " << m_impedance_param[name].avoid_gain << std::endl;
00743         std::cerr << "[" << m_profile.instance_name << "]   reference_gain : " << m_impedance_param[name].reference_gain << std::endl;
00744         std::cerr << "[" << m_profile.instance_name << "]   use_sh_base_pos_rpy : " << (use_sh_base_pos_rpy?"true":"false") << std::endl;
00745     }
00746     return true;
00747 }
00748 
00749 void ImpedanceController::copyImpedanceParam (ImpedanceControllerService::impedanceParam& i_param_, const ImpedanceParam& param)
00750 {
00751   i_param_.M_p = param.M_p;
00752   i_param_.D_p = param.D_p;
00753   i_param_.K_p = param.K_p;
00754   i_param_.M_r = param.M_r;
00755   i_param_.D_r = param.D_r;
00756   i_param_.K_r = param.K_r;
00757   for (size_t i = 0; i < 3; i++) i_param_.force_gain[i] = param.force_gain(i,i);
00758   for (size_t i = 0; i < 3; i++) i_param_.moment_gain[i] = param.moment_gain(i,i);
00759   i_param_.sr_gain = param.sr_gain;
00760   i_param_.avoid_gain = param.avoid_gain;
00761   i_param_.reference_gain = param.reference_gain;
00762   i_param_.manipulability_limit = param.manipulability_limit;
00763   if (param.is_active) i_param_.controller_mode = OpenHRP::ImpedanceControllerService::MODE_IMP;
00764   else i_param_.controller_mode = OpenHRP::ImpedanceControllerService::MODE_IDLE;
00765   if (param.manip == NULL) i_param_.ik_optional_weight_vector.length(0);
00766   else {
00767       i_param_.ik_optional_weight_vector.length(param.manip->numJoints());
00768       std::vector<double> ov;
00769       ov.resize(param.manip->numJoints());
00770       param.manip->getOptionalWeightVector(ov);
00771       for (size_t i = 0; i < param.manip->numJoints(); i++) {
00772           i_param_.ik_optional_weight_vector[i] = ov[i];
00773       }
00774   }
00775 }
00776 
00777 void ImpedanceController::updateRootLinkPosRot (TimedOrientation3D tmprpy)
00778 {
00779   if ( m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) {
00780       hrp::Sensor *sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
00781       hrp::Matrix33 tmpr;
00782       rats::rotm3times(tmpr, hrp::Matrix33(sensor->link->R*sensor->localR).transpose(), m_robot->rootLink()->R);
00783       rats::rotm3times(m_robot->rootLink()->R, hrp::rotFromRpy(tmprpy.data.r, tmprpy.data.p, tmprpy.data.y), tmpr);
00784   }
00785 }
00786 
00787 bool ImpedanceController::getImpedanceControllerParam(const std::string& i_name_, ImpedanceControllerService::impedanceParam& i_param_)
00788 {
00789     if ( m_impedance_param.find(i_name_) == m_impedance_param.end() ) {
00790         std::cerr << "[" << m_profile.instance_name << "] Could not found impedance controller param [" << i_name_ << "]" << std::endl;
00791         // if impedance param of i_name_ is not found, return default impedance parameter ;; default parameter is specified ImpedanceParam struct's default constructer
00792         copyImpedanceParam(i_param_, ImpedanceParam());
00793         i_param_.use_sh_base_pos_rpy = use_sh_base_pos_rpy;
00794         return false;
00795     }
00796     copyImpedanceParam(i_param_, m_impedance_param[i_name_]);
00797     i_param_.use_sh_base_pos_rpy = use_sh_base_pos_rpy;
00798     return true;
00799 }
00800 
00801 void ImpedanceController::waitImpedanceControllerTransition(std::string i_name_)
00802 {
00803     while (m_impedance_param.find(i_name_) != m_impedance_param.end() &&
00804            m_impedance_param[i_name_].transition_count != 0) {
00805       usleep(10);
00806     }
00807     return;
00808 }
00809 
00810 extern "C"
00811 {
00812 
00813     void ImpedanceControllerInit(RTC::Manager* manager)
00814     {
00815         RTC::Properties profile(impedancecontroller_spec);
00816         manager->registerFactory(profile,
00817                                  RTC::Create<ImpedanceController>,
00818                                  RTC::Delete<ImpedanceController>);
00819     }
00820 
00821 };


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