ProjectUtil.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <rtm/Manager.h>
00003 #include <rtm/RTObject.h>
00004 #include <rtm/DataFlowComponentBase.h>
00005 #include <hrpModel/Link.h>
00006 #include "ProjectUtil.h"
00007 #include "BVutil.h"
00008 
00009 void initWorld(Project& prj, BodyFactory &factory, 
00010                hrp::World<hrp::ConstraintForceSolver>& world,
00011                std::vector<hrp::ColdetLinkPairPtr> &pairs)
00012 {
00013     world.clearBodies();
00014     world.constraintForceSolver.clearCollisionCheckLinkPairs();
00015     world.setCurrentTime(0.0);
00016     
00017     world.setTimeStep(prj.timeStep());
00018     if(prj.isEuler()){
00019         world.setEulerMethod();
00020     } else {
00021         world.setRungeKuttaMethod();
00022     }
00023 
00024     // add bodies
00025     for (std::map<std::string, ModelItem>::iterator it=prj.models().begin();
00026          it != prj.models().end(); it++){
00027         const std::string name
00028             = it->second.rtcName == "" ? it->first : it->second.rtcName; 
00029         hrp::BodyPtr body = factory(name, it->second);
00030         if (body){
00031             body->setName(name);
00032             world.addBody(body);
00033             // <-- for OpenRTM-1.0.0 bug
00034             RTC::DataFlowComponentBase *rtc
00035                 = dynamic_cast<RTC::DataFlowComponentBase *>(body.get());
00036             if (rtc && rtc->getInstanceName() != name){
00037                 rtc->setInstanceName(name.c_str());
00038             }
00039             // -->
00040         }
00041     }
00042 
00043     for (unsigned int i=0; i<prj.collisionPairs().size(); i++){
00044         const CollisionPairItem &cpi = prj.collisionPairs()[i];
00045         int bodyIndex1 = world.bodyIndex(cpi.objectName1);
00046         if (bodyIndex1 < 0){
00047             // different name is used for RTC
00048             if (prj.models().find(cpi.objectName1) != prj.models().end()){
00049                 bodyIndex1 
00050                     = world.bodyIndex(prj.models()[cpi.objectName1].rtcName);
00051             }
00052         }
00053         int bodyIndex2 = world.bodyIndex(cpi.objectName2);
00054         if (bodyIndex2 < 0){
00055             // different name is used for RTC
00056             if (prj.models().find(cpi.objectName2) != prj.models().end()){
00057                 bodyIndex2 
00058                     = world.bodyIndex(prj.models()[cpi.objectName2].rtcName);
00059             }
00060         }
00061 
00062         if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
00063             hrp::BodyPtr bodyPtr1 = world.body(bodyIndex1);
00064             hrp::BodyPtr bodyPtr2 = world.body(bodyIndex2);
00065 
00066             std::vector<hrp::Link*> links1;
00067             if(cpi.jointName1.empty()){
00068                 const hrp::LinkTraverse& traverse = bodyPtr1->linkTraverse();
00069                 links1.resize(traverse.numLinks());
00070                 std::copy(traverse.begin(), traverse.end(), links1.begin());
00071             } else {
00072                 links1.push_back(bodyPtr1->link(cpi.jointName1));
00073             }
00074 
00075             std::vector<hrp::Link*> links2;
00076             if(cpi.jointName2.empty()){
00077                 const hrp::LinkTraverse& traverse = bodyPtr2->linkTraverse();
00078                 links2.resize(traverse.numLinks());
00079                 std::copy(traverse.begin(), traverse.end(), links2.begin());
00080             } else {
00081                 links2.push_back(bodyPtr2->link(cpi.jointName2));
00082             }
00083 
00084             for(size_t j=0; j < links1.size(); ++j){
00085                 for(size_t k=0; k < links2.size(); ++k){
00086                     hrp::Link* link1 = links1[j];
00087                     hrp::Link* link2 = links2[k];
00088 
00089                     if(link1 && link2 && link1 != link2 
00090                        && link1->parent != link2 && link1 != link2->parent){
00091                         world.constraintForceSolver.addCollisionCheckLinkPair
00092                             (bodyIndex1, link1, bodyIndex2, link2, 
00093                              cpi.staticFriction, cpi.slidingFriction, 
00094                              cpi.cullingThresh, cpi.restitution, 0.0);
00095                         pairs.push_back(new hrp::ColdetLinkPair(link1, link2));
00096                     }
00097                 }
00098             }
00099         }
00100     }
00101 
00102     for (unsigned int i=0; i<prj.extraJoints().size(); i++){
00103         const ExtraJointItem &ej = prj.extraJoints()[i];
00104         int bodyIndex1 = world.bodyIndex(ej.object1Name);
00105         if (bodyIndex1 < 0){
00106             // different name is used for RTC
00107             if (prj.models().find(ej.object1Name) != prj.models().end()){
00108                 bodyIndex1 
00109                     = world.bodyIndex(prj.models()[ej.object1Name].rtcName);
00110             }
00111         }
00112         int bodyIndex2 = world.bodyIndex(ej.object2Name);
00113         if (bodyIndex2 < 0){
00114             // different name is used for RTC
00115             if (prj.models().find(ej.object2Name) != prj.models().end()){
00116                 bodyIndex2 
00117                     = world.bodyIndex(prj.models()[ej.object2Name].rtcName);
00118             }
00119         }
00120 
00121         if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
00122             hrp::BodyPtr bodyPtr1 = world.body(bodyIndex1);
00123             hrp::BodyPtr bodyPtr2 = world.body(bodyIndex2);
00124 
00125             hrp::Link *link1 = bodyPtr1->link(ej.link1Name);
00126             hrp::Link *link2 = bodyPtr2->link(ej.link2Name);
00127 
00128             int jtype;
00129             if (ej.jointType == "xyz"){
00130                 jtype = 0;
00131             }else if (ej.jointType == "xy"){
00132                 jtype = 1;
00133             }else if (ej.jointType == "z"){
00134                 jtype = 2;
00135             }
00136 
00137             if (link1 && link2){
00138                 world.constraintForceSolver.addExtraJoint(
00139                     bodyIndex1, link1, bodyIndex2, link2, 
00140                     ej.link1LocalPos.data(), ej.link2LocalPos.data(), 
00141                     jtype, ej.jointAxis.data());
00142             }
00143         }
00144     }
00145 
00146     world.enableSensors(true);
00147 
00148     int nBodies = world.numBodies();
00149     for(int i=0; i < nBodies; ++i){
00150         hrp::BodyPtr bodyPtr = world.body(i);
00151         bodyPtr->initializeConfiguration();
00152     }
00153         
00154     for (std::map<std::string, ModelItem>::iterator it=prj.models().begin();
00155          it != prj.models().end(); it++){
00156         hrp::BodyPtr body;
00157         if (it->second.rtcName != "") body = world.body(it->second.rtcName);
00158         if (!body) body = world.body(it->first);
00159         if (!body){
00160             std::cerr << "can't find a body named " << it->first << " or "
00161                       << it->second.rtcName << std::endl;
00162             continue;
00163         }
00164         for (std::map<std::string, JointItem>::iterator it2=it->second.joint.begin();
00165              it2 != it->second.joint.end(); it2++){
00166             hrp::Link *link = body->link(it2->first);
00167             if (!link) continue;
00168             if (link->isRoot()){
00169                 link->p = it2->second.translation;
00170                 link->setAttitude(it2->second.rotation);
00171                 link->v = it2->second.linearVelocity;
00172                 link->w = it2->second.angularVelocity;
00173                 link->vo = link->v - link->w.cross(link->p);
00174             }else{
00175                 link->q = it2->second.angle;
00176             }
00177         }
00178         body->calcForwardKinematics();
00179         body->setDefaultRootPosition(body->rootLink()->p, body->rootLink()->attitude());
00180     }
00181     world.initialize();
00182 #if 0
00183     world.constraintForceSolver.useBuiltinCollisionDetector(true);
00184 #endif
00185 }
00186 
00187 void initRTS(Project &prj, std::vector<ClockReceiver>& receivers)
00188 {
00189     RTC::Manager& manager = RTC::Manager::instance();
00190 
00191     RTSItem& rts = prj.RTS();
00192     // load factories
00193     for (std::map<std::string, RTSItem::rtc>::iterator it 
00194              = rts.components.begin(); it != rts.components.end(); it++){
00195         std::string path = it->second.path;
00196         if (path == "") continue;
00197 #ifdef __APPLE__
00198         path += ".dylib";
00199 #else
00200         path += ".so";
00201 #endif
00202         std::cout << "loading " << path << std::endl; 
00203         std::string initfunc = it->second.name + "Init";
00204         manager.load(path.c_str(), initfunc.c_str());
00205     }
00206     // create components
00207     for (std::map<std::string, RTSItem::rtc>::iterator it 
00208              = rts.components.begin(); it != rts.components.end(); it++){
00209         RTC::RTObject_impl *rtc = manager.getComponent(it->first.c_str());
00210         if (!rtc){
00211             if (it->second.name == ""){
00212                 std::cerr << "factory name for " << it->first << " is not defined" << std::endl;
00213                 continue;
00214             }                
00215             std::cout << "creating " << it->first << std::endl;
00216             std::string args = it->second.name + "?instance_name=" + it->first;
00217             rtc = manager.createComponent(args.c_str());
00218         }
00219         if (!rtc){
00220             std::cerr << "failed to create RTC(" << it->first << ")" << std::endl;
00221             continue;
00222         }
00223         RTC::ExecutionContextList_var eclist = rtc->get_owned_contexts();
00224         for(CORBA::ULong i=0; i < eclist->length(); ++i){
00225             if(!CORBA::is_nil(eclist[i])){
00226                 OpenRTM::ExtTrigExecutionContextService_var execContext = OpenRTM::ExtTrigExecutionContextService::_narrow(eclist[i]);
00227                 if(!CORBA::is_nil(execContext)){
00228                     std::cout << it->first << ":" << it->second.period << std::endl;
00229                     receivers.push_back(ClockReceiver(execContext, it->second.period));
00230                     execContext->activate_component(rtc->getObjRef());
00231                 }
00232             }
00233         }
00234         // set configuration
00235         {
00236             RTC::RTObject_var rtc = findRTC(it->first);
00237             for (size_t i=0; i<it->second.configuration.size(); i++){
00238                 setConfiguration(rtc, it->second.configuration[i].first,
00239                                  it->second.configuration[i].second);
00240             }
00241         }
00242     }
00243     // make connections
00244     for (std::vector<std::pair<std::string, std::string> >::iterator it
00245              = rts.connections.begin(); it != rts.connections.end(); it++){
00246         std::cout << "making a connection between "
00247                   << it->first << " and " << it->second << std::endl;
00248         int pos1 = it->first.find('.');
00249         std::string comp1 = it->first.substr(0, pos1);
00250         std::string port1 = it->first;
00251         int pos2 = it->second.find('.');
00252         std::string comp2 = it->second.substr(0, pos2);
00253         std::string port2 = it->second;
00254 
00255         RTC::RTObject_var rtc1 = findRTC(comp1);
00256         if (!rtc1){
00257             std::cerr << "can't find a component named " << comp1 << std::endl;
00258             return;
00259         }
00260         RTC::RTObject_var rtc2 = findRTC(comp2);
00261         if (!rtc2){
00262             std::cerr << "can't find a component named " << comp2 << std::endl;
00263             return;
00264         }
00265         RTC::PortServiceList_var ports1 = rtc1->get_ports();
00266         RTC::PortServiceList_var ports2 = rtc2->get_ports();
00267 
00268         RTC::PortService_ptr portObj1=NULL, portObj2=NULL; 
00269         for(CORBA::ULong i = 0; i < ports1->length(); ++i ){
00270             RTC::PortProfile_var profile = ports1[i]->get_port_profile();
00271             std::string portName(profile->name);
00272             if (portName == port1){
00273                 portObj1 = ports1[i];
00274                 break;
00275             }
00276         }
00277         if (!portObj1) {
00278             std::cerr << "can't find a port named " << port1 << std::endl;
00279             return; 
00280         }
00281         for(CORBA::ULong i = 0; i < ports2->length(); ++i ){
00282             RTC::PortProfile_var profile = ports2[i]->get_port_profile();
00283             std::string portName(profile->name);
00284             if (portName == port2){
00285                 portObj2 = ports2[i];
00286                 break;
00287             }
00288         }
00289         if (!portObj2) {
00290             std::cerr << "can't find a port named " << port2 << std::endl;
00291             return;
00292         }
00293         connectPorts(portObj1, portObj2);
00294     }
00295 }


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