Blender  V3.3
CopyPose.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Benoit Bolsee. */
3 
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  unsigned 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 
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 
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 
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  unsigned 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 
423 {
424  //IMO this should be done, no idea if it is enough (wrt Distance impl)
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 {
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).array().abs().maxCoeff();
474  if (timestep*maxChidot > m_maxDeltaChi) {
475  timestep = m_maxDeltaChi/maxChidot;
476  }
477  return timestep;
478 }
479 
480 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble ny
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
represents both translational and rotational velocities.
Definition: frames.hpp:679
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:143
int addChannel(const void *device, const char *name, unsigned int maxItemSize)
Definition: Cache.cpp:201
double * addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold)
Definition: Cache.cpp:599
const void * getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp)
Definition: Cache.cpp:543
virtual void reset(unsigned int nc, double accuracy, unsigned int maximum_iterations)
ConstraintCallback m_constraintCallback
virtual void initCache(Cache *_cache)
Definition: CopyPose.cpp:129
virtual double getMaxTimestep(double &timestep)
Definition: CopyPose.cpp:468
virtual bool initialise(Frame &init_pose)
Definition: CopyPose.cpp:116
virtual ~CopyPose()
Definition: CopyPose.cpp:112
virtual bool setControlParameters(struct ConstraintValues *_values, unsigned int _nvalues, double timestep)
Definition: CopyPose.cpp:358
virtual void updateJacobian()
Definition: CopyPose.cpp:273
CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100)
Definition: CopyPose.cpp:17
virtual void modelUpdate(Frame &_external_pose, const Timestamp &timestamp)
Definition: CopyPose.cpp:123
virtual void pushCache(const Timestamp &timestamp)
Definition: CopyPose.cpp:255
virtual const ConstraintValues * getControlParameters(unsigned int *_nvalues)
Definition: CopyPose.cpp:453
virtual void updateKinematics(const Timestamp &timestamp)
Definition: CopyPose.cpp:262
virtual void updateControlOutput(const Timestamp &timestamp)
Definition: CopyPose.cpp:422
int len
Definition: draw_manager.c:108
#define e_scalar
Definition: eigen_types.hpp:37
#define e_identity_matrix
Definition: eigen_types.hpp:42
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
Definition: math_float4.h:513
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:367
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cpp:22
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:351
const unsigned int maxPoseCacheSize
Definition: CopyPose.cpp:16
const Frame F_identity
unsigned int CacheTS
Definition: Cache.hpp:32
@ ACT_TOLERANCE
@ ACT_FEEDBACK
@ ACT_VELOCITY
struct ConstraintSingleValue * values
unsigned int interpolate
Definition: Cache.hpp:44
double realTimestep
Definition: Cache.hpp:37
unsigned int cache
Definition: Cache.hpp:42
CacheTS cacheTimestamp
Definition: Cache.hpp:38
unsigned int reiterate
Definition: Cache.hpp:41
unsigned int substep
Definition: Cache.hpp:40