← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/Body/Link.h

  • Committer: Thomas Moulard
  • Date: 2012-10-23 12:43:24 UTC
  • Revision ID: git-v1:351cf736ad49bc7a9a7b9767dee760a013517a5d
Tags: upstream/1.1.0
Imported Upstream version 1.1.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Body/Link.h
 
1
/**
 
2
   \file
 
3
   \author Shin'ichiro Nakaoka
 
4
*/
 
5
 
 
6
#ifndef CNOID_BODY_LINK_H_INCLUDED
 
7
#define CNOID_BODY_LINK_H_INCLUDED
 
8
 
 
9
#include <string>
 
10
#include <ostream>
 
11
#include <vector>
 
12
#include <cnoid/EigenTypes>
 
13
#include <cnoid/ColdetModel>
 
14
#include "exportdecl.h"
 
15
 
 
16
namespace cnoid {
 
17
    class Link;
 
18
}
 
19
 
 
20
CNOID_EXPORT std::ostream& operator<<(std::ostream &out, cnoid::Link& link);
 
21
 
 
22
namespace cnoid {
 
23
 
 
24
    class Body;
 
25
 
 
26
    class CNOID_EXPORT Link {
 
27
 
 
28
      public:
 
29
 
 
30
        Link();
 
31
        Link(const Link& link);
 
32
        ~Link();
 
33
 
 
34
        inline const std::string& name() {
 
35
            return name_;
 
36
        }
 
37
        inline void setName(const std::string& name){
 
38
            name_ = name;
 
39
        }
 
40
 
 
41
        inline bool isValid() { return (index >= 0); }
 
42
        void addChild(Link* link);
 
43
        bool detachChild(Link* link);
 
44
        inline bool isRoot() { return !parent; }
 
45
 
 
46
        inline void setAttitude(const Matrix3& R) { this->R = R * Rs.transpose(); }
 
47
        inline Matrix3 attitude() { return this->R * Rs; }
 
48
        inline Matrix3 calcRfromAttitude(const Matrix3& R) { return R * Rs.transpose(); }
 
49
 
 
50
        /**
 
51
           @brief compute sum of m x wc of subtree
 
52
           @note assuming wc is already computed by Body::calcCM()
 
53
        */
 
54
        void calcSubMassCM();
 
55
 
 
56
        /**
 
57
           @deprecated use setAttitude().
 
58
        */
 
59
        inline void setSegmentAttitude(const Matrix3& R) { this->R = R * Rs.transpose(); }
 
60
 
 
61
        /**
 
62
           @deprecated use attitude().
 
63
        */
 
64
        inline Matrix3 segmentAttitude() { return this->R * Rs; }
 
65
 
 
66
        void updateColdetModelPosition() {
 
67
            coldetModel->setPosition(R, p);
 
68
        }
 
69
 
 
70
        Body* body;
 
71
 
 
72
        int index; 
 
73
        int jointId;  ///< jointId value written in a model file
 
74
                
 
75
        enum JointType {
 
76
            FIXED_JOINT,   ///< fixed joint(0 dof)
 
77
            FREE_JOINT,   /// 6-DOF root link
 
78
            ROTATIONAL_JOINT,   ///< rotational joint (1 dof)
 
79
            SLIDE_JOINT ///< translational joint (1 dof)
 
80
        };
 
81
                
 
82
        JointType jointType;
 
83
 
 
84
        Link* parent;
 
85
        Link* sibling;
 
86
        Link* child;
 
87
 
 
88
        Vector3 p;      ///< position
 
89
 
 
90
        /**
 
91
           Internal world attitude.
 
92
           In the model computation, it corresponds to the identity matrix
 
93
           when all the joint angles of a robot are zero so that multiplication of
 
94
           local attitdue matrices can be omitted to simplify the computation.
 
95
           If you want to use the original coordinate in the model file,
 
96
           use setAttitude() and attitude() to access.
 
97
        */
 
98
        Matrix3 R;
 
99
 
 
100
        Vector3 v;      ///< linear velocity 
 
101
        Vector3 w;      ///< angular velocity, omega
 
102
        Vector3 dv;     ///< linear acceleration
 
103
        Vector3 dw;     ///< derivative of omega
 
104
 
 
105
        double q;       ///< joint value
 
106
        double dq;      ///< joint velocity
 
107
        double ddq;     ///< joint acceleration
 
108
        double u;       ///< joint torque
 
109
 
 
110
        Vector3 a;      ///< rotational joint axis (self local)
 
111
        Vector3 d;      ///< translation joint axis (self local)
 
112
        Vector3 b;      ///< relative position (parent local)
 
113
 
 
114
        Matrix3 Rs;    ///< relative attitude of the link segment (self local)
 
115
 
 
116
        double m;       ///< mass
 
117
        Matrix3 I;     ///< inertia tensor (self local, around c)
 
118
        Vector3 c;      ///< center of mass (self local)
 
119
        Vector3 wc;     ///< R * c + p
 
120
        
 
121
        Vector3 vo;     ///< translation elements of spacial velocity
 
122
        Vector3 dvo;    ///< derivative of vo
 
123
 
 
124
        /** A unit vector of angular velocity (the world coordinate) generated by the joint 
 
125
            The value is parent->R * a when the joint is the rotational type. */
 
126
        Vector3 sw;
 
127
                
 
128
        /** A unit vector of spatial velocity (the world coordinate) generated by the joint.
 
129
            The value is parent->R * d when the joint is the translation type. */
 
130
        Vector3 sv;
 
131
                
 
132
        Vector3 cv;     ///< dsv * dq (cross velocity term)
 
133
        Vector3 cw;     ///< dsw * dq (cross velocity term)
 
134
 
 
135
        Vector3 fext;   ///< external force 
 
136
        Vector3 tauext; ///< external torque (around the world origin)
 
137
 
 
138
        // needed ?
 
139
        //Vector3                       f;      ///< force from the parent link 
 
140
        //Vector3                       tau;    ///< torque from the parent link
 
141
                
 
142
        Matrix3 Iww;   ///< bottm right block of the articulated inertia
 
143
        Matrix3 Iwv;   ///< bottom left block (transpose of top right block) of the articulated inertia
 
144
        Matrix3 Ivv;   ///< top left block of the articulated inertia
 
145
        Vector3 pf;     ///< bias force (linear element)
 
146
        Vector3 ptau;   ///< bias force (torque element)
 
147
        Vector3 hhv;    ///< top block of Ia*s
 
148
        Vector3 hhw;    ///< bottom bock of Ia*s 
 
149
        double uu;
 
150
        double dd;      ///< Ia*s*s^T  
 
151
                
 
152
        double Jm2;    ///< Equivalent rotor inertia: n^2*Jm [kg.m^2]
 
153
 
 
154
        double ulimit;  ///< the upper limit of joint values
 
155
        double llimit;  ///< the lower limit of joint values
 
156
        double uvlimit; ///< the upper limit of joint velocities
 
157
        double lvlimit; ///< the lower limit of joint velocities
 
158
 
 
159
        double defaultJointValue;
 
160
        double torqueConst;
 
161
        double encoderPulse;
 
162
        double Ir;      ///< rotor inertia [kg.m^2]
 
163
        double gearRatio;
 
164
        double gearEfficiency;
 
165
        double rotorResistor;
 
166
 
 
167
        bool isHighGainMode;
 
168
 
 
169
        ColdetModelPtr coldetModel;
 
170
 
 
171
        struct ConstraintForce {
 
172
            Vector3 point;
 
173
            Vector3 force;
 
174
        };
 
175
 
 
176
        typedef std::vector<ConstraintForce> ConstraintForceArray;
 
177
        ConstraintForceArray constraintForces;
 
178
 
 
179
        double  subm;                   ///< mass of subtree
 
180
        Vector3 submwc;                 ///< sum of m x wc of subtree
 
181
 
 
182
      private:
 
183
 
 
184
        std::string name_;
 
185
 
 
186
        Link& operator=(const Link& link); // no implementation is given to disable the copy operator
 
187
        void setBodyIter(Body* body);
 
188
        friend std::ostream& ::operator<<(std::ostream &out, Link& link);
 
189
        void putInformation(std::ostream& out); // for the iostream output
 
190
    };
 
191
 
 
192
};
 
193
        
 
194
 
 
195
#endif