17 CopyPose::CopyPose(
unsigned int control_output,
unsigned int dynamic_output,
double armlength,
double accuracy,
unsigned int maximum_iterations):
20 m_poseCCh(-1),m_poseCTs(0)
22 m_maxerror = armlength/2.0;
23 m_outputControl = (control_output &
CTL_ALL);
24 unsigned int _nc = nBitsOn(m_outputControl);
28 reset(_nc, accuracy, maximum_iterations);
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));
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;
48 m_Wy(_nc) = m_pos.alpha;
55 m_Wy(_nc) = m_pos.alpha;
62 m_Wy(_nc) = m_pos.alpha;
68 m_values[m_nvalues].
number = npos;
69 m_values[m_nvalues++].
values = m_posData;
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;
82 m_Wy(_nc) = m_rot.alpha;
89 m_Wy(_nc) = m_rot.alpha;
96 m_Wy(_nc) = m_rot.alpha;
102 m_values[m_nvalues].
number = nrot;
103 m_values[m_nvalues++].
values = m_rotData;
109 m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
135 m_poseCCh = m_cache->
addChannel(
this,
"Xf", m_poseCacheSize*
sizeof(
double));
141 double* CopyPose::pushValues(
double* item, ControlState* _state,
unsigned int mask)
143 ControlState::ControlValue* _yval;
146 *item++ = _state->alpha;
148 *item++ = _state->tolerance;
150 for (i=0, _yval=_state->output; i<_state->
ny;
mask<<=1) {
151 if (m_outputControl &
mask) {
152 if (m_outputDynamic &
mask) {
154 *item++ = _yval->yddot;
163 void CopyPose::pushPose(
CacheTS timestamp)
165 if (m_poseCCh >= 0) {
166 if (m_poseCacheSize) {
176 m_poseCTs = timestamp;
180 double* CopyPose::restoreValues(
double* item, ConstraintValues* _values, ControlState* _state,
unsigned int mask)
182 ConstraintSingleValue* _data;
183 ControlState::ControlValue* _yval;
186 _values->alpha = _state->alpha = *item++;
187 _values->feedback = _state->K = *item++;
188 _values->tolerance = _state->tolerance = *item++;
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;
193 if (m_outputDynamic &
mask) {
194 _data->yd = _yval->yd = *item++;
195 _data->yddot = _yval->yddot = *item++;
205 bool CopyPose::popPose(
CacheTS timestamp)
208 if (m_poseCCh >= 0) {
212 if (timestamp != m_poseCTs) {
216 item = restoreValues(item, &m_values[i], &m_pos,
CTL_POSITIONX);
222 item = restoreValues(item, &m_values[i], &m_rot,
CTL_ROTATIONX);
226 m_poseCTs = timestamp;
234 void CopyPose::interpolateOutput(ControlState* _state,
unsigned int mask,
const Timestamp& timestamp)
236 ControlState::ControlValue* _yval;
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;
245 _yval->yd = _yval->nextyd;
246 _yval->yddot = _yval->nextyddot;
285 void CopyPose::updateState(
ConstraintValues* _values, ControlState* _state,
unsigned int mask,
double timestep)
288 ControlState::ControlValue* _yval;
294 _state->alpha = _values->
alpha;
305 for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j;
mask <<= 1,
id++) {
306 if (m_outputControl &
mask) {
308 m_Wy(i) = _state->alpha;
310 for (k=0, _data=_values->
values; k<_values->number; k++, _data++) {
311 if (_data->
id ==
id) {
318 _data->
yd = _yval->yd+_data->
yddot*timestep;
321 _yval->nextyd = _data->
yd;
326 _yval->yddot = (_data->
yd-_yval->yd)/timestep;
330 _yval->yd = _yval->nextyd;
331 _yval->yddot = _yval->nextyddot;
338 _yval->yd = _data->
yd - _data->
yddot*timestep;
339 _yval->nextyd = _data->
yd;
340 _yval->nextyddot = _data->
yddot;
342 _yval->yddot = (_data->
yd-_yval->yd)/timestep;
344 _yval->yd = _yval->nextyd;
345 _yval->yddot = _yval->nextyddot;
360 while (_nvalues > 0) {
376 ControlState::ControlValue* _yval;
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);
385 _data->
yd = _yval->yd;
386 _data->
yddot = _yval->yddot;
395 void CopyPose::updateOutput(Vector& vel, ControlState* _state,
unsigned int mask)
397 ControlState::ControlValue* _yval;
403 for (j=0, _yval=_state->output; j<3; j++) {
404 if (m_outputControl & (
mask<<j)) {
410 if (
len > m_maxerror)
411 coef = m_maxerror/
len;
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));
_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)
represents both translational and rotational velocities.
A concrete implementation of a 3 dimensional vector class.
int addChannel(const void *device, const char *name, unsigned int maxItemSize)
double * addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold)
const void * getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp)
KDL::Frame m_internalPose
KDL::Frame m_externalPose
virtual void reset(unsigned int nc, double accuracy, unsigned int maximum_iterations)
ConstraintCallback m_constraintCallback
virtual void initCache(Cache *_cache)
virtual double getMaxTimestep(double ×tep)
virtual bool initialise(Frame &init_pose)
virtual bool setControlParameters(struct ConstraintValues *_values, unsigned int _nvalues, double timestep)
virtual void updateJacobian()
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)
virtual void modelUpdate(Frame &_external_pose, const Timestamp ×tamp)
virtual void pushCache(const Timestamp ×tamp)
virtual const ConstraintValues * getControlParameters(unsigned int *_nvalues)
virtual void updateKinematics(const Timestamp ×tamp)
virtual void updateControlOutput(const Timestamp ×tamp)
#define e_identity_matrix
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
const unsigned int maxPoseCacheSize
struct ConstraintSingleValue * values