Fix OSX compilation problem with malloc.h in itasc
[blender.git] / intern / itasc / CopyPose.cpp
1 /* $Id$
2  * CopyPose.cpp
3  *
4  *  Created on: Mar 17, 2009
5  *      Author: benoit bolsee
6  */
7
8 #include "CopyPose.hpp"
9 #include "kdl/kinfam_io.hpp"
10 #include <math.h>
11 #include <string.h>
12
13 namespace iTaSC
14 {
15
16 const unsigned int maxPoseCacheSize = (2*(3+3*2));
17 CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations):
18     ConstraintSet(),
19         m_cache(NULL),
20     m_poseCCh(-1),m_poseCTs(0)
21 {
22         m_maxerror = armlength/2.0;
23         m_outputControl = (control_output & CTL_ALL);
24         int _nc = nBitsOn(m_outputControl);
25         if (!_nc) 
26                 return;
27         // reset the constraint set
28         reset(_nc, accuracy, maximum_iterations);
29         _nc = 0;
30         m_nvalues = 0;
31         int nrot = 0, npos = 0;
32         int nposCache = 0, nrotCache = 0;
33         m_outputDynamic = (dynamic_output & m_outputControl);
34         memset(m_values, 0, sizeof(m_values));
35         memset(m_posData, 0, sizeof(m_posData));
36         memset(m_rotData, 0, sizeof(m_rotData));
37         memset(&m_rot, 0, sizeof(m_rot));
38         memset(&m_pos, 0, sizeof(m_pos));
39         if (m_outputControl & CTL_POSITION) {
40                 m_pos.alpha = 1.0;              
41                 m_pos.K = 20.0;         
42                 m_pos.tolerance = 0.05; 
43                 m_values[m_nvalues].alpha = m_pos.alpha;
44                 m_values[m_nvalues].feedback = m_pos.K;
45                 m_values[m_nvalues].tolerance = m_pos.tolerance;
46                 m_values[m_nvalues].id = ID_POSITION;
47                 if (m_outputControl & CTL_POSITIONX) {
48                     m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
49                         m_Cf(_nc++,0)=1.0;
50                         m_posData[npos++].id = ID_POSITIONX;
51                         if (m_outputDynamic & CTL_POSITIONX)
52                                 nposCache++;
53                 } 
54                 if (m_outputControl & CTL_POSITIONY) {
55                     m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
56                         m_Cf(_nc++,1)=1.0;
57                         m_posData[npos++].id = ID_POSITIONY;
58                         if (m_outputDynamic & CTL_POSITIONY)
59                                 nposCache++;
60                 }
61                 if (m_outputControl & CTL_POSITIONZ) {
62                     m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
63                         m_Cf(_nc++,2)=1.0;
64                         m_posData[npos++].id = ID_POSITIONZ;
65                         if (m_outputDynamic & CTL_POSITIONZ)
66                                 nposCache++;
67                 }
68                 m_values[m_nvalues].number = npos;
69                 m_values[m_nvalues++].values = m_posData;
70                 m_pos.firsty = 0;
71                 m_pos.ny = npos;
72         }
73         if (m_outputControl & CTL_ROTATION) {
74                 m_rot.alpha = 1.0;              
75                 m_rot.K = 20.0;         
76                 m_rot.tolerance = 0.05; 
77                 m_values[m_nvalues].alpha = m_rot.alpha;
78                 m_values[m_nvalues].feedback = m_rot.K;
79                 m_values[m_nvalues].tolerance = m_rot.tolerance;
80                 m_values[m_nvalues].id = ID_ROTATION;
81                 if (m_outputControl & CTL_ROTATIONX) {
82                     m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
83                         m_Cf(_nc++,3)=1.0;
84                         m_rotData[nrot++].id = ID_ROTATIONX;
85                         if (m_outputDynamic & CTL_ROTATIONX)
86                                 nrotCache++;
87                 }
88                 if (m_outputControl & CTL_ROTATIONY) {
89                     m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
90                         m_Cf(_nc++,4)=1.0;
91                         m_rotData[nrot++].id = ID_ROTATIONY;
92                         if (m_outputDynamic & CTL_ROTATIONY)
93                                 nrotCache++;
94                 }
95                 if (m_outputControl & CTL_ROTATIONZ) {
96                     m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
97                         m_Cf(_nc++,5)=1.0;
98                         m_rotData[nrot++].id = ID_ROTATIONZ;
99                         if (m_outputDynamic & CTL_ROTATIONZ)
100                                 nrotCache++;
101                 }
102                 m_values[m_nvalues].number = nrot;
103                 m_values[m_nvalues++].values = m_rotData;
104                 m_rot.firsty = npos;
105                 m_rot.ny = nrot;
106         }
107         assert(_nc == m_nc);
108     m_Jf=e_identity_matrix(6,6);
109         m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
110 }
111
112 CopyPose::~CopyPose()
113 {
114 }
115
116 bool CopyPose::initialise(Frame& init_pose)
117 {
118     m_externalPose = m_internalPose = init_pose;
119         updateJacobian();
120     return true;
121 }
122
123 void CopyPose::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
124 {
125         m_internalPose = m_externalPose = _external_pose;
126         updateJacobian();
127 }
128
129 void CopyPose::initCache(Cache *_cache)
130 {
131     m_cache = _cache;
132     m_poseCCh = -1;
133     if (m_cache) {
134         // create one channel for the coordinates
135         m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize*sizeof(double));
136         // don't save initial value, it will be recomputed from external pose
137         //pushPose(0);
138     }
139 }
140
141 double* CopyPose::pushValues(double* item, ControlState* _state, unsigned int mask)
142 {
143         ControlState::ControlValue* _yval;
144         int i;
145
146         *item++ = _state->alpha;
147         *item++ = _state->K;
148         *item++ = _state->tolerance;
149
150         for (i=0, _yval=_state->output; i<_state->ny; mask<<=1) {
151                 if (m_outputControl & mask) {
152                         if (m_outputDynamic & mask) {
153                                 *item++ = _yval->yd;
154                                 *item++ = _yval->yddot;
155                         }
156                         _yval++;
157                         i++;
158                 }
159         }
160         return item;
161 }
162
163 void CopyPose::pushPose(CacheTS timestamp)
164 {
165     if (m_poseCCh >= 0) {
166                 if (m_poseCacheSize) {
167                         double buf[maxPoseCacheSize];
168                         double *item = buf;
169                         if (m_outputDynamic & CTL_POSITION)
170                                 item = pushValues(item, &m_pos, CTL_POSITIONX);
171                         if (m_outputDynamic & CTL_ROTATION)
172                                 item = pushValues(item, &m_rot, CTL_ROTATIONX);
173                         m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, m_poseCacheSize, KDL::epsilon);
174                 } else
175                         m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, NULL, 0, KDL::epsilon);
176                 m_poseCTs = timestamp;
177     }
178 }
179
180 double* CopyPose::restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask)
181 {
182         ConstraintSingleValue* _data;
183         ControlState::ControlValue* _yval;
184         int i, j;
185
186         _values->alpha = _state->alpha = *item++;
187         _values->feedback = _state->K = *item++;
188         _values->tolerance = _state->tolerance = *item++;
189
190         for (i=_state->firsty, j=i+_state->ny, _yval=_state->output, _data=_values->values; i<j; mask<<=1) {
191                 if (m_outputControl & mask) {
192                         m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
193                         if (m_outputDynamic & mask) {
194                                 _data->yd = _yval->yd = *item++;
195                                 _data->yddot = _yval->yddot = *item++;
196                         }
197                         _data++;
198                         _yval++;
199                         i++;
200                 }
201         }
202         return item;
203 }
204
205 bool CopyPose::popPose(CacheTS timestamp)
206 {
207         bool found = false;
208     if (m_poseCCh >= 0) {
209         double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
210                 if (item) {
211                         found = true;
212                         if (timestamp != m_poseCTs) {
213                                 int i=0;
214                                 if (m_outputControl & CTL_POSITION) {
215                                         if (m_outputDynamic & CTL_POSITION) {
216                                                 item = restoreValues(item, &m_values[i], &m_pos, CTL_POSITIONX);
217                                         }
218                                         i++;
219                                 }
220                                 if (m_outputControl & CTL_ROTATION) {
221                                         if (m_outputDynamic & CTL_ROTATION) {
222                                                 item = restoreValues(item, &m_values[i], &m_rot, CTL_ROTATIONX);
223                                         }
224                                         i++;
225                                 }
226                                 m_poseCTs = timestamp;
227                                 item = NULL;
228                         }
229         }
230     }
231     return found;
232 }
233
234 void CopyPose::interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp)
235 {
236         ControlState::ControlValue* _yval;
237         int i;
238
239         for (i=0, _yval=_state->output; i<_state->ny; mask <<= 1) {
240                 if (m_outputControl & mask) {
241                         if (m_outputDynamic & mask) {
242                                 if (timestamp.substep && timestamp.interpolate) {
243                                         _yval->yd += _yval->yddot*timestamp.realTimestep;
244                                 } else {
245                                         _yval->yd = _yval->nextyd;
246                                         _yval->yddot = _yval->nextyddot;
247                                 }
248                         }
249                         i++;
250                         _yval++;
251                 }
252         }
253 }
254
255 void CopyPose::pushCache(const Timestamp& timestamp)
256 {
257         if (!timestamp.substep && timestamp.cache) {
258         pushPose(timestamp.cacheTimestamp);
259         }
260 }
261
262 void CopyPose::updateKinematics(const Timestamp& timestamp)
263 {
264         if (timestamp.interpolate) {
265                 if (m_outputDynamic & CTL_POSITION)
266                         interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
267                 if (m_outputDynamic & CTL_ROTATION)
268                         interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp);
269         }
270         pushCache(timestamp);
271 }
272
273 void CopyPose::updateJacobian()
274 {
275     //Jacobian is always identity at the start of the constraint chain
276         //instead of going through complicated jacobian operation, implemented direct formula
277         //m_Jf(1,3) = m_internalPose.p.z();
278         //m_Jf(2,3) = -m_internalPose.p.y();
279         //m_Jf(0,4) = -m_internalPose.p.z();
280         //m_Jf(2,4) = m_internalPose.p.x();
281         //m_Jf(0,5) = m_internalPose.p.y();
282         //m_Jf(1,5) = -m_internalPose.p.x();
283 }
284
285 void CopyPose::updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep)
286 {
287         int id = (mask == CTL_ROTATIONX) ? ID_ROTATIONX : ID_POSITIONX;
288         ControlState::ControlValue* _yval;
289         ConstraintSingleValue* _data;
290         int i, j, k;
291     int action = 0;
292
293     if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
294         _state->alpha = _values->alpha;
295         action |= ACT_ALPHA;
296     }
297     if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
298         _state->tolerance = _values->tolerance;
299         action |= ACT_TOLERANCE;
300     }
301     if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
302         _state->K = _values->feedback;
303         action |= ACT_FEEDBACK;
304     }
305         for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j; mask <<= 1, id++) {
306                 if (m_outputControl & mask) {
307                         if (action)
308                                 m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
309                         // check if this controlled output is provided
310                         for (k=0, _data=_values->values; k<_values->number; k++, _data++) {
311                                 if (_data->id == id) {
312                                         switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
313                                         case 0:
314                                                 // no indication, keep current values
315                                                 break;
316                                         case ACT_VELOCITY:
317                                                 // only the velocity is given estimate the new value by integration
318                                                 _data->yd = _yval->yd+_data->yddot*timestep;
319                                                 // walkthrough
320                                         case ACT_VALUE:
321                                                 _yval->nextyd = _data->yd;
322                                                 // if the user sets the value, we assume future velocity is zero
323                                                 // (until the user changes the value again)
324                                                 _yval->nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
325                                                 if (timestep>0.0) {
326                                                         _yval->yddot = (_data->yd-_yval->yd)/timestep;
327                                                 } else {
328                                                         // allow the user to change target instantenously when this function
329                                                         // if called from setControlParameter with timestep = 0
330                                                         _yval->yd = _yval->nextyd;
331                                                         _yval->yddot = _yval->nextyddot;
332                                                 }
333                                                 break;
334                                         case (ACT_VALUE|ACT_VELOCITY):
335                                                 // the user should not set the value and velocity at the same time.
336                                                 // In this case, we will assume that he wants to set the future value
337                                                 // and we compute the current value to match the velocity
338                                                 _yval->yd = _data->yd - _data->yddot*timestep;
339                                                 _yval->nextyd = _data->yd;
340                                                 _yval->nextyddot = _data->yddot;
341                                                 if (timestep>0.0) {
342                                                         _yval->yddot = (_data->yd-_yval->yd)/timestep;
343                                                 } else {
344                                                         _yval->yd = _yval->nextyd;
345                                                         _yval->yddot = _yval->nextyddot;
346                                                 }
347                                                 break;
348                                         }
349                                 }
350                         }
351                         _yval++;
352                         i++;
353                 }
354         }
355 }
356
357
358 bool CopyPose::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
359 {
360         while (_nvalues > 0) {
361                 if (_values->id >= ID_POSITION && _values->id <= ID_POSITIONZ && (m_outputControl & CTL_POSITION)) {
362                         updateState(_values, &m_pos, CTL_POSITIONX, timestep);
363                 } 
364                 if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ && (m_outputControl & CTL_ROTATION)) {
365                         updateState(_values, &m_rot, CTL_ROTATIONX, timestep);
366                 }
367                 _values++;
368                 _nvalues--;
369         }
370     return true;
371 }
372
373 void CopyPose::updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask)
374 {
375         ConstraintSingleValue* _data;
376         ControlState::ControlValue* _yval;
377         int i, j;
378
379         _values->action = 0;
380
381         for (i=_state->firsty, j=0, _yval=_state->output, _data=_values->values; j<3; j++, mask<<=1) {
382                 if (m_outputControl & mask) {
383                         *(double*)&_data->y = vel(j);
384                         *(double*)&_data->ydot = m_ydot(i);
385                         _data->yd = _yval->yd;
386                         _data->yddot = _yval->yddot;
387                         _data->action = 0;
388                         i++;
389                         _data++;
390                         _yval++;
391                 }
392         }
393 }
394
395 void CopyPose::updateOutput(Vector& vel, ControlState* _state, unsigned int mask)
396 {
397         ControlState::ControlValue* _yval;
398         int i, j;
399         double coef=1.0;
400         if (mask & CTL_POSITION) {
401                 // put a limit on position error
402                 double len=0.0;
403                 for (j=0, _yval=_state->output; j<3; j++) {
404                         if (m_outputControl & (mask<<j)) {
405                                 len += KDL::sqr(_yval->yd-vel(j));
406                                 _yval++;
407                         }
408                 }
409                 len = KDL::sqrt(len);
410                 if (len > m_maxerror)
411                         coef = m_maxerror/len;
412         }
413         for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) {
414                 if (m_outputControl & (mask<<j)) {
415                         m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd-vel(j));
416                         _yval++;
417                         i++;
418                 }
419         }
420 }
421
422 void CopyPose::updateControlOutput(const Timestamp& timestamp)
423 {
424     //IMO this should be done, no idea if it is enough (wrt Distance impl)
425         Twist y = diff(F_identity, m_internalPose);
426         bool found = true;
427         if (!timestamp.substep) {
428                 if (!timestamp.reiterate) {
429                         found = popPose(timestamp.cacheTimestamp);
430                 }
431         }
432         if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
433                 // initialize first callback the application to get the current values
434                 int i=0;
435                 if (m_outputControl & CTL_POSITION) {
436                         updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
437                 }
438                 if (m_outputControl & CTL_ROTATION) {
439                         updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
440                 }
441                 if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
442                         setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0);
443                 }
444         }
445         if (m_outputControl & CTL_POSITION) {
446                 updateOutput(y.vel, &m_pos, CTL_POSITIONX);
447         }
448         if (m_outputControl & CTL_ROTATION) {
449                 updateOutput(y.rot, &m_rot, CTL_ROTATIONX);
450         }
451 }
452
453 const ConstraintValues* CopyPose::getControlParameters(unsigned int* _nvalues)
454 {
455         Twist y = diff(m_internalPose,F_identity);
456         int i=0;
457         if (m_outputControl & CTL_POSITION) {
458                 updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
459         }
460         if (m_outputControl & CTL_ROTATION) {
461                 updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
462         }
463         if (_nvalues)
464                 *_nvalues=m_nvalues; 
465         return m_values; 
466 }
467
468 double CopyPose::getMaxTimestep(double& timestep)
469 {
470         // CopyPose should not have any limit on linear velocity: 
471         // in case the target is out of reach, this can be very high.
472         // We will simply limit on rotation
473         e_scalar maxChidot = m_chidot.block(3,0,3,1).cwise().abs().maxCoeff();
474         if (timestep*maxChidot > m_maxDeltaChi) {
475                 timestep = m_maxDeltaChi/maxChidot;
476         }
477         return timestep;
478 }
479
480 }