1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
|
/* SPDX-License-Identifier: LGPL-2.1-or-later
* Copyright 2009 Benoit Bolsee. */
/** \file
* \ingroup intern_itasc
*/
#ifndef COPYPOSE_H_
#define COPYPOSE_H_
#include "ConstraintSet.hpp"
namespace iTaSC{
using namespace KDL;
class CopyPose: public iTaSC::ConstraintSet
{
protected:
virtual void updateKinematics(const Timestamp& timestamp);
virtual void pushCache(const Timestamp& timestamp);
virtual void updateJacobian();
virtual bool initialise(Frame& init_pose);
virtual void initCache(Cache *_cache);
virtual void updateControlOutput(const Timestamp& timestamp);
virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
virtual double getMaxTimestep(double& timestep);
public:
enum ID { // constraint ID in callback and setControlParameter
ID_POSITION=0,
ID_POSITIONX=1,
ID_POSITIONY=2,
ID_POSITIONZ=3,
ID_ROTATION=4,
ID_ROTATIONX=5,
ID_ROTATIONY=6,
ID_ROTATIONZ=7,
};
enum CTL { // control ID in constructor to specify which output is constrainted
CTL_NONE=0x00,
CTL_POSITIONX=0x01, // the bit order is important: it matches the y output order
CTL_POSITIONY=0x02,
CTL_POSITIONZ=0x04,
CTL_POSITION=0x07,
CTL_ROTATIONX=0x08,
CTL_ROTATIONY=0x10,
CTL_ROTATIONZ=0x20,
CTL_ROTATION=0x38,
CTL_ALL=0x3F,
};
// use a combination of CTL_.. in control_output to specify which
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 ~CopyPose();
virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
private:
struct ConstraintSingleValue m_posData[3]; // index = controlled output in X,Y,Z order
struct ConstraintSingleValue m_rotData[3];
struct ConstraintValues m_values[2]; // index = group of controlled output, in position, rotation order
Cache* m_cache;
int m_poseCCh;
CacheTS m_poseCTs;
unsigned int m_poseCacheSize;
unsigned int m_outputDynamic; // combination of CTL_... determine which variables are dynamically controlled by the application
unsigned int m_outputControl; // combination of CTL_... determine which output are constrained
unsigned int m_nvalues; // number of elements used in m_values[]
double m_maxerror;
struct ControlState {
int firsty; // first y index
int ny; // number of y in output
double alpha;
double K;
double tolerance;
struct ControlValue {
double yddot;
double yd;
double nextyd;
double nextyddot;
} output[3]; // inded numbex = same as m_rotData
} m_rot, m_pos;
void pushPose(CacheTS timestamp);
bool popPose(CacheTS timestamp);
int nBitsOn(unsigned int v)
{ int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
double* pushValues(double* item, ControlState* _state, unsigned int mask);
void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);
};
}
#endif /* COPYROTATION_H_ */
|
