Simulator.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/CorbaNaming.h>
00011 
00012 #include "Simulator.h"
00013 #include <hrpCorba/OpenHRPCommon.hh>
00014 #include <hrpModel/ModelLoaderUtil.h>
00015 #include <hrpModel/OnlineViewerUtil.h>
00016 #include <hrpModel/Link.h>
00017 #include "hrpsys/util/Project.h"
00018 
00019 // Module specification
00020 // <rtc-template block="module_spec">
00021 static const char* component_spec[] =
00022 {
00023     "implementation_id", "Simulator",
00024     "type_name",         "Simulator",
00025     "description",       "dynamics simulator component",
00026     "version",           HRPSYS_PACKAGE_VERSION,
00027     "vendor",            "AIST",
00028     "category",          "example",
00029     "activity_type",     "DataFlowComponent",
00030     "max_instance",      "10",
00031     "language",          "C++",
00032     "lang_type",         "compile",
00033     // Configuration variables
00034     "conf.default.project", "",
00035     "conf.default.kinematics_only", "0",
00036     "conf.default.useOLV", "0",
00037     ""
00038 };
00039 // </rtc-template>
00040 
00041 Simulator::Simulator(RTC::Manager* manager)
00042     : RTC::DataFlowComponentBase(manager),
00043       // <rtc-template block="initializer">
00044       m_sceneStateOut("state", m_sceneState),
00045       // </rtc-template>
00046       dummy(0)
00047 {
00048 }
00049 
00050 Simulator::~Simulator()
00051 {
00052 }
00053 
00054 
00055 
00056 RTC::ReturnCode_t Simulator::onInitialize()
00057 {
00058     std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00059     // <rtc-template block="bind_config">
00060     // Bind variables and configuration variable
00061     bindParameter("project", m_project, "");  
00062     bindParameter("kinematics_only", m_kinematicsOnly, "0");  
00063     bindParameter("useOLV", m_useOLV, "0");  
00064   
00065     // </rtc-template>
00066 
00067     // Registration: InPort/OutPort/Service
00068     // <rtc-template block="registration">
00069     // Set InPort buffers
00070 
00071     // Set OutPort buffer
00072     addOutPort("state", m_sceneStateOut);
00073   
00074     // Set service provider to Ports
00075   
00076     // Set service consumers to Ports
00077   
00078     // Set CORBA Service Ports
00079   
00080     // </rtc-template>
00081 
00082     //RTC::Properties& prop = getProperties();
00083 
00084     return RTC::RTC_OK;
00085 }
00086 
00087 
00088 
00089 /*
00090   RTC::ReturnCode_t Simulator::onFinalize()
00091   {
00092   return RTC::RTC_OK;
00093   }
00094 */
00095 
00096 /*
00097   RTC::ReturnCode_t Simulator::onStartup(RTC::UniqueId ec_id)
00098   {
00099   return RTC::RTC_OK;
00100   }
00101 */
00102 
00103 /*
00104   RTC::ReturnCode_t Simulator::onShutdown(RTC::UniqueId ec_id)
00105   {
00106   return RTC::RTC_OK;
00107   }
00108 */
00109 
00110 RTC::ReturnCode_t Simulator::onActivated(RTC::UniqueId ec_id)
00111 {
00112     std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
00113 
00114     if (m_project == ""){
00115         std::cerr << "Project file is not specified." << std::endl;
00116         return RTC::RTC_ERROR;
00117     }
00118 
00119     Project prj;
00120     if (!prj.parse(m_project)) return RTC::RTC_ERROR;
00121 
00122     if ( m_kinematicsOnly == false ) {
00123         m_kinematicsOnly = prj.kinematicsOnly();
00124     }
00125     std::cout << "kinematics_only : " << m_kinematicsOnly << std::endl;
00126 
00127     m_world.clearBodies();
00128     m_world.constraintForceSolver.clearCollisionCheckLinkPairs();
00129     m_world.setCurrentTime(0.0);
00130     m_world.setTimeStep(prj.timeStep());
00131     std::cout << "time step = " << prj.timeStep() << std::endl;
00132     if(prj.isEuler()){
00133         m_world.setEulerMethod();
00134     } else {
00135         m_world.setRungeKuttaMethod();
00136     }
00137 
00138     RTC::Manager& rtcManager = RTC::Manager::instance();
00139     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00140     int comPos = nameServer.find(",");
00141     if (comPos < 0){
00142         comPos = nameServer.length();
00143     }
00144     nameServer = nameServer.substr(0, comPos);
00145     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00146 
00147     std::cout << "m_useOLV:" << m_useOLV << std::endl;
00148     if (m_useOLV){
00149         m_olv = hrp::getOnlineViewer(CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00150     }
00151 
00152     for (std::map<std::string, ModelItem>::iterator it=prj.models().begin();
00153          it != prj.models().end(); it++){
00154         RTCBodyPtr body(new RTCBody());
00155         if (!loadBodyFromModelLoader(body, it->second.url.c_str(), 
00156                                      CosNaming::NamingContext::_duplicate(naming.getRootContext()),
00157                                      true)){
00158             std::cerr << "failed to load model[" << it->second.url << "]" << std::endl;
00159         }else{
00160             body->setName(it->first);
00161             for (std::map<std::string, JointItem>::iterator it2=it->second.joint.begin();
00162                  it2 != it->second.joint.end(); it2++){
00163                 hrp::Link *link = body->link(it2->first);
00164                 if (link) link->isHighGainMode = it2->second.isHighGain;
00165             }
00166             m_world.addBody(body);
00167             body->createPorts(this);
00168             m_bodies.push_back(body);
00169         }
00170         if (m_useOLV){
00171             m_olv->load(it->first.c_str(), it->second.url.c_str());
00172         }
00173     }
00174     if (m_useOLV){
00175         m_olv->clearLog();
00176         initWorldState(m_state, m_world); 
00177     }
00178 
00179 
00180     for (unsigned int i=0; i<prj.collisionPairs().size(); i++){
00181         const CollisionPairItem &cpi = prj.collisionPairs()[i];
00182         int bodyIndex1 = m_world.bodyIndex(cpi.objectName1);
00183         int bodyIndex2 = m_world.bodyIndex(cpi.objectName2);
00184 
00185         if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
00186             hrp::BodyPtr bodyPtr1 = m_world.body(bodyIndex1);
00187             hrp::BodyPtr bodyPtr2 = m_world.body(bodyIndex2);
00188 
00189             std::vector<hrp::Link*> links1;
00190             if(cpi.jointName1.empty()){
00191                 const hrp::LinkTraverse& traverse = bodyPtr1->linkTraverse();
00192                 links1.resize(traverse.numLinks());
00193                 std::copy(traverse.begin(), traverse.end(), links1.begin());
00194             } else {
00195                 links1.push_back(bodyPtr1->link(cpi.jointName1));
00196             }
00197 
00198             std::vector<hrp::Link*> links2;
00199             if(cpi.jointName2.empty()){
00200                 const hrp::LinkTraverse& traverse = bodyPtr2->linkTraverse();
00201                 links2.resize(traverse.numLinks());
00202                 std::copy(traverse.begin(), traverse.end(), links2.begin());
00203             } else {
00204                 links2.push_back(bodyPtr2->link(cpi.jointName2));
00205             }
00206 
00207             for(size_t j=0; j < links1.size(); ++j){
00208                 for(size_t k=0; k < links2.size(); ++k){
00209                     hrp::Link* link1 = links1[j];
00210                     hrp::Link* link2 = links2[k];
00211 
00212                     if(link1 && link2 && link1 != link2){
00213                         m_world.constraintForceSolver.addCollisionCheckLinkPair
00214                             (bodyIndex1, link1, bodyIndex2, link2, 
00215                              cpi.staticFriction, cpi.slidingFriction, 0.01, 0.0, 0.0);
00216                     }
00217                 }
00218             }
00219         }
00220     }
00221 
00222     m_world.enableSensors(false);
00223   
00224     int nBodies = m_world.numBodies();
00225     for(int i=0; i < nBodies; ++i){
00226         hrp::BodyPtr bodyPtr = m_world.body(i);
00227         bodyPtr->initializeConfiguration();
00228     }
00229 
00230     for (std::map<std::string, ModelItem>::iterator it=prj.models().begin();
00231          it != prj.models().end(); it++){
00232         hrp::BodyPtr body = m_world.body(it->first);
00233         for (std::map<std::string, JointItem>::iterator it2=it->second.joint.begin();
00234              it2 != it->second.joint.end(); it2++){
00235             hrp::Link *link = body->link(it2->first);
00236             if (!link) continue;
00237             if (link->isRoot()){
00238                 link->p = it2->second.translation;
00239                 link->setAttitude(it2->second.rotation);
00240             }else{
00241                 link->q = it2->second.angle;
00242             }
00243         }
00244         body->calcForwardKinematics();
00245     }
00246   
00247     m_world.initialize();
00248     m_world.constraintForceSolver.useBuiltinCollisionDetector(true);
00249     m_world.constraintForceSolver.enableConstraintForceOutput(true);
00250   
00251     m_sceneState.states.length(m_bodies.size());
00252     for (unsigned int i=0; i<m_bodies.size(); i++){
00253         m_sceneState.states[i].name = CORBA::string_dup(m_bodies[i]->name().c_str());
00254         m_sceneState.states[i].q.length(m_bodies[i]->numJoints());
00255     }
00256 
00257 
00258     return RTC::RTC_OK;
00259 }
00260 
00261 RTC::ReturnCode_t Simulator::onDeactivated(RTC::UniqueId ec_id)
00262 {
00263     std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
00264     return RTC::RTC_OK;
00265 }
00266 
00267 RTC::ReturnCode_t Simulator::onExecute(RTC::UniqueId ec_id)
00268 {
00269     //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00270     // output current state
00271     m_sceneState.time = m_world.currentTime();
00272     for (unsigned int i=0; i<m_bodies.size(); i++){
00273         m_bodies[i]->output(m_sceneState.states[i]);
00274     }
00275     m_sceneStateOut.write();
00276 
00277     // input command
00278     for (unsigned int i=0; i<m_bodies.size(); i++) m_bodies[i]->input();
00279 
00280     if (m_kinematicsOnly){
00281         for(unsigned int i=0; i < m_world.numBodies(); ++i){
00282             m_world.body(i)->calcForwardKinematics();
00283         }
00284         m_world.setCurrentTime(m_world.currentTime() + m_world.timeStep());
00285     }else{
00286         m_world.constraintForceSolver.clearExternalForces();
00287     
00288         OpenHRP::CollisionSequence collision;
00289         m_world.calcNextState(collision);
00290     }
00291 
00292     if (m_useOLV){
00293         getWorldState(m_state, m_world);
00294         m_olv->update( m_state );
00295     }
00296     return RTC::RTC_OK;
00297 }
00298 
00299 /*
00300   RTC::ReturnCode_t Simulator::onAborting(RTC::UniqueId ec_id)
00301   {
00302   return RTC::RTC_OK;
00303   }
00304 */
00305 
00306 /*
00307   RTC::ReturnCode_t Simulator::onError(RTC::UniqueId ec_id)
00308   {
00309   return RTC::RTC_OK;
00310   }
00311 */
00312 
00313 /*
00314   RTC::ReturnCode_t Simulator::onReset(RTC::UniqueId ec_id)
00315   {
00316   return RTC::RTC_OK;
00317   }
00318 */
00319 
00320 /*
00321   RTC::ReturnCode_t Simulator::onStateUpdate(RTC::UniqueId ec_id)
00322   {
00323   return RTC::RTC_OK;
00324   }
00325 */
00326 
00327 /*
00328   RTC::ReturnCode_t Simulator::onRateChanged(RTC::UniqueId ec_id)
00329   {
00330   return RTC::RTC_OK;
00331   }
00332 */
00333 
00334 
00335 
00336 extern "C"
00337 {
00338 
00339     void SimulatorInit(RTC::Manager* manager)
00340     {
00341         RTC::Properties profile(component_spec);
00342         manager->registerFactory(profile,
00343                                  RTC::Create<Simulator>,
00344                                  RTC::Delete<Simulator>);
00345     }
00346 
00347 };
00348 
00349 


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