cens_serialized_engine.h

Go to the documentation of this file.

// Computational Embodied Neuroscience Simulator (CENS) Library
// Copyright (c) 2013 Francesco Mannella
//
// cens_engine.h
// Copyright (c) 2013 Francesco Mannella
//
// This file is part of CENS library.
//
// CENS library is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// CENS library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with CENS library.  If not, see <http://www.gnu.org/licenses/>.

#ifndef CENS_SERIALIZED_ENGINE_H
#define CENS_SERIALIZED_ENGINE_H

#include "cens_engine.h"
#include "cens_parameter_manager.h"
#include <BulletWorldImporter/btBulletWorldImporter.h>
#include <vector>
#include <map>
#include <string>
#include <sstream>


namespace cens 
{


    // Inherited by btHashMap to get the array of keys
    template <class Key, class Value>
        class CENSHashMap : public btHashMap<Key, Value>
    {
        public:
            const Key &getKeyAtIndex(int index)
            {
                return (btHashMap<Key, Value>::m_keyArray)[index]; 
            }
    };

 
    // Inherited from btBulletWorldImporter to get 
    // the protected maps of rigid bodies and constraints
    // when loading a bullet file
    class CENSImporter : public btBulletWorldImporter
    {
        public:

            CENSImporter(btDynamicsWorld* world=0)
                : btBulletWorldImporter(world) 
            { 
            };

            virtual ~CENSImporter()
            {
            };

            btAlignedObjectArray< char * > &getAllocatedNames()
            {
                return m_allocatedNames;
            }


            btHashMap<btHashString,btRigidBody*> &getBodyMap()
            {
                return m_nameBodyMap;
            }

            btHashMap<btHashString,btTypedConstraint*> &getConstraintMap()
            {
                return m_nameConstraintMap;
            }
    };


    struct CENSHingeData
    {
         CENSHingeConstraint *hinge;
        std::string motor_type;
        std::string  enabled;
        double max_motor_impulse;
        double kp;
        double ki;
        double kd;
        double dyn_kd;
        double motor_dt;
        double current_angle;
        double lower_limit;
        double upper_limit;
        std::string enable_collision;

        CENSHingeData():  
            hinge(0),
            motor_type(CENS_DEFAULT_CONTROL),
            enabled("TRUE"),
            max_motor_impulse(50),
            kp(.99),
            ki(.0),
            kd(.01),
            dyn_kd(.01),
            motor_dt(0.01),
            current_angle(0),
            lower_limit(-M_PI),
            upper_limit(M_PI),
            enable_collision("FALSE")
        {
        }

        ~CENSHingeData(){}
    };


    struct CENSSerializedRobot : public CENSParameterManager 
    {
        typedef std::map<std::string, CENSHingeData> Hinges;
        typedef std::map<std::string, btTypedConstraint *> GenericConstraints;
        typedef std::map<std::string, btRigidBody *> Bodies;
        typedef std::map<std::string, btTransform> Transforms;
        
        std::string robotName;
        
        Hinges hinges;

        GenericConstraints constraints;
 
        Bodies bodies;

        Transforms transforms;
 
        CENSEngine *engine;


        CENSSerializedRobot(const std::string robotName, CENSEngine *en):
            CENSParameterManager(robotName),engine(en)
        {

        }
        
        ~CENSSerializedRobot()
        {
        }
 
        void addHinge( std::string hingeName, CENSHingeConstraint *_hinge );

        void addGenericConstraint( std::string constraintName, 
                btTypedConstraint *_constraint );

        void addBody( std::string bodyName, btRigidBody *_body );
        
        void addTransform( std::string transformName, btTransform _transform );
        
        bool init();

        // disable all bodies and hinges in the robot 
        void disableBodies();

        // enable all bodies and hinges in the robot 
        void enableBodies();
 
        void update();

        void move(const std::string hingeName, double target_angle, 
                double impulse = 0);
    };

    
    typedef std::map<std::string, btRigidBody *> Bodies;
    typedef std::map<std::string, btTypedConstraint *> Constraints;
    typedef std::map<const std::string, CENSSerializedRobot *> Robots;
    typedef std::map<const std::string, CENSTouchSensor *> Sensors;
    typedef std::map<const std::string, int > Cameras;

    class CENSSerializedEngine : public CENSEngine
    {
        public:


            virtual void step(int timestep);

            virtual CENSSerializedRobot *loadBulletFile(
                    const std::string &robotName, 
                    const std::string &file);

            virtual CENSTouchSensor *addTouchSensor(btRigidBody *body, 
                    std::string name)
            {
                CENSTouchSensor *sensor = CENSPhysics::addTouchSensor(body);
                m_eSensors[name] = sensor;

                return sensor;
            }
           
            virtual int addCamera(
                    std::string cameraName,         
                    btRigidBody *body,              
                    btTransform local_transform,    
                    std::string screenTitle,        
                    int screenWidth,                
                    int screenHeight,               
                    int screenXGap,                 
                    int screenYGap,                 
                    btVector3 up,                   
                    float target,                   
                    float foV=70.0,                 
                    float near=1.0,                 
                    float far=10000 )               
            {
                
                int index = attachCamera( body, local_transform, screenTitle,     
                        screenWidth, screenHeight, screenXGap,    
                        screenYGap, up, target, foV, near,far );

                m_eCameras[cameraName] = index;

                return index;
            }

        protected:

            Robots m_eRobots;

            Sensors m_eSensors;
            
            Cameras m_eCameras;

            Bodies m_eBodies;

            Constraints m_eConstraints;
    };

}

#endif // CENS_SERIALIZED_ENGINE_H