← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/Body/LinkTraverse.cpp

  • 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/LinkTraverse.cpp
 
1
/** 
 
2
    \file
 
3
    \brief Implementations of the LinkTraverse class
 
4
    \author Shin'ichiro Nakaoka
 
5
*/
 
6
  
 
7
#include "LinkTraverse.h"
 
8
#include "Link.h"
 
9
 
 
10
using namespace std;
 
11
using namespace cnoid;
 
12
 
 
13
 
 
14
LinkTraverse::LinkTraverse()
 
15
{
 
16
 
 
17
}
 
18
 
 
19
 
 
20
LinkTraverse::LinkTraverse(int size)
 
21
    : links(size)
 
22
{
 
23
    links.clear();
 
24
}
 
25
 
 
26
 
 
27
LinkTraverse::LinkTraverse(Link* root, bool doUpward, bool doDownward)
 
28
{
 
29
    find(root, doUpward, doDownward);
 
30
}
 
31
 
 
32
 
 
33
LinkTraverse::~LinkTraverse()
 
34
{
 
35
 
 
36
}
 
37
 
 
38
 
 
39
void LinkTraverse::find(Link* root, bool doUpward, bool doDownward)
 
40
{
 
41
    numUpwardConnections = 0;
 
42
    links.clear();
 
43
    traverse(root, doUpward, doDownward, false, 0);
 
44
}
 
45
 
 
46
 
 
47
void LinkTraverse::traverse(Link* link, bool doUpward, bool doDownward, bool isUpward, Link* prev)
 
48
{
 
49
    links.push_back(link);
 
50
    if(isUpward){
 
51
        ++numUpwardConnections;
 
52
    }
 
53
    
 
54
    if(doUpward && link->parent){
 
55
        traverse(link->parent, doUpward, true, true, link);
 
56
    }
 
57
    if(doDownward){
 
58
        for(Link* child = link->child; child; child = child->sibling){
 
59
            if(child != prev){
 
60
                traverse(child, false, true, false, 0);
 
61
            }
 
62
        }
 
63
    }
 
64
}
 
65
 
 
66
 
 
67
void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const
 
68
{
 
69
    Vector3 arm;
 
70
    int i;
 
71
    for(i=1; i <= numUpwardConnections; ++i){
 
72
 
 
73
        Link* link = links[i];
 
74
        Link* child = links[i-1];
 
75
 
 
76
        switch(child->jointType){
 
77
 
 
78
        case Link::ROTATIONAL_JOINT:
 
79
            link->R.noalias() = child->R * AngleAxisd(child->q, child->a).inverse();
 
80
            arm.noalias() = link->R * child->b;
 
81
            link->p = child->p - arm;
 
82
 
 
83
            if(calcVelocity){
 
84
                child->sw.noalias() = link->R * child->a;
 
85
                link->w = child->w - child->dq * child->sw;
 
86
                link->v = child->v - link->w.cross(arm);
 
87
 
 
88
                if(calcAcceleration){
 
89
                    link->dw = child->dw - child->dq * child->w.cross(child->sw) - (child->ddq * child->sw);
 
90
                    link->dv = child->dv - child->w.cross(child->w.cross(arm)) - child->dw.cross(arm);
 
91
                }
 
92
            }
 
93
            break;
 
94
            
 
95
        case Link::SLIDE_JOINT:
 
96
            link->R = child->R;
 
97
            arm.noalias() = link->R * (child->b + child->q * child->d);
 
98
            link->p = child->p - arm;
 
99
 
 
100
            if(calcVelocity){
 
101
                child->sv.noalias() = link->R * child->d;
 
102
                link->w = child->w;
 
103
                link->v = child->v - child->dq * child->sv;
 
104
 
 
105
                if(calcAcceleration){
 
106
                    link->dw = child->dw;
 
107
                    link->dv = child->dv - child->w.cross(child->w.cross(arm)) - child->dw.cross(arm)
 
108
                        - 2.0 * child->dq * child->w.cross(child->sv) - child->ddq * child->sv;
 
109
                }
 
110
            }
 
111
            break;
 
112
            
 
113
        case Link::FIXED_JOINT:
 
114
        default:
 
115
            link->R = child->R;
 
116
            link->p.noalias() = child->p - (link->R * child->b);
 
117
 
 
118
            if(calcVelocity){
 
119
                link->w = child->w;
 
120
                link->v = child->v;
 
121
                                
 
122
                if(calcAcceleration){
 
123
                    link->dw = child->dw;
 
124
                    link->dv = child->dv;
 
125
                }
 
126
            }
 
127
            break;
 
128
        }
 
129
    }
 
130
 
 
131
    int n = links.size();
 
132
    for( ; i < n; ++i){
 
133
        
 
134
        Link* link = links[i];
 
135
        Link* parent = link->parent;
 
136
 
 
137
        switch(link->jointType){
 
138
            
 
139
        case Link::ROTATIONAL_JOINT:
 
140
            link->R.noalias() = parent->R * AngleAxisd(link->q, link->a);
 
141
            arm.noalias() = parent->R * link->b;
 
142
            link->p = parent->p + arm;
 
143
 
 
144
            if(calcVelocity){
 
145
                link->sw.noalias() = parent->R * link->a;
 
146
                link->w = parent->w + link->sw * link->dq;
 
147
                link->v = parent->v + parent->w.cross(arm);
 
148
 
 
149
                if(calcAcceleration){
 
150
                    link->dw = parent->dw + link->dq * parent->w.cross(link->sw) + (link->ddq * link->sw);
 
151
                    link->dv = parent->dv + parent->w.cross(parent->w.cross(arm)) + parent->dw.cross(arm);
 
152
                }
 
153
            }
 
154
            break;
 
155
            
 
156
        case Link::SLIDE_JOINT:
 
157
            link->R = parent->R;
 
158
            arm.noalias() = parent->R * (link->b + link->q * link->d);
 
159
            link->p = parent->p + arm;
 
160
 
 
161
            if(calcVelocity){
 
162
                link->sv.noalias() = parent->R * link->d;
 
163
                link->w = parent->w;
 
164
                link->v = parent->v + link->sv * link->dq;
 
165
 
 
166
                if(calcAcceleration){
 
167
                    link->dw = parent->dw;
 
168
                    link->dv = parent->dv + parent->w.cross(parent->w.cross(arm)) + parent->dw.cross(arm)
 
169
                        + 2.0 * link->dq * parent->w.cross(link->sv) + link->ddq * link->sv;
 
170
                }
 
171
            }
 
172
            break;
 
173
 
 
174
        case Link::FIXED_JOINT:
 
175
        default:
 
176
            link->R = parent->R;
 
177
            link->p.noalias() = parent->R * link->b + parent->p;
 
178
 
 
179
            if(calcVelocity){
 
180
                link->w = parent->w;
 
181
                link->v = parent->v;
 
182
 
 
183
                if(calcAcceleration){
 
184
                    link->dw = parent->dw;
 
185
                    link->dv = parent->dv;
 
186
                }
 
187
            }
 
188
            break;
 
189
        }
 
190
    }
 
191
}
 
192
 
 
193
 
 
194
std::ostream& operator<<(std::ostream& os, LinkTraverse& traverse)
 
195
{
 
196
    int n = traverse.numLinks();
 
197
    for(int i=0; i < n; ++i){
 
198
        Link* link = traverse[i];
 
199
        os << link->name();
 
200
        if(i != n){
 
201
            os << (traverse.isDownward(i) ? " => " : " <= ");
 
202
        }
 
203
    }
 
204
    os << std::endl;
 
205
 
 
206
    return os;
 
207
}