← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/BodyPlugin/KinematicsSimulatorItem.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/BodyPlugin/KinematicsSimulatorItem.cpp
 
1
/*!
 
2
  @file
 
3
  @author Shin'ichiro Nakaoka
 
4
*/
 
5
 
 
6
#include "KinematicsSimulatorItem.h"
 
7
#include "WorldItem.h"
 
8
#include "BodyItem.h"
 
9
#include "BodyMotionItem.h"
 
10
#include <cnoid/ItemTreeView>
 
11
#include <cnoid/ItemManager>
 
12
#include <cnoid/Body>
 
13
#include <iostream>
 
14
#include "gettext.h"
 
15
 
 
16
using namespace std;
 
17
using namespace cnoid;
 
18
 
 
19
namespace {
 
20
 
 
21
    const bool TRACE_FUNCTIONS = false;
 
22
 
 
23
    class BodyUnit
 
24
    {
 
25
    public:
 
26
        int frame;
 
27
        double frameRate;
 
28
        BodyPtr body;
 
29
        MultiValueSeqPtr qseqRef;
 
30
        Vector3SeqPtr zmpSeq;
 
31
        MultiValueSeqPtr qseqResultBuf;
 
32
        MultiValueSeqPtr qseqResult;
 
33
        MultiAffine3SeqItemPtr rootResultItem;
 
34
        MultiAffine3SeqPtr rootResultBuf;
 
35
        MultiAffine3SeqPtr rootResult;
 
36
        Link* baseLink;
 
37
        LinkTraverse fkTraverse;
 
38
        vector<Link*> footLinks;
 
39
        vector<double> footLinkOriginHeights;
 
40
 
 
41
        bool initialize(BodyItemPtr bodyItem);
 
42
        bool updatePosition(int newFrame);
 
43
        void putResultToBuf();
 
44
        void setBaseLink(Link* link, double originHeight);
 
45
        void setBaseLinkByZmp(int frame);
 
46
        bool flushBuf();
 
47
    };
 
48
}
 
49
 
 
50
 
 
51
namespace cnoid {
 
52
        
 
53
    class KSIImpl
 
54
    {
 
55
    public:
 
56
        KSIImpl(KinematicsSimulatorItem* self);
 
57
        KSIImpl(KinematicsSimulatorItem* self, const KSIImpl& org);
 
58
            
 
59
        bool doStartSimulation();
 
60
        bool setupBodies();
 
61
        void addBody(BodyItemPtr bodyItem);
 
62
        bool doStepSimulation();
 
63
        double doFlushResults();        
 
64
 
 
65
        KinematicsSimulatorItem* self;
 
66
            
 
67
        vector<BodyUnit> bodyUnits;
 
68
        vector<BodyUnit*> bodyUnitsToPutResult;
 
69
        vector<BodyUnit*> bodyUnitsToNotifyUpdate;
 
70
 
 
71
        int currentFrame;
 
72
        int frameAtLastBufferWriting;
 
73
        double mainFrameRate;
 
74
    };
 
75
}
 
76
 
 
77
 
 
78
void cnoid::initializeKinematicsSimulatorItem(ExtensionManager& ext)
 
79
{
 
80
    ext.itemManager().registerClass<KinematicsSimulatorItem>(N_("KinematicsSimulatorItem"));
 
81
    ext.itemManager().addCreationPanel<KinematicsSimulatorItem>();
 
82
}
 
83
 
 
84
 
 
85
bool BodyUnit::initialize(BodyItemPtr bodyItem)
 
86
{
 
87
    if(TRACE_FUNCTIONS){
 
88
        cout << "BodyUnit::initialize()" << endl;
 
89
    }
 
90
 
 
91
    if(bodyItem->body()->isStaticModel()){
 
92
        return false;
 
93
    }
 
94
        
 
95
    frame = 0;
 
96
    body = bodyItem->body()->duplicate();
 
97
    baseLink = 0;
 
98
 
 
99
    ItemList<BodyMotionItem> motionItems = bodyItem->getSubItems<BodyMotionItem>();
 
100
    if(!motionItems.empty()){
 
101
        BodyMotionItemPtr motionItem;
 
102
        // prefer the first checked item
 
103
        for(size_t i=0; i < motionItems.size(); ++i){
 
104
            if(ItemTreeView::mainInstance()->isItemChecked(motionItems[i])){
 
105
                motionItem = motionItems[i];
 
106
                break;
 
107
            }
 
108
        }
 
109
        if(!motionItem){
 
110
            motionItem = motionItems[0];
 
111
        }
 
112
        
 
113
        frameRate = motionItem->motion()->frameRate();
 
114
        qseqRef = motionItem->jointPosSeq();
 
115
        if(qseqResult){
 
116
            qseqResultBuf.reset(new MultiValueSeq(qseqResult->numParts(), 0, frameRate));
 
117
        }
 
118
 
 
119
        if(motionItem->hasRelativeZmpSeqItem()){
 
120
            zmpSeq = motionItem->relativeZmpSeq();
 
121
        }
 
122
 
 
123
        rootResultItem = motionItem->linkPosSeqItem();
 
124
        rootResultBuf.reset(new MultiAffine3Seq(1, 0, frameRate));
 
125
        rootResult = motionItem->linkPosSeq();
 
126
        rootResult->setFrameRate(frameRate);
 
127
        rootResult->setDimension(1, 1);
 
128
    }
 
129
 
 
130
    const YamlMapping& info = *bodyItem->body()->info();
 
131
    const YamlSequence& footLinkInfos = *info.findSequence("footLinks");
 
132
    if(footLinkInfos.isValid()){
 
133
        for(int i=0; i < footLinkInfos.size(); ++i){
 
134
            const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping();
 
135
            Link* link = body->link(footLinkInfo["link"].toString());
 
136
            Vector3 soleCenter;
 
137
            if(link && read(footLinkInfo, "soleCenter", soleCenter)){
 
138
                footLinks.push_back(link);
 
139
                footLinkOriginHeights.push_back(-soleCenter[2]);
 
140
            }
 
141
        }
 
142
    }
 
143
 
 
144
    if(!footLinks.empty()){
 
145
        if(zmpSeq){
 
146
            setBaseLinkByZmp(0);
 
147
        } else {
 
148
            if(bodyItem->currentBaseLink()){
 
149
                int currentBaseLinkIndex = bodyItem->currentBaseLink()->index;
 
150
                for(size_t i=0; i < footLinks.size(); ++i){
 
151
                    Link* footLink = footLinks[i];
 
152
                    if(footLink->index == currentBaseLinkIndex){
 
153
                        setBaseLink(footLink, footLinkOriginHeights[i]);
 
154
                    }
 
155
                }
 
156
            } else{
 
157
                setBaseLink(footLinks[0], footLinkOriginHeights[0]);
 
158
            }
 
159
        }
 
160
    }
 
161
 
 
162
    if(!baseLink){
 
163
        if(bodyItem->currentBaseLink()){
 
164
            baseLink = body->link(bodyItem->currentBaseLink()->index);
 
165
        } else {
 
166
            baseLink = body->rootLink();
 
167
        }
 
168
        fkTraverse.find(baseLink, true, true);
 
169
    }
 
170
 
 
171
    if(rootResult){
 
172
        Link* rootLink = body->rootLink();
 
173
        Affine3& initialPos = rootResult->at(0, 0);
 
174
        initialPos.translation() = rootLink->p;
 
175
        initialPos.linear() = rootLink->R;
 
176
    }
 
177
 
 
178
    return (qseqRef != 0);
 
179
}
 
180
 
 
181
 
 
182
void BodyUnit::setBaseLink(Link* link, double originHeight)
 
183
{
 
184
    if(TRACE_FUNCTIONS){
 
185
        cout << "BodyUnit::setBaseLink()" << endl;
 
186
    }
 
187
 
 
188
    baseLink = link;
 
189
 
 
190
    link->p[2] = originHeight;
 
191
 
 
192
    Matrix3& R = link->R;
 
193
    Matrix3::ColXpr x = R.col(0);
 
194
    Matrix3::ColXpr y = R.col(1);
 
195
    Matrix3::ColXpr z = R.col(2);
 
196
    z.normalize();
 
197
    y = z.cross(x).normalized();
 
198
    x = y.cross(z);
 
199
 
 
200
    fkTraverse.find(link, true, true);
 
201
    fkTraverse.calcForwardKinematics();
 
202
}
 
203
 
 
204
 
 
205
void BodyUnit::setBaseLinkByZmp(int frame)
 
206
{
 
207
    Link* root = body->rootLink();
 
208
    const Vector3 zmp = root->R * zmpSeq->at(frame) + root->p;
 
209
    int footLinkOnZmpId = -1;
 
210
    double l2min = std::numeric_limits<double>::max();
 
211
    for(size_t i=0; i < footLinks.size(); ++i){
 
212
        double l2 = (footLinks[i]->p - zmp).squaredNorm();
 
213
        if(l2 < l2min){
 
214
            footLinkOnZmpId = i;
 
215
            l2min = l2;
 
216
        }
 
217
    }
 
218
    if(footLinkOnZmpId >= 0 && footLinks[footLinkOnZmpId] != baseLink){
 
219
        setBaseLink(footLinks[footLinkOnZmpId], footLinkOriginHeights[footLinkOnZmpId]);
 
220
    }
 
221
}
 
222
 
 
223
 
 
224
bool BodyUnit::updatePosition(int newFrame)
 
225
{
 
226
    if(TRACE_FUNCTIONS){
 
227
        cout << "BodyUnit::updatePosition()" << endl;
 
228
    }
 
229
 
 
230
    if(newFrame >= qseqRef->numFrames()){
 
231
        return false;
 
232
    }
 
233
 
 
234
    frame = newFrame;
 
235
    
 
236
    const int n = std::min(body->numJoints(), qseqRef->numParts());
 
237
 
 
238
    MultiValueSeq::View qRef = qseqRef->frame(frame);
 
239
    for(int i=0; i < n; ++i){
 
240
        body->joint(i)->q = qRef[i];
 
241
    }
 
242
 
 
243
    fkTraverse.calcForwardKinematics();
 
244
 
 
245
    if(zmpSeq){
 
246
        setBaseLinkByZmp(frame);
 
247
    } else {
 
248
        for(size_t i=0; i < footLinks.size(); ++i){
 
249
            Link* link = footLinks[i];
 
250
            if(link != baseLink){
 
251
                if(link->p[2] < footLinkOriginHeights[i]){
 
252
                    setBaseLink(link, footLinkOriginHeights[i]);
 
253
                }
 
254
            }
 
255
        }
 
256
    }
 
257
 
 
258
    return true;
 
259
}
 
260
 
 
261
void BodyUnit::putResultToBuf()
 
262
{
 
263
    if(TRACE_FUNCTIONS){
 
264
        cout << "BodyUnit::putResultToBuf()" << endl;
 
265
    }
 
266
 
 
267
    const int bufFrame = rootResultBuf->numFrames();
 
268
 
 
269
    if(qseqResultBuf){
 
270
        qseqResultBuf->setNumFrames(bufFrame + 1);
 
271
        MultiValueSeq::View qResultBuf = qseqResultBuf->frame(bufFrame);
 
272
        const int n = qResultBuf.size();
 
273
        for(int i=0; i < n; ++i){
 
274
            qResultBuf[i] = body->joint(i)->q;
 
275
        }
 
276
    }
 
277
 
 
278
    rootResultBuf->setNumFrames(bufFrame + 1);
 
279
    Link* rootLink = body->rootLink();
 
280
    Affine3& pos = rootResultBuf->at(bufFrame, 0);
 
281
    pos.translation() = rootLink->p;
 
282
    pos.linear() = rootLink->R;
 
283
}
 
284
 
 
285
 
 
286
bool BodyUnit::flushBuf()
 
287
{
 
288
    if(TRACE_FUNCTIONS){
 
289
        cout << "BodyUnit::flushBuf()" << endl;
 
290
    }
 
291
 
 
292
    const int frame = rootResult->numFrames();
 
293
    const int numBufFrames = rootResultBuf->numFrames();
 
294
 
 
295
    if(numBufFrames > 0){
 
296
    
 
297
        if(qseqResult){
 
298
            const int n = body->numJoints();
 
299
            qseqResult->setNumFrames(frame + numBufFrames);
 
300
            for(int i=0; i < numBufFrames; ++i){
 
301
                MultiValueSeq::View qResult = qseqResult->frame(frame + i);
 
302
                MultiValueSeq::View qResultBuf = qseqResultBuf->frame(i);
 
303
                for(int j=0; j < n; ++j){
 
304
                    qResult[j] = qResultBuf[j];
 
305
                }
 
306
            }
 
307
            qseqResultBuf->setNumFrames(0);
 
308
        }
 
309
        
 
310
        rootResult->setNumFrames(frame + numBufFrames);
 
311
        for(int i=0; i < numBufFrames; ++i){
 
312
            rootResult->at(frame + i, 0) = rootResultBuf->at(i, 0);
 
313
        }
 
314
        rootResultBuf->setNumFrames(0);
 
315
    }
 
316
 
 
317
    return (numBufFrames > 0);
 
318
}
 
319
    
 
320
 
 
321
KinematicsSimulatorItem::KinematicsSimulatorItem()
 
322
{
 
323
    impl = new KSIImpl(this);
 
324
}
 
325
 
 
326
 
 
327
KSIImpl::KSIImpl(KinematicsSimulatorItem* self)
 
328
    : self(self)
 
329
{
 
330
 
 
331
}
 
332
 
 
333
 
 
334
KinematicsSimulatorItem::KinematicsSimulatorItem(const KinematicsSimulatorItem& org)
 
335
    : SimulatorItem(org),
 
336
      impl(new KSIImpl(this, *org.impl))
 
337
{
 
338
    
 
339
}
 
340
 
 
341
 
 
342
KSIImpl::KSIImpl(KinematicsSimulatorItem* self, const KSIImpl& org)
 
343
    : self(self)
 
344
{
 
345
 
 
346
}
 
347
 
 
348
 
 
349
KinematicsSimulatorItem::~KinematicsSimulatorItem()
 
350
{
 
351
    delete impl;
 
352
}
 
353
 
 
354
 
 
355
ItemPtr KinematicsSimulatorItem::doDuplicate() const
 
356
{
 
357
    return new KinematicsSimulatorItem(*this);
 
358
}
 
359
 
 
360
 
 
361
QWidget* KinematicsSimulatorItem::settingPanel()
 
362
{
 
363
    return 0;
 
364
}
 
365
 
 
366
 
 
367
bool KinematicsSimulatorItem::doStartSimulation()
 
368
{
 
369
    return impl->doStartSimulation();
 
370
}
 
371
 
 
372
 
 
373
bool KSIImpl::doStartSimulation()
 
374
{
 
375
    if(TRACE_FUNCTIONS){
 
376
        cout << "BodyUnit::doStartSimulation()" << endl;
 
377
    }
 
378
 
 
379
    bool result = setupBodies();
 
380
 
 
381
    if(result){
 
382
        currentFrame = 0;
 
383
        frameAtLastBufferWriting = 0;
 
384
    }
 
385
    
 
386
    return result;
 
387
}
 
388
 
 
389
 
 
390
bool KSIImpl::setupBodies()
 
391
{
 
392
    if(TRACE_FUNCTIONS){
 
393
        cout << "KSIImpl::setupBodies()" << endl;
 
394
    }
 
395
    
 
396
    mainFrameRate = 0;
 
397
    bodyUnits.clear();
 
398
    
 
399
    WorldItemPtr worldItem = self->findOwnerItem<WorldItem>();
 
400
    if(worldItem){
 
401
        ItemList<BodyItem> bodyItems = worldItem->getBodyItems();
 
402
        for(size_t i=0; i < bodyItems.size(); ++i){
 
403
            BodyUnit unit;
 
404
            if(unit.initialize(bodyItems[i])){
 
405
                bodyUnits.push_back(unit);
 
406
                if(unit.frameRate > mainFrameRate){
 
407
                    mainFrameRate = unit.frameRate;
 
408
                }
 
409
            }
 
410
        }
 
411
    }
 
412
 
 
413
    return (!bodyUnits.empty() && mainFrameRate > 0);
 
414
}
 
415
 
 
416
 
 
417
bool KinematicsSimulatorItem::doStepSimulation()
 
418
{
 
419
    return impl->doStepSimulation();
 
420
}
 
421
 
 
422
 
 
423
bool KSIImpl::doStepSimulation()
 
424
{
 
425
    if(TRACE_FUNCTIONS){
 
426
        cout << "KSIImpl::doStepSimulation()" << endl;
 
427
    }
 
428
    
 
429
    currentFrame++;
 
430
 
 
431
    bodyUnitsToPutResult.clear();
 
432
    
 
433
    for(size_t i=0; i < bodyUnits.size(); ++i){
 
434
        BodyUnit& unit = bodyUnits[i];
 
435
        int frame = static_cast<int>(currentFrame * unit.frameRate / mainFrameRate);
 
436
        if(frame > unit.frame){
 
437
            if(unit.updatePosition(frame)){
 
438
                bodyUnitsToPutResult.push_back(&unit);
 
439
            }
 
440
        }
 
441
    }
 
442
 
 
443
    bool doContinue = !bodyUnitsToPutResult.empty();
 
444
 
 
445
    if(doContinue){
 
446
        self->lockResults();
 
447
        
 
448
        for(size_t i=0; i < bodyUnitsToPutResult.size(); ++i){
 
449
            bodyUnitsToPutResult[i]->putResultToBuf();
 
450
        }
 
451
        frameAtLastBufferWriting = currentFrame;
 
452
 
 
453
        self->requestToFlushResults();
 
454
        
 
455
        self->unlockResults();
 
456
    }
 
457
 
 
458
    return doContinue;
 
459
}
 
460
 
 
461
 
 
462
double KinematicsSimulatorItem::doFlushResults()
 
463
{
 
464
    return impl->doFlushResults();
 
465
}
 
466
 
 
467
 
 
468
double KSIImpl::doFlushResults()
 
469
{
 
470
    if(TRACE_FUNCTIONS){
 
471
        cout << "KSIImpl::doFlushResults()" << endl;
 
472
    }
 
473
    
 
474
    bodyUnitsToNotifyUpdate.clear();
 
475
 
 
476
    self->lockResults();
 
477
    
 
478
    for(size_t i=0; i < bodyUnits.size(); ++i){
 
479
        if(bodyUnits[i].flushBuf()){
 
480
            bodyUnitsToNotifyUpdate.push_back(&bodyUnits[i]);
 
481
        }
 
482
    }
 
483
 
 
484
    int frame = frameAtLastBufferWriting;
 
485
    
 
486
    self->unlockResults();
 
487
 
 
488
    for(size_t i=0; i < bodyUnitsToNotifyUpdate.size(); ++i){
 
489
        bodyUnitsToNotifyUpdate[i]->rootResultItem->notifyUpdate();
 
490
    }
 
491
 
 
492
    return (frame / mainFrameRate);
 
493
}
 
494
 
 
495
 
 
496
double KinematicsSimulatorItem::doStopSimulation()
 
497
{
 
498
    if(TRACE_FUNCTIONS){
 
499
        cout << "KinematicsSimulatorItem::doStopSimulation()" << endl;
 
500
    }
 
501
 
 
502
    return (impl->currentFrame / impl->mainFrameRate);
 
503
}
 
504
 
 
505
 
 
506
bool KinematicsSimulatorItem::store(Archive& archive)
 
507
{
 
508
    return true;
 
509
}
 
510
 
 
511
 
 
512
bool KinematicsSimulatorItem::restore(const Archive& archive)
 
513
{
 
514
    return true;
 
515
}