Body.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 HRPMODEL_BODY_H_INCLUDED
#define HRPMODEL_BODY_H_INCLUDED

#include <map>
#include <vector>
#include <ostream>
#include <boost/shared_ptr.hpp>
#include <hrpUtil/Referenced.h>
#include <hrpUtil/Eigen3d.h>
#include "LinkTraverse.h"
#include "Config.h"

namespace hrp {
    class Sensor;
    class Light;
    class Body;
    class JointPath;
    typedef boost::shared_ptr<JointPath> JointPathPtr;
}

namespace hrp {

    struct BodyHandleEntity {
        Body* body;
    };

    struct BodyInterface;
    struct BodyCustomizerInterface;
    typedef void* BodyHandle;
    typedef void* BodyCustomizerHandle;

    class HRPMODEL_API Body : public Referenced
    {

      public:

        static BodyInterface* bodyInterface();

        Body();
        Body(const Body& org);

        virtual ~Body();

        const std::string& name() {
            return name_;
        }

        void setName(const std::string& name) {
            name_ = name;
        }

        const std::string& modelName() {
            return modelName_;
        }
        
        void setModelName(const std::string& name) {
            modelName_ = name;
        }

        void setRootLink(Link* link);

        void updateLinkTree();

        inline unsigned int numJoints() const {
            return jointIdToLinkArray.size();
        }

        inline Link* joint(int id) const {
            return jointIdToLinkArray[id];
        }

        inline const std::vector<Link*>& joints() const {
            return jointIdToLinkArray;
        }

        inline unsigned int numLinks() const {
            return linkTraverse_.numLinks();
        }

        inline Link* link(int index) const {
            return linkTraverse_.link(index);
        }

        inline const LinkTraverse& links() const {
            return linkTraverse_;
        }

        inline const LinkTraverse& linkTraverse() const {
            return linkTraverse_;
        }

        Link* link(const std::string& name) const;

        inline Link* rootLink() const {
            return rootLink_;
        }

        Light *createLight(Link *link, int lightType, const std::string& name);
        inline Light* light(const std::string& name) { 
            return nameToLightMap[name];
        }

        // sensor access methods
        Sensor* createSensor(Link* link, int sensorType, int id, const std::string& name);

        void addSensor(Sensor* sensor, int sensorType, int id );

        inline Sensor* sensor(int sensorType, int sensorId) const {
            return allSensors[sensorType][sensorId];
        }

        inline unsigned int numSensors(int sensorType) const {
            return allSensors[sensorType].size();
        }

        inline unsigned int numSensorTypes() const {
            return allSensors.size();
        }

        void clearSensorValues();

        template <class TSensor> inline TSensor* sensor(int id) const {
            return static_cast<TSensor*>(allSensors[TSensor::TYPE][id]);
        }

        template <class TSensor> inline TSensor* sensor(const std::string& name) const {
            TSensor* sensor = 0;
            NameToSensorMap::const_iterator p = nameToSensorMap.find(name);
            if(p != nameToSensorMap.end()){
                sensor = dynamic_cast<TSensor*>(p->second);
            }
            return sensor;
        }

        inline bool isStaticModel() {
            return isStaticModel_;
        }

        double calcTotalMass();

        inline double totalMass() const {
            return totalMass_;
        }

        Vector3 calcCM();

        /*
          The motion equation for calcMassMatrix()
          |       |   | dv   |   |    |   | fext      |
          | out_M | * | dw   | + | b1 | = | tauext    |
          |       |   |ddq   |   |    |   | u         |
        */
        void calcMassMatrix(dmatrix& out_M);

        void setColumnOfMassMatrix(dmatrix& M, int column);

        void calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau);

        void calcTotalMomentum(Vector3& out_P, Vector3& out_L);

        void calcTotalMomentumFromJacobian(Vector3& out_P, Vector3& out_L);

        void setDefaultRootPosition(const Vector3& p, const Matrix33& R);

        void getDefaultRootPosition(Vector3& out_p, Matrix33& out_R);

        void initializeConfiguration();

        void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false);

        void clearExternalForces();

        JointPathPtr getJointPath(Link* baseLink, Link* targetLink);

        inline void setVirtualJointForces(){
            if(customizerInterface){
                setVirtualJointForcesSub();
            }
        }

        void updateLinkColdetModelPositions();

        void putInformation(std::ostream &out);

        bool installCustomizer();
        bool installCustomizer(BodyCustomizerInterface* customizerInterface);

                enum ExtraJointType { EJ_XYZ, EJ_XY, EJ_Z };

        struct ExtraJoint {
                        std::string name;
            ExtraJointType type;
            Vector3 axis;
            Link* link[2];
            Vector3 point[2];
        };

        std::vector<ExtraJoint> extraJoints;

        void calcCMJacobian(Link *base, dmatrix &J);
        void calcAngularMomentumJacobian(Link *base, dmatrix &H);
      private:

        bool isStaticModel_;
        Link* rootLink_;

        std::string name_;
        std::string modelName_;

        typedef std::vector<Link*> LinkArray;
        LinkArray jointIdToLinkArray;

        LinkTraverse linkTraverse_;

        typedef std::map<std::string, Link*> NameToLinkMap;
        NameToLinkMap nameToLinkMap;

        // sensor = sensors[type][sensorId]
        typedef std::vector<Sensor*> SensorArray;
        std::vector<SensorArray> allSensors;

        typedef std::map<std::string, Sensor*> NameToSensorMap;
        NameToSensorMap nameToSensorMap;
        typedef std::map<std::string, Light*> NameToLightMap;
        NameToLightMap nameToLightMap;

        double totalMass_;

        Vector3 defaultRootPosition;
        Matrix33 defaultRootAttitude;

        // Members for customizer
        BodyCustomizerHandle customizerHandle;
        BodyCustomizerInterface* customizerInterface;
        BodyHandleEntity bodyHandleEntity;
        BodyHandle bodyHandle;

        void initialize();
        Link* createEmptyJoint(int jointId);
        void setVirtualJointForcesSub();

        friend class CustomizedJointPath;
    };

    typedef boost::shared_ptr<Body> BodyPtr;

};


HRPMODEL_API std::ostream &operator<< (std::ostream& out, hrp::Body& body);


#endif