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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
|
/* SPDX-License-Identifier: LGPL-2.1-or-later
* Copyright 2009 Ruben Smits. */
/** \file
* \ingroup intern_itasc
*/
#ifndef CONSTRAINTSET_HPP_
#define CONSTRAINTSET_HPP_
#include "kdl/frames.hpp"
#include "eigen_types.hpp"
#include "Cache.hpp"
#include <vector>
namespace iTaSC {
enum ConstraintAction {
ACT_NONE= 0,
ACT_VALUE= 1,
ACT_VELOCITY= 2,
ACT_TOLERANCE= 4,
ACT_FEEDBACK= 8,
ACT_ALPHA= 16
};
struct ConstraintSingleValue {
unsigned int id; // identifier of constraint value, depends on constraint
unsigned int action;// action performed, compbination of ACT_..., set on return
const double y; // actual constraint value
const double ydot; // actual constraint velocity
double yd; // current desired constraint value, changed on return
double yddot; // current desired constraint velocity, changed on return
ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {}
};
struct ConstraintValues {
unsigned int id; // identifier of group of constraint values, depend on constraint
unsigned short number; // number of constraints in list
unsigned short action; // action performed, ACT_..., set on return
double alpha; // constraint activation coefficient, should be [0..1]
double tolerance; // current desired tolerance on constraint, same unit than yd, changed on return
double feedback; // current desired feedback on error, in 1/sec, changed on return
struct ConstraintSingleValue* values;
ConstraintValues(): id(0), number(0), action(0), values(NULL) {}
};
class ConstraintSet;
typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
class ConstraintSet {
protected:
unsigned int m_nc;
e_scalar m_maxDeltaChi;
e_matrix m_Cf;
e_vector m_Wy,m_y,m_ydot;
e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta;
e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv;
KDL::Frame m_internalPose,m_externalPose;
ConstraintCallback m_constraintCallback;
void* m_constraintParam;
void* m_poseParam;
bool m_toggle;
bool m_substep;
double m_threshold;
unsigned int m_maxIter;
friend class Scene;
virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp);
virtual void updateKinematics(const Timestamp& timestamp)=0;
virtual void pushCache(const Timestamp& timestamp)=0;
virtual void updateJacobian()=0;
virtual void updateControlOutput(const Timestamp& timestamp)=0;
virtual void initCache(Cache *_cache) = 0;
virtual bool initialise(KDL::Frame& init_pose);
virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
virtual bool closeLoop();
virtual double getMaxTimestep(double& timestep);
public:
ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations);
ConstraintSet();
virtual ~ConstraintSet();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual bool registerCallback(ConstraintCallback _function, void* _param)
{
m_constraintCallback = _function;
m_constraintParam = _param;
return true;
}
virtual const e_vector& getControlOutput()const{return m_ydot;};
virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0;
virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0;
bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0);
virtual const e_matrix6& getJf() const{return m_Jf;};
virtual const KDL::Frame& getPose() const{return m_internalPose;};
virtual const e_matrix& getCf() const{return m_Cf;};
virtual const e_vector& getWy() const {return m_Wy;};
virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;};
virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
virtual unsigned int getNrOfConstraints(){return m_nc;};
void substep(bool _substep) {m_substep=_substep;}
bool substep() {return m_substep;}
};
}
#endif /* CONSTRAINTSET_HPP_ */
|
