Simulator.cpp
Go to the documentation of this file.
00001 #include "Simulator.h"
00002 #include "hrpsys/util/BodyRTC.h"
00003 
00004 Simulator::Simulator(LogManager<SceneState> *i_log) 
00005   : log(i_log), adjustTime(false)
00006 {
00007 }
00008 
00009 void Simulator::init(Project &prj, BodyFactory &factory){
00010     initWorld(prj, factory, *this, pairs);
00011     initRTS(prj, receivers);
00012     std::cout << "number of receivers:" << receivers.size() << std::endl;
00013     m_totalTime = prj.totalTime();
00014     m_logTimeStep = prj.logTimeStep();
00015     m_kinematicsOnly = prj.kinematicsOnly();
00016     realTime(prj.realTime());
00017 
00018     collisions.length(pairs.size());
00019     for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
00020         hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
00021         hrp::Link *link0 = linkPair->link(0);
00022         hrp::Link *link1 = linkPair->link(1);
00023         OpenHRP::LinkPair& pair = collisions[colIndex].pair;
00024         pair.charName1 = CORBA::string_dup(link0->body->name().c_str());
00025         pair.charName2 = CORBA::string_dup(link1->body->name().c_str());
00026         pair.linkName1 = CORBA::string_dup(link0->name.c_str());
00027         pair.linkName2 = CORBA::string_dup(link1->name.c_str());
00028     }
00029 
00030     m_nextLogTime = 0;
00031     appendLog();
00032 }
00033 
00034 void Simulator::appendLog()
00035 {
00036     if (log && currentTime() >= m_nextLogTime){
00037         state.set(*this, collisions);
00038         log->add(state);
00039         m_nextLogTime += m_logTimeStep;
00040     }
00041 }
00042 
00043 void Simulator::checkCollision()
00044 {
00045     checkCollision(collisions);
00046 }
00047 
00048 void Simulator::checkCollision(OpenHRP::CollisionSequence &collisions)
00049 {
00050     for (unsigned int i=0; i<numBodies(); i++){
00051         body(i)->updateLinkColdetModelPositions();
00052     }
00053     for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
00054         hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
00055         OpenHRP::Collision& collision = collisions[colIndex];
00056         OpenHRP::CollisionPointSequence* pCollisionPoints = &collision.points;
00057         std::vector<hrp::collision_data>& cdata = linkPair->detectCollisions();
00058             
00059         if(cdata.empty()){
00060             pCollisionPoints->length(0);
00061         } else {
00062             int npoints = 0;
00063             for(unsigned int i = 0; i < cdata.size(); i++) {
00064                 for(int j = 0; j < cdata[i].num_of_i_points; j++){
00065                     if(cdata[i].i_point_new[j]) npoints++;
00066                 }
00067             }
00068             pCollisionPoints->length(npoints);
00069             int idx = 0;
00070             for (unsigned int i = 0; i < cdata.size(); i++) {
00071                 hrp::collision_data& cd = cdata[i];
00072                 for(int j=0; j < cd.num_of_i_points; j++){
00073                     if (cd.i_point_new[j]){
00074                         OpenHRP::CollisionPoint& point = (*pCollisionPoints)[idx];
00075                         for(int k=0; k < 3; k++){
00076                             point.position[k] = cd.i_points[j][k];
00077                         }
00078                         for(int k=0; k < 3; k++){
00079                             point.normal[k] = cd.n_vector[k];
00080                         }
00081                         point.idepth = cd.depth;
00082                         idx++;
00083                     }
00084                 }
00085             }
00086         }
00087     }
00088 }
00089 
00090 bool Simulator::oneStep(){
00091     ThreadedObject::oneStep();
00092 
00093     if (!currentTime()) gettimeofday(&beginTime, NULL);
00094     if (adjustTime){
00095         struct timeval tv;
00096         gettimeofday(&tv, NULL);
00097         startTimes.push_back(tv);
00098         if (startTimes.size() > 1.0/timeStep()){
00099             startTimes.pop_front();
00100         }
00101         if (startTimes.size() >= 2){
00102             const struct timeval& first = startTimes.front();
00103             const struct timeval& last  = startTimes.back();
00104             int realT = (last.tv_sec - first.tv_sec)*1e6
00105                 + (last.tv_usec - first.tv_usec);
00106             int simT = timeStep()*(startTimes.size()-1)*1e6;
00107             int usec = simT - realT;
00108             if (usec > 1000){
00109                 usleep(usec);
00110             }
00111         }
00112     }
00113 
00114     tm_control.begin();
00115     for (unsigned int i=0; i<numBodies(); i++){
00116         BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
00117         bodyrtc->writeDataPorts(currentTime());
00118     }
00119     
00120     for (unsigned int i=0; i<numBodies(); i++){
00121         BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
00122         bodyrtc->readDataPorts();
00123     }
00124     for (unsigned int i=0; i<numBodies(); i++){
00125         BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
00126         bodyrtc->preOneStep();
00127     }
00128     for (unsigned int i=0; i<receivers.size(); i++){
00129         receivers[i].tick(timeStep());
00130     }
00131     tm_control.end();
00132 
00133 #if 1
00134     tm_collision.begin();
00135     checkCollision(collisions);
00136     tm_collision.end();
00137 #endif
00138 
00139     tm_dynamics.begin();
00140     constraintForceSolver.clearExternalForces();
00141     if (m_kinematicsOnly){
00142         for (unsigned int i=0; i<numBodies(); i++){
00143             body(i)->calcForwardKinematics();
00144         }
00145         currentTime_ += timeStep();
00146     }else{
00147         calcNextState(collisions);
00148     }
00149     
00150     for (unsigned int i=0; i<numBodies(); i++){
00151         BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
00152         bodyrtc->postOneStep();
00153     }
00154     appendLog();
00155     tm_dynamics.end();
00156     
00157     if (m_totalTime && currentTime() > m_totalTime){
00158         struct timeval endTime;
00159         gettimeofday(&endTime, NULL);
00160         double realT = (endTime.tv_sec - beginTime.tv_sec)
00161             + (endTime.tv_usec - beginTime.tv_usec)/1e6;
00162         printf("total     :%8.3f[s], %8.3f[sim/real]\n",
00163                realT, m_totalTime/realT);
00164         printf("controller:%8.3f[s], %8.3f[ms/frame]\n",
00165                tm_control.totalTime(), tm_control.averageTime()*1000);
00166         printf("collision :%8.3f[s], %8.3f[ms/frame]\n",
00167                tm_collision.totalTime(), tm_collision.averageTime()*1000);
00168         printf("dynamics  :%8.3f[s], %8.3f[ms/frame]\n",
00169                tm_dynamics.totalTime(), tm_dynamics.averageTime()*1000);
00170         for (unsigned int i=0; i<numBodies(); i++){
00171             hrp::BodyPtr body = this->body(i);
00172             int ntri=0;
00173             for (unsigned int j=0; j<body->numLinks(); j++){
00174                 hrp::Link *l = body->link(j);
00175                 if (l && l->coldetModel){
00176                     ntri += l->coldetModel->getNumTriangles();
00177                 }
00178             }
00179             printf("num of triangles : %s : %d\n", body->name().c_str(), ntri);
00180         }
00181         fflush(stdout);
00182         return false;
00183     }else{
00184         return true;
00185     }
00186 }
00187 
00188 void Simulator::clear()
00189 {
00190     RTC::Manager* manager = &RTC::Manager::instance();
00191     for (unsigned int i=0; i<numBodies(); i++){
00192         BodyRTC *bodyrtc = dynamic_cast<BodyRTC *>(body(i).get());
00193         bodyrtc->exit();
00194     }
00195     manager->cleanupComponents();
00196     clearBodies();
00197     constraintForceSolver.clearCollisionCheckLinkPairs();
00198     setCurrentTime(0.0);
00199     pairs.clear();
00200     receivers.clear();
00201 }
00202 
00203 void Simulator::addCollisionCheckPair(BodyRTC *bodyPtr1, BodyRTC *bodyPtr2)
00204 {
00205     int bodyIndex1 = bodyIndex(bodyPtr1->name());
00206     int bodyIndex2 = bodyIndex(bodyPtr2->name());
00207 
00208     std::vector<hrp::Link*> links1;
00209     const hrp::LinkTraverse& traverse1 = bodyPtr1->linkTraverse();
00210     links1.resize(traverse1.numLinks());
00211     std::copy(traverse1.begin(), traverse1.end(), links1.begin());
00212     
00213     std::vector<hrp::Link*> links2;
00214     const hrp::LinkTraverse& traverse2 = bodyPtr2->linkTraverse();
00215     links2.resize(traverse2.numLinks());
00216     std::copy(traverse2.begin(), traverse2.end(), links2.begin());
00217     
00218     for(size_t j=0; j < links1.size(); ++j){
00219         for(size_t k=0; k < links2.size(); ++k){
00220             hrp::Link* link1 = links1[j];
00221             hrp::Link* link2 = links2[k];
00222             
00223             if(link1 && link2 && link1 != link2){
00224                 constraintForceSolver.addCollisionCheckLinkPair
00225                     (bodyIndex1, link1, bodyIndex2, link2, 
00226                      0.5, 0.5, 0.01, 0.0, 0.0);
00227                 pairs.push_back(new hrp::ColdetLinkPair(link1, link2));
00228             }
00229         }
00230     }
00231 
00232     collisions.length(pairs.size());
00233     for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
00234         hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
00235         hrp::Link *link0 = linkPair->link(0);
00236         hrp::Link *link1 = linkPair->link(1);
00237         OpenHRP::LinkPair& pair = collisions[colIndex].pair;
00238         pair.charName1 = CORBA::string_dup(link0->body->name().c_str());
00239         pair.charName2 = CORBA::string_dup(link1->body->name().c_str());
00240         pair.linkName1 = CORBA::string_dup(link0->name.c_str());
00241         pair.linkName2 = CORBA::string_dup(link1->name.c_str());
00242     }
00243 }
00244 
00245 void Simulator::kinematicsOnly(bool flag)
00246 {
00247     m_kinematicsOnly = flag;
00248 }


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