PySimulator.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 #include <boost/bind.hpp>
00003 #include <boost/function.hpp>
00004 #include <boost/python.hpp>
00005 #include <boost/foreach.hpp>
00006 #include <boost/range/value_type.hpp>
00007 #include <rtm/Manager.h>
00008 #include <rtm/CorbaNaming.h>
00009 #include <hrpModel/ModelLoaderUtil.h>
00010 #ifdef __APPLE__
00011 #include <GLUT/glut.h>
00012 #else
00013 #include <GL/glut.h>
00014 #endif
00015 #include <SDL_thread.h>
00016 #include "hrpsys/util/GLbody.h"
00017 #include "hrpsys/util/GLlink.h"
00018 #include "hrpsys/util/GLutil.h"
00019 #include "hrpsys/util/Project.h"
00020 #include "hrpsys/util/OpenRTMUtil.h"
00021 #include "hrpsys/util/BVutil.h"
00022 #include "PyBody.h"
00023 #include "PyLink.h"
00024 #include "PyShape.h"
00025 #include "PySimulator.h"
00026 
00027 using namespace std;
00028 using namespace hrp;
00029 using namespace OpenHRP;
00030 
00031 static hrp::Link *createLink() { return new PyLink(); }
00032 hrp::BodyPtr createBody(const std::string& name, const ModelItem& mitem,
00033                         ModelLoader_ptr modelloader, GLscene *scene,
00034                         bool usebbox)
00035 {
00036     std::cout << "createBody(" << name << "," << mitem.url << ")" << std::endl;
00037     RTC::Manager& manager = RTC::Manager::instance();
00038     std::string args = "PyBody?instance_name="+name;
00039     PyBody *pybody = (PyBody *)manager.createComponent(args.c_str());
00040     hrp::BodyPtr body = hrp::BodyPtr(pybody);
00041     BodyInfo_var binfo;
00042     try{
00043         OpenHRP::ModelLoader::ModelLoadOption opt;
00044         opt.readImage = true;
00045         opt.AABBdata.length(0);
00046         opt.AABBtype = OpenHRP::ModelLoader::AABB_NUM;
00047         binfo = modelloader->getBodyInfoEx(mitem.url.c_str(), opt);
00048     }catch(OpenHRP::ModelLoader::ModelLoaderException ex){
00049         std::cerr << ex.description << std::endl;
00050         return hrp::BodyPtr();
00051     }
00052     if (!loadBodyFromBodyInfo(body, binfo, true, GLlinkFactory)){
00053         std::cerr << "failed to load model[" << mitem.url << "]" << std::endl;
00054         manager.deleteComponent(pybody);
00055         return hrp::BodyPtr();
00056     }else{
00057         if (usebbox) convertToAABB(body);
00058         for (std::map<std::string, JointItem>::const_iterator it2=mitem.joint.begin();
00059              it2 != mitem.joint.end(); it2++){
00060             hrp::Link *link = body->link(it2->first);
00061             if (!link) continue;
00062             link->isHighGainMode = it2->second.isHighGain;
00063             if (it2->second.collisionShape == ""){
00064                 // do nothing
00065             }else if (it2->second.collisionShape == "convex hull"){
00066                 convertToConvexHull(link);
00067             }else if (it2->second.collisionShape == "AABB"){
00068                 convertToAABB(link);
00069             }else{
00070                 std::cerr << "unknown value of collisionShape property:" 
00071                           << it2->second.collisionShape << std::endl;
00072             }
00073         }
00074         for (size_t i=0; i<mitem.inports.size(); i++){
00075             pybody->createInPort(mitem.inports[i]);
00076         }
00077         for (size_t i=0; i<mitem.outports.size(); i++){
00078             pybody->createOutPort(mitem.outports[i]);
00079         }
00080         loadShapeFromBodyInfo(pybody, binfo, createPyShape);
00081         body->setName(name);
00082         scene->addBody(body);
00083         return body;
00084     }
00085 }
00086 
00087 PySimulator::PySimulator() : 
00088     Simulator(&log), scene(&log), window(&scene, &log, this), manager(NULL),
00089     useBBox(false), maxLogLen(60)
00090 {
00091     initRTCmanager();
00092 }
00093 
00094 PySimulator::PySimulator(PyObject *pyo) : 
00095     Simulator(&log), scene(&log), window(&scene, &log, this), manager(NULL),
00096     useBBox(false)
00097 {
00098     initRTCmanager(pyo);
00099 }
00100 
00101 PySimulator::~PySimulator(){
00102     stop();
00103     window.stop();
00104     clear();
00105     if (manager) manager->shutdown();
00106 }
00107 
00108 void PySimulator::initViewer()
00109 {
00110     window.start();
00111 }
00112 
00113 void PySimulator::initRTCmanager()
00114 {
00115     int argc=1;
00116     char *argv[] = {(char *)"dummy"};
00117     initRTCmanager(argc, argv);
00118 }
00119 
00120 void PySimulator::initRTCmanager(int argc, char **argv)
00121 {
00122     manager = RTC::Manager::init(argc, argv);
00123     manager->init(argc, argv);
00124     PyBody::moduleInit(manager);
00125     manager->activateManager();
00126     manager->runManager(true);
00127 }
00128 
00129 void PySimulator::initRTCmanager(PyObject *pyo)
00130 {
00131     std::vector<char *> args(PySequence_Size(pyo)+1);
00132     args[0] = (char *)"dummy";
00133     for (int i=0; i<PySequence_Size(pyo); i++) {
00134         args[i+1] = boost::python::extract<char *>(PySequence_GetItem(pyo, i));
00135     }
00136     initRTCmanager(args.size(), &args[0]);
00137 }
00138 
00139 PyBody* PySimulator::loadBody(std::string name, std::string url){
00140     RTC::Manager* manager = &RTC::Manager::instance();
00141     std::string nameServer = manager->getConfig()["corba.nameservers"];
00142     int comPos = nameServer.find(",");
00143     if (comPos < 0){
00144         comPos = nameServer.length();
00145     }
00146     nameServer = nameServer.substr(0, comPos);
00147     RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
00148     
00149     ModelLoader_var modelloader = getModelLoader(CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00150     OpenHRP::BodyInfo_var binfo;
00151     try{
00152         binfo = modelloader->loadBodyInfo(url.c_str());
00153     }catch(OpenHRP::ModelLoader::ModelLoaderException ex){
00154         std::cerr << ex.description << std::endl;
00155         return NULL;
00156     }
00157     std::string args = "PyBody?instance_name="+name;
00158     PyBody *pybody = (PyBody *)manager->createComponent(args.c_str());
00159     pybody->setListener(this);
00160     hrp::BodyPtr body = hrp::BodyPtr(pybody);
00161     if (!loadBodyFromBodyInfo(body, binfo, true, createLink)){
00162         std::cerr << "failed to load model[" << url << "]" << std::endl;
00163         manager->deleteComponent(pybody);
00164         return NULL;
00165     }else{
00166         if (useBBox) convertToAABB(body);
00167         body->setName(name);
00168         addBody(body);
00169         loadShapeFromBodyInfo(pybody, binfo, createPyShape);
00170         scene.addBody(body);
00171         return pybody;
00172     }
00173 }
00174 
00175 bool PySimulator::loadProject(std::string fname){
00176     clear();
00177 
00178     Project prj;
00179     if (!prj.parse(fname)){
00180         std::cerr << "failed to parse " << fname << std::endl;
00181         return false;
00182     }
00183     
00184     RTC::Manager* manager = &RTC::Manager::instance();
00185     std::string nameServer = manager->getConfig()["corba.nameservers"];
00186     int comPos = nameServer.find(",");
00187     if (comPos < 0){
00188         comPos = nameServer.length();
00189     }
00190     nameServer = nameServer.substr(0, comPos);
00191     RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
00192     
00193     ModelLoader_var modelloader = getModelLoader(CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00194     //================= setup Simulator ======================
00195     BodyFactory factory = boost::bind(::createBody, _1, _2, modelloader, 
00196                                       &scene, useBBox);
00197     init(prj, factory);
00198     for (unsigned int i=0; i<numBodies(); i++){
00199         PyBody *pybody = dynamic_cast<PyBody *>(body(i).get());
00200         pybody->setListener(this);
00201     }
00202     window.setView(prj.view().T);
00203     
00204     std::cout << "timestep = " << prj.timeStep() << ", total time = " 
00205               << prj.totalTime() << std::endl;
00206     return true;
00207 }
00208 
00209 void PySimulator::simulate()
00210 {
00211     while(oneStep());
00212 }
00213 
00214 void PySimulator::simulate(double time)
00215 {
00216     setTotalTime(currentTime()+time);
00217     simulate();
00218 }
00219 
00220 void PySimulator::start(double time)
00221 {
00222     setTotalTime(currentTime()+time);
00223     Simulator::start();
00224 }
00225 
00226 void PySimulator::endless(bool flag)
00227 {
00228     if (flag){
00229         setTotalTime(0);
00230         log.enableRingBuffer(maxLogLen/timeStep());
00231     }
00232 } 
00233 
00234 void PySimulator::clear()
00235 {
00236     Simulator::clear();
00237     log.clear();
00238     if (window.isRunning()) scene.requestClear();
00239 }
00240 
00241 void PySimulator::play()
00242 {
00243     log.play();
00244 }
00245 
00246 void PySimulator::pause()
00247 {
00248     log.play();
00249 }
00250 
00251 void PySimulator::notifyChanged()
00252 {
00253     appendLog();
00254 }
00255 
00256 PyBody *PySimulator::createBody(std::string name)
00257 {
00258     RTC::Manager* manager = &RTC::Manager::instance();
00259     std::string args = "PyBody?instance_name="+name;
00260     PyBody *pybody = (PyBody *)manager->createComponent(args.c_str());
00261     pybody->setListener(this);
00262     pybody->setName(name);
00263     PyLink *root = new PyLink();
00264     root->name = "root";
00265     pybody->setRootLink(root);
00266     
00267     hrp::BodyPtr body = hrp::BodyPtr(pybody);
00268     addBody(body);
00269     scene.addBody(body);
00270 
00271     return pybody;
00272 }
00273 
00274 PyObject *PySimulator::bodies()
00275 {
00276     boost::python::list retval;
00277     for (unsigned int i=0; i<numBodies(); i++){
00278         PyBody *b = dynamic_cast<PyBody *>(body(i).get());
00279         retval.append(boost::python::ptr(b));
00280     }
00281     return boost::python::incref(retval.ptr());
00282 }
00283 
00284 void PySimulator::addCollisionCheckPair(PyBody *b1, PyBody *b2)
00285 {
00286     Simulator::addCollisionCheckPair(b1, b2);
00287 }
00288 
00289 void PySimulator::capture(std::string fname)
00290 {
00291     scene.requestCapture(fname.c_str());
00292 }
00293 
00294 unsigned int PySimulator::logLength()
00295 {
00296     return log.length();
00297 }
00298 
00299 PyBody *PySimulator::getBody(std::string name)
00300 {
00301     return dynamic_cast<PyBody *>(body(name).get());
00302 }
00303 
00304 bool PySimulator::showSensors()
00305 {
00306     return scene.showSensors();
00307 }
00308 
00309 void PySimulator::setShowSensors(bool flag)
00310 {
00311     scene.showSensors(flag);
00312 }
00313 
00314 void PySimulator::setMaxLogLength(double len)
00315 {
00316     maxLogLen = len;
00317 }
00318 
00319 double PySimulator::maxLogLength()
00320 {
00321     return maxLogLen;
00322 }
00323 
00324 void PySimulator::reset()
00325 {
00326     log.clear();
00327     setCurrentTime(0.0);
00328     for (unsigned int i=0; i<numBodies(); i++){
00329         body(i)->initializeConfiguration();
00330     }
00331     checkCollision();
00332     appendLog();
00333 }
00334 
00335 void PySimulator::setUseBBox(bool flag)
00336 {
00337     useBBox = flag;
00338 }
00339 
00340 void PySimulator::setWindowSize(int s)
00341 {
00342     window.setSize(s,s);
00343 }
00344 
00345 BOOST_PYTHON_MODULE( hrpsysext )
00346 {
00347     using namespace boost::python;
00348 
00349     class_<PySimulator, boost::noncopyable>("Simulator")
00350         .def(init<PyObject *>())
00351         .def("initViewer", &PySimulator::initViewer)
00352         .def("loadBody", &PySimulator::loadBody, return_internal_reference<>())
00353         .def("createBody", &PySimulator::createBody, return_internal_reference<>())
00354         .def("addCollisionCheckPair", &PySimulator::addCollisionCheckPair)
00355         .def("loadProject", &PySimulator::loadProject)
00356         .def("oneStep", &PySimulator::oneStep)
00357         .def("simulate", (void(PySimulator::*)())&PySimulator::simulate)
00358         .def("simulate", (void(PySimulator::*)(double))&PySimulator::simulate)
00359         .def("realTime", &PySimulator::realTime)
00360         .def("useBBox", &PySimulator::setUseBBox)
00361         .def("windowSize", &PySimulator::setWindowSize)
00362         .def("endless", &PySimulator::endless)
00363         .def("start", &PySimulator::start)
00364         .def("stop", &PySimulator::stop)
00365         .def("wait", &PySimulator::wait)
00366         .def("clear", &PySimulator::clear)
00367         .def("reset", &PySimulator::reset)
00368         .def("play", &PySimulator::play)
00369         .def("pause", &PySimulator::pause)
00370         .def("capture", &PySimulator::capture)
00371         .def("logLength", &PySimulator::logLength)
00372         .def("body", &PySimulator::getBody, return_internal_reference<>())
00373         .def("bodies", &PySimulator::bodies)
00374         .def("initialize", &PySimulator::initialize)
00375         .add_property("timeStep", 
00376                       &PySimulator::timeStep, &PySimulator::setTimeStep)
00377         .add_property("time", &PySimulator::currentTime)
00378         .add_property("totalTime", 
00379                       &PySimulator::totalTime, &PySimulator::setTotalTime)
00380         .add_property("showSensors", 
00381                       &PySimulator::showSensors, &PySimulator::setShowSensors)
00382         .add_property("maxLogLength", 
00383                       &PySimulator::maxLogLength, &PySimulator::setMaxLogLength)
00384         ;
00385 
00386     class_<PyBody, boost::noncopyable>("Body", no_init)
00387         .def("calcForwardKinematics", &PyBody::calcForwardKinematics)
00388         .def("rootLink", &PyBody::rootLink, return_internal_reference<>())
00389         .def("link", &PyBody::link, return_internal_reference<>())
00390         .def("links", &PyBody::links)
00391         .def("joint", &PyBody::joint, return_internal_reference<>())
00392         .def("joints", &PyBody::joints)
00393         .def("calcTotalMass", &PyBody::calcTotalMass)
00394         .def("calcCM", &PyBody::calcCM)
00395         .def("totalMass", &PyBody::totalMass)
00396         .def("numJoints", &PyBody::numJoints)
00397         .def("numLinks", &PyBody::numLinks)
00398         .def("creteInPort", &PyBody::createInPort)
00399         .def("creteOutPort", &PyBody::createOutPort)
00400         .add_property("name", &PyBody::getName, &PyBody::setName)
00401         .add_property("p", &PyBody::getPosition, &PyBody::setPosition)
00402         .add_property("R", &PyBody::getRotation, &PyBody::setRotation)
00403         .add_property("q", &PyBody::getPosture, &PyBody::setPosture)
00404         ;
00405 
00406     class_<PyLink, boost::noncopyable>("Link", no_init)
00407         .def("addChildLink", &PyLink::addChildLink, return_internal_reference<>()) 
00408         .def("addShapeFromFile", &PyLink::addShapeFromFile)
00409         .def("addCube", &PyLink::addCube, return_internal_reference<>())
00410         .def("parent", &PyLink::getParent, return_internal_reference<>())
00411         .def("children", &PyLink::getChildren)
00412         .def("showAxes", &PyLink::showAxes)
00413         .def("shapes", &PyLink::shapes)
00414         .def("highlight", &PyLink::highlight)
00415         .def_readwrite("name", &PyLink::name)
00416         .def_readwrite("m", &PyLink::m)
00417         .def_readwrite("u", &PyLink::u)
00418         .def_readwrite("Ir", &PyLink::Ir)
00419         .def_readwrite("gearRatio", &PyLink::gearRatio)
00420         .def_readwrite("dq", &PyLink::dq)
00421         .def_readwrite("ddq", &PyLink::ddq)
00422         .def_readwrite("isHighGainMode", &PyLink::isHighGainMode)
00423         .add_property("jointId", &PyLink::getJointId, &PyLink::setJointId)
00424         .add_property("q", &PyLink::getPosture, &PyLink::setPosture)
00425         .add_property("p", &PyLink::getPosition, &PyLink::setPosition)
00426         .add_property("R", &PyLink::getRotation, &PyLink::setRotation)
00427         .add_property("b", &PyLink::getRelPosition, &PyLink::setRelPosition)
00428         .add_property("Rs", &PyLink::getRelRotation, &PyLink::setRelRotation)
00429         .add_property("a", &PyLink::getRotationAxis, &PyLink::setRotationAxis)
00430         .add_property("d", &PyLink::getTranslationAxis, &PyLink::setTranslationAxis)
00431         .add_property("c", &PyLink::getCoM, &PyLink::setCoM)
00432         .add_property("I", &PyLink::getInertia, &PyLink::setInertia)
00433         .add_property("v", &PyLink::getLinVel, &PyLink::setLinVel)
00434         .add_property("w", &PyLink::getAngVel, &PyLink::setAngVel)
00435         .add_property("jointType", &PyLink::getJointType, &PyLink::setJointType)
00436         ;
00437 
00438     class_<PyShape, boost::noncopyable>("Shape", no_init)
00439         .add_property("b", &PyShape::getRelPosition, &PyShape::setRelPosition)
00440         .add_property("Rs", &PyShape::getRelRotation, &PyShape::setRelRotation)
00441         .add_property("diffuse", &PyShape::getDiffuseColor, &PyShape::setDiffuseColor)
00442         ;
00443 }


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