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 };