SimulatorUtil.h
Go to the documentation of this file.
00001 #include <boost/bind.hpp>
00002 #include <boost/function.hpp>
00003 #include <rtm/CorbaNaming.h>
00004 #include <hrpModel/World.h>
00005 #include <hrpModel/ConstraintForceSolver.h>
00006 #include <hrpModel/ModelLoaderUtil.h>
00007 #include <hrpUtil/TimeMeasure.h>
00008 #include "Project.h"
00009 #include "BodyRTC.h"
00010 #include "ProjectUtil.h"
00011 #include "GLmodel.h"
00012 
00013 
00014 hrp::BodyPtr createBody(const std::string& name, const std::string& url,
00015                         std::vector<BodyRTCPtr> *bodies,
00016                         RTC::CorbaNaming *naming)
00017 {
00018     std::cout << "createBody(" << name << "," << url << ")" << std::endl;
00019     RTC::Manager& manager = RTC::Manager::instance();
00020     std::string args = "BodyRTC?instance_name="+name;
00021     BodyRTCPtr body = (BodyRTC *)manager.createComponent(args.c_str());
00022     if (!loadBodyFromModelLoader(body, url.c_str(), 
00023                                  CosNaming::NamingContext::_duplicate(naming->getRootContext()),
00024                                  true)){
00025         std::cerr << "failed to load model[" << url << "]" << std::endl;
00026         manager.deleteComponent(body.get());
00027         return hrp::BodyPtr();
00028     }else{
00029         body->createDataPorts();
00030         bodies->push_back(body);
00031         return body;
00032     }
00033 }
00034 
00035 class Simulator
00036 {
00037 public:
00038     void init(Project &prj, GLscene *i_scene, RTC::CorbaNaming &naming){
00039         BodyFactory factory = boost::bind(createBody, _1, _2, &bodies, &naming);
00040         initWorld(prj, factory, world);
00041         initRTS(prj, receivers);
00042         std::cout << "number of receivers:" << receivers.size() << std::endl;
00043         initWorldState(state, world);
00044         totalTime = prj.totalTime();
00045         scene = i_scene;
00046     }
00047 
00048     bool oneStep(){
00049         tm_control.begin();
00050         for (unsigned int i=0; i<bodies.size(); i++){
00051             bodies[i]->writeDataPorts();
00052         }
00053         
00054         for (unsigned int i=0; i<bodies.size(); i++){
00055             bodies[i]->readDataPorts();
00056         }
00057         
00058         for (unsigned int i=0; i<receivers.size(); i++){
00059             receivers[i].tick(world.timeStep());
00060         }
00061         tm_control.end();
00062         
00063         tm_dynamics.begin();
00064         world.constraintForceSolver.clearExternalForces();
00065         world.calcNextState(state.collisions);
00066 
00067         getWorldState(state, world);
00068         scene->addState(state);
00069         tm_dynamics.end();
00070 
00071         if (world.currentTime() > totalTime){
00072             std::cout << "controller:" << tm_control.totalTime() 
00073                       << "[s], " << tm_control.averageTime()*1000 
00074                       << "[ms/frame]" << std::endl;
00075             std::cout << "dynamics  :" << tm_dynamics.totalTime() 
00076                       << "[s], " << tm_dynamics.averageTime()*1000 
00077                       << "[ms/frame]" << std::endl;
00078         }
00079 
00080         return world.currentTime() <= totalTime;
00081     }
00082 
00083     TimeMeasure tm_collision, tm_dynamics, tm_control;
00084     hrp::World<hrp::ConstraintForceSolver> world;
00085     std::vector<BodyRTCPtr> bodies; 
00086     std::vector<ClockReceiver> receivers;
00087     OpenHRP::WorldState state;
00088     double totalTime;
00089     GLscene *scene;
00090 };


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