← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/BodyPlugin/BodyMotionEngine.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/BodyMotionEngine.cpp
 
1
/**
 
2
   \file
 
3
   \author Shin'ichiro Nakaoka
 
4
*/
 
5
 
 
6
#include "BodyMotionEngine.h"
 
7
#include "BodyItem.h"
 
8
#include "BodyMotionItem.h"
 
9
#include <cnoid/TimeSyncItemEngineManager>
 
10
#include <boost/bind.hpp>
 
11
 
 
12
using namespace boost;
 
13
using namespace cnoid;
 
14
 
 
15
namespace {
 
16
 
 
17
    const bool TRACE_FUNCTIONS = false;    
 
18
 
 
19
    class BodyMotionEngine : public TimeSyncItemEngine
 
20
    {
 
21
    public:
 
22
        BodyItemPtr bodyItem;
 
23
        BodyPtr body;
 
24
        MultiValueSeqPtr qSeq;
 
25
        MultiAffine3SeqPtr positions;
 
26
        Vector3SeqPtr relativeZmpSeq;
 
27
        bool calcForwardKinematics;
 
28
 
 
29
        BodyMotionEngine() { }
 
30
 
 
31
        bool initialize(Item* item) {
 
32
 
 
33
            BodyMotionItem* bodyMotionItem;
 
34
            MultiValueSeqItem* multiValueSeqItem;
 
35
            MultiAffine3SeqItem* multiAffine3SeqItem;;
 
36
 
 
37
            if((bodyMotionItem = dynamic_cast<BodyMotionItem*>(item))){
 
38
                if(setBodyItem(item)){
 
39
                    qSeq = bodyMotionItem->jointPosSeq();
 
40
                    positions = bodyMotionItem->linkPosSeq();
 
41
                    if(bodyMotionItem->hasRelativeZmpSeqItem()){
 
42
                        relativeZmpSeq = bodyMotionItem->relativeZmpSeq();
 
43
                    }
 
44
                }
 
45
 
 
46
            } else if((multiValueSeqItem = dynamic_cast<MultiValueSeqItem*>(item))){
 
47
                if(setBodyItem(item)){
 
48
                    qSeq = multiValueSeqItem->seq();
 
49
                }
 
50
                
 
51
            } else if((multiAffine3SeqItem = dynamic_cast<MultiAffine3SeqItem*>(item))){
 
52
                if(setBodyItem(item)){
 
53
                    positions = multiAffine3SeqItem->seq();
 
54
                }
 
55
            }
 
56
 
 
57
            if(bodyItem){
 
58
                calcForwardKinematics = !(positions && positions->numParts() > 1);
 
59
                item->sigUpdated().connect(bind(&BodyMotionEngine::notifyUpdate, this));
 
60
            }
 
61
            
 
62
            return (bodyItem);
 
63
        }
 
64
 
 
65
        bool setBodyItem(Item* item){
 
66
            bodyItem = item->findOwnerItem<BodyItem>();
 
67
            if(bodyItem){
 
68
                body = bodyItem->body();
 
69
            }
 
70
            return (bodyItem);
 
71
        }
 
72
        
 
73
        virtual bool onTimeChanged(double time){
 
74
 
 
75
            bool isActive = false;
 
76
            bool fkDone = false;
 
77
            
 
78
            if(qSeq){
 
79
                isActive = setJointPositions(time);
 
80
            }
 
81
            if(positions){
 
82
                isActive |= setLinkPositions(time);
 
83
                if(positions->numParts() == 1){
 
84
                    body->calcForwardKinematics(); // FK from the root
 
85
                    fkDone = true;
 
86
                }
 
87
            }
 
88
            if(relativeZmpSeq){
 
89
                if(calcForwardKinematics){
 
90
                    bodyItem->calcForwardKinematics();
 
91
                    fkDone = true;
 
92
                }
 
93
                isActive |= setRelativeZmp(time);
 
94
            }
 
95
 
 
96
            bodyItem->notifyKinematicStateChange(!fkDone && calcForwardKinematics);
 
97
 
 
98
            return isActive;
 
99
        }
 
100
 
 
101
        static inline bool clamp(int& frame, int numFrames) {
 
102
 
 
103
            if(frame < 0){
 
104
                frame = 0;
 
105
                return false;
 
106
            } else if(frame >= numFrames){
 
107
                frame = numFrames - 1;
 
108
                return false;
 
109
            }
 
110
            return true;
 
111
        }
 
112
 
 
113
        bool setLinkPositions(double time){
 
114
 
 
115
            bool isValidTime = false;
 
116
 
 
117
            const int numLinks = positions->numParts();
 
118
            const int numFrames = positions->numFrames();
 
119
            if(numLinks > 0 && numFrames > 0){
 
120
                int frame = positions->frameOfTime(time);
 
121
                isValidTime = clamp(frame, numFrames);
 
122
                for(int i=0; i < numLinks; ++i){
 
123
                    Link* link = body->link(i);
 
124
                    const Affine3& position = positions->at(frame, i);
 
125
                    link->p = position.translation();
 
126
                    link->R = position.linear();
 
127
                }
 
128
            }
 
129
 
 
130
            return isValidTime;
 
131
        }
 
132
 
 
133
        bool setJointPositions(double time){
 
134
 
 
135
            bool isValidTime = false;
 
136
 
 
137
            const int numJoints = qSeq->numParts();
 
138
            int frame = qSeq->frameOfTime(time);
 
139
            const int numFrames = qSeq->numFrames();
 
140
            if(numJoints > 0 && numFrames > 0){
 
141
                isValidTime = clamp(frame, numFrames);
 
142
                MultiValueSeq::View q = qSeq->frame(frame);
 
143
                for(int i=0; i < numJoints; ++i){
 
144
                    Link* link = body->joint(i);
 
145
                    link->q = q[i];
 
146
                }
 
147
            }
 
148
 
 
149
            return isValidTime;
 
150
        }    
 
151
 
 
152
        bool setRelativeZmp(double time){
 
153
 
 
154
            bool isValidTime = false;
 
155
            int frame = relativeZmpSeq->frameOfTime(time);
 
156
            const int numFrames = relativeZmpSeq->numFrames();
 
157
            if(numFrames > 0){
 
158
                isValidTime = clamp(frame, numFrames);
 
159
                Link* rootLink = body->rootLink();
 
160
                const Vector3& relativeZmp = (*relativeZmpSeq)[frame];
 
161
                const Vector3 absZmp = rootLink->R * relativeZmp + rootLink->p;
 
162
                bodyItem->setZmp(absZmp);
 
163
            }
 
164
            return isValidTime;
 
165
        }    
 
166
 
 
167
    };
 
168
 
 
169
    typedef boost::intrusive_ptr<BodyMotionEngine> BodyMotionEnginePtr;
 
170
    
 
171
 
 
172
    TimeSyncItemEnginePtr createBodyMotionEngine(Item* sourceItem)
 
173
    {
 
174
        BodyMotionEnginePtr engine = new BodyMotionEngine();
 
175
        if(!engine->initialize(sourceItem)){
 
176
            engine.reset(NULL);
 
177
        }
 
178
        return engine;
 
179
    }
 
180
}
 
181
 
 
182
 
 
183
void cnoid::initializeBodyMotionEngine(ExtensionManager& em)
 
184
{
 
185
    em.timeSyncItemEngineManger().addEngineFactory(createBodyMotionEngine);
 
186
}