Link.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_LINK_H_INCLUDED
#define HRPMODEL_LINK_H_INCLUDED

#include <string>
#include <ostream>
#include <vector>
#include <boost/intrusive_ptr.hpp>
#include <hrpUtil/Eigen3d.h>
#include <hrpCollision/ColdetModel.h>
#include "Config.h"

namespace hrp {
    class Link;
}

HRPMODEL_API std::ostream& operator<<(std::ostream &out, hrp::Link& link);

namespace hrp {

    class Body;
    class Sensor;
    class Light;

    class ColdetModel;
    typedef boost::intrusive_ptr<ColdetModel> ColdetModelPtr;

    class HRPMODEL_API Link {

      public:

        Link();
        Link(const Link& link);
        virtual ~Link();

        bool isValid() { return (index >= 0); }
        void addChild(Link* link);
        bool detachChild(Link* link);
        bool isRoot() { return !parent; }

        void setAttitude(const Matrix33& R) { this->R = R * Rs.transpose(); }
        Matrix33 attitude() { return Matrix33(this->R * Rs); }
        Matrix33 calcRfromAttitude(const Matrix33& R) { return Matrix33(R * Rs.transpose()); }

        void calcSubMassCM();

        void calcSubMassInertia(Matrix33& subIw);

        void setSegmentAttitude(const Matrix33& R) { this->R = R * Rs.transpose(); }

        Matrix33 segmentAttitude() { return Matrix33(this->R * Rs); }

        void updateColdetModelPosition() {
            coldetModel->setPosition(R, p);
        }

        Body* body;

        int index; 
        int jointId;  
                
        std::string name;

        enum JointType {
            FIXED_JOINT,   
            FREE_JOINT,   
            ROTATIONAL_JOINT,   
            SLIDE_JOINT 
        };
                
        JointType jointType;

        Link* parent;
        Link* sibling;
        Link* child;

        Vector3 p;      

        Matrix33 R;

        Vector3 v;      
        Vector3 w;      
        Vector3 dv;     
        Vector3 dw;     

        double q;       
        double dq;      
        double ddq;     
        double u;       

        Vector3 a;      
        Vector3 d;      
        Vector3 b;      

        Matrix33 Rs;    

        double m;       
        Matrix33 I;     
        Vector3 c;      
        Vector3 wc;     
        
        Vector3 vo;     
        Vector3 dvo;    

        Vector3 sw;
                
        Vector3 sv;
                
        Vector3 cv;     
        Vector3 cw;     

        Vector3 fext;   
        Vector3 tauext; 

        // needed ?
        //Vector3                       f;      ///< force from the parent link 
        //Vector3                       tau;    ///< torque from the parent link
                
        Matrix33 Iww;   
        Matrix33 Iwv;   
        Matrix33 Ivv;   
        Vector3 pf;     
        Vector3 ptau;   
        Vector3 hhv;    
        Vector3 hhw;    
        double uu;
        double dd;      
                
        double Jm2;    

        double ulimit;  
        double llimit;  
        double uvlimit; 
        double lvlimit; 
        double climit;  

        double defaultJointValue;
        double torqueConst;
        double encoderPulse;
        double Ir;      
        double gearRatio;
        double gearEfficiency;
        double rotorResistor;

        bool isHighGainMode;
        bool isCrawler;

        ColdetModelPtr coldetModel;

        struct ConstraintForce {
            Vector3 point;
            Vector3 force;
        };

        typedef std::vector<ConstraintForce> ConstraintForceArray;
        ConstraintForceArray constraintForces;

        double  subm;                   
        Vector3 submwc;                 
        std::vector<Sensor *> sensors;  
        std::vector<Light *>  lights;   
      private:

        Link& operator=(const Link& link); // no implementation is given to disable the copy operator
        void setBodyIter(Body* body);
        friend std::ostream& ::operator<<(std::ostream &out, hrp::Link& link);
        void putInformation(std::ostream& out); // for the iostream output
    };

};
        

#endif