hrplib/hrpModel/World.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * National Institute of Advanced Industrial Science and Technology (AIST)
 */
#ifndef OPENHRP_WORLD_H_INCLUDED
#define OPENHRP_WORLD_H_INCLUDED
 
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/intrusive_ptr.hpp>
#include <hrpUtil/Eigen3d.h>
#include "Body.h"
#include "ForwardDynamics.h"
#include "Config.h"
 
namespace OpenHRP {
        class CollisionSequence;
}
 
namespace hrp {
 
    class Link;
    class RangeSensor;
 
    class HRPMODEL_API WorldBase
    {
    public:
        WorldBase();
        virtual ~WorldBase();
 
        unsigned int numBodies() { return bodyInfoArray.size(); }
 
        BodyPtr body(int index);
 
        BodyPtr body(const std::string& name);
 
        ForwardDynamicsPtr forwardDynamics(int index) {
            return bodyInfoArray[index].forwardDynamics;
        }
 
        int bodyIndex(const std::string& name);
 
        int addBody(BodyPtr body);
 
        void clearBodies();
 
        void clearCollisionPairs();
 
        void setTimeStep(double dt);
 
        double timeStep(void) const { return timeStep_; }
        
        void setCurrentTime(double tm);
 
        double currentTime(void) const { return currentTime_; }
        
        void setGravityAcceleration(const Vector3& g);
 
        const Vector3& getGravityAcceleration() { return g; }
 
        void enableSensors(bool on);
 
        void setEulerMethod();
 
        void setRungeKuttaMethod();
 
        virtual void initialize();
 
        virtual void calcNextState();
 
        std::pair<int,bool> getIndexOfLinkPairs(Link* link1, Link* link2);
 
    protected:
 
        double currentTime_;
        double timeStep_;
 
        struct BodyInfo {
            BodyPtr body;
            ForwardDynamicsPtr forwardDynamics;
        };
        std::vector<BodyInfo> bodyInfoArray;
 
        bool sensorsAreEnabled;
 
    private:
        void updateRangeSensors();
        void updateRangeSensor(RangeSensor *sensor);
 
        typedef std::map<std::string, int> NameToIndexMap;
        NameToIndexMap nameToBodyIndexMap;
 
        typedef std::map<BodyPtr, int> BodyToIndexMap;
        BodyToIndexMap bodyToIndexMap;
                
        struct LinkPairKey {
                        Link* link1;
                        Link* link2;
                        bool operator<(const LinkPairKey& pair2) const;
                };
                typedef std::map<LinkPairKey, int> LinkPairKeyToIndexMap;
                LinkPairKeyToIndexMap linkPairKeyToIndexMap;
 
                int numRegisteredLinkPairs;
                
        Vector3 g;
 
        bool isEulerMethod; // Euler or Runge Kutta ?
 
    };
 
 
        template <class TConstraintForceSolver> class World : public WorldBase
        {
        public:
                TConstraintForceSolver constraintForceSolver;
 
                World() : constraintForceSolver(*this) { }
 
                virtual void initialize() {
                        WorldBase::initialize();
                        constraintForceSolver.initialize();
                }
 
                virtual void calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence) {
                        constraintForceSolver.solve(corbaCollisionSequence);
                        WorldBase::calcNextState();
                }
        };
 
};
 
#endif