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