World.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00013 #ifndef OPENHRP_WORLD_H_INCLUDED
00014 #define OPENHRP_WORLD_H_INCLUDED
00015 
00016 #include <vector>
00017 #include <map>
00018 #include <boost/shared_ptr.hpp>
00019 #include <boost/intrusive_ptr.hpp>
00020 #include <hrpUtil/Eigen3d.h>
00021 #include "Body.h"
00022 #include "ForwardDynamics.h"
00023 #include "Config.h"
00024 
00025 namespace OpenHRP {
00026         class CollisionSequence;
00027 }
00028 
00029 namespace hrp {
00030 
00031     class Link;
00032     class RangeSensor;
00033 
00034     class HRPMODEL_API WorldBase
00035     {
00036     public:
00037         WorldBase();
00038         virtual ~WorldBase();
00039 
00044         unsigned int numBodies() { return bodyInfoArray.size(); }
00045 
00051         BodyPtr body(int index);
00052 
00058         BodyPtr body(const std::string& name);
00059 
00065         ForwardDynamicsPtr forwardDynamics(int index) {
00066             return bodyInfoArray[index].forwardDynamics;
00067         }
00068 
00074         int bodyIndex(const std::string& name);
00075 
00082         int addBody(BodyPtr body);
00083 
00087         void clearBodies();
00088 
00092         void clearCollisionPairs();
00093 
00098         void setTimeStep(double dt);
00099 
00104         double timeStep(void) const { return timeStep_; }
00105         
00110         void setCurrentTime(double tm);
00111 
00116         double currentTime(void) const { return currentTime_; }
00117         
00122         void setGravityAcceleration(const Vector3& g);
00123 
00128         const Vector3& getGravityAcceleration() { return g; }
00129 
00135         void enableSensors(bool on);
00136 
00140         void setEulerMethod();
00141 
00145         void setRungeKuttaMethod();
00146 
00150         virtual void initialize();
00151 
00155         virtual void calcNextState();
00156 
00163         std::pair<int,bool> getIndexOfLinkPairs(Link* link1, Link* link2);
00164 
00165     protected:
00166 
00167         double currentTime_;
00168         double timeStep_;
00169 
00170         struct BodyInfo {
00171             BodyPtr body;
00172             ForwardDynamicsPtr forwardDynamics;
00173         };
00174         std::vector<BodyInfo> bodyInfoArray;
00175 
00176         bool sensorsAreEnabled;
00177 
00178     private:
00179         void updateRangeSensors();
00180         void updateRangeSensor(RangeSensor *sensor);
00181 
00182         typedef std::map<std::string, int> NameToIndexMap;
00183         NameToIndexMap nameToBodyIndexMap;
00184 
00185         typedef std::map<BodyPtr, int> BodyToIndexMap;
00186         BodyToIndexMap bodyToIndexMap;
00187                 
00188         struct LinkPairKey {
00189                         Link* link1;
00190                         Link* link2;
00191                         bool operator<(const LinkPairKey& pair2) const;
00192                 };
00193                 typedef std::map<LinkPairKey, int> LinkPairKeyToIndexMap;
00194                 LinkPairKeyToIndexMap linkPairKeyToIndexMap;
00195 
00196                 int numRegisteredLinkPairs;
00197                 
00198         Vector3 g;
00199 
00200         bool isEulerMethod; // Euler or Runge Kutta ?
00201 
00202     };
00203 
00204 
00205         template <class TConstraintForceSolver> class World : public WorldBase
00206         {
00207         public:
00208                 TConstraintForceSolver constraintForceSolver;
00209 
00210                 World() : constraintForceSolver(*this) { }
00211 
00212                 virtual void initialize() {
00213                         WorldBase::initialize();
00214                         constraintForceSolver.initialize();
00215                 }
00216 
00217                 virtual void calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence) {
00218                         constraintForceSolver.solve(corbaCollisionSequence);
00219                         WorldBase::calcNextState();
00220                 }
00221         };
00222 
00223 };
00224 
00225 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:57