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