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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
|
/* SPDX-License-Identifier: LGPL-2.1-or-later
* Copyright 2009 Benoit Bolsee. */
/** \file
* \ingroup intern_itasc
*/
#ifndef ARMATURE_HPP_
#define ARMATURE_HPP_
#include "ControlledObject.hpp"
#include "ConstraintSet.hpp"
#include "kdl/treejnttojacsolver.hpp"
#include "kdl/treefksolverpos_recursive.hpp"
#include <vector>
namespace iTaSC {
class Armature: public iTaSC::ControlledObject {
public:
Armature();
virtual ~Armature();
bool addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip=F_identity, const Inertia& M = Inertia::Zero());
// general purpose constraint on joint
int addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param=NULL, bool _freeParam=false, bool _substep=false);
// specific limit constraint on joint
int addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max);
double getMaxJointChange();
double getMaxEndEffectorChange();
bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip);
bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root);
virtual bool finalize();
virtual int addEndEffector(const std::string& name);
virtual const Frame& getPose(const unsigned int end_effector);
virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback);
virtual void updateKinematics(const Timestamp& timestamp);
virtual void pushCache(const Timestamp& timestamp);
virtual void updateControlOutput(const Timestamp& timestamp);
virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0);
virtual void initCache(Cache *_cache);
virtual bool setJointArray(const KDL::JntArray& joints);
virtual const KDL::JntArray& getJointArray();
virtual double getArmLength()
{
return m_armlength;
}
struct Effector_struct {
std::string name;
Frame oldpose;
Frame pose;
Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;}
};
typedef std::vector<Effector_struct> EffectorList;
enum ID {
ID_JOINT=1,
ID_JOINT_RX=2,
ID_JOINT_RY=3,
ID_JOINT_RZ=4,
ID_JOINT_TX=2,
ID_JOINT_TY=3,
ID_JOINT_TZ=4,
};
struct JointConstraint_struct {
SegmentMap::const_iterator segment;
ConstraintSingleValue value[3];
ConstraintValues values[3];
ConstraintCallback function;
unsigned int v_nr;
unsigned int y_nr; // first coordinate of constraint in Y vector
void* param;
bool freeParam;
bool substep;
JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep);
~JointConstraint_struct();
};
typedef std::vector<JointConstraint_struct*> JointConstraintList;
struct Joint_struct {
KDL::Joint::JointType type;
unsigned short ndof;
bool useLimit;
bool locked;
double rest;
double min;
double max;
Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) :
type(_type), ndof(_ndof), rest(_rest) { useLimit=locked=false; min=0.0; max=0.0; }
};
typedef std::vector<Joint_struct> JointList;
protected:
virtual void updateJacobian();
private:
static std::string m_root;
Tree m_tree;
unsigned int m_njoint;
unsigned int m_nconstraint;
unsigned int m_noutput;
unsigned int m_neffector;
bool m_finalized;
Cache* m_cache;
double *m_buf;
int m_qCCh;
CacheTS m_qCTs;
int m_yCCh;
#if 0
CacheTS m_yCTs;
#endif
JntArray m_qKdl;
JntArray m_oldqKdl;
JntArray m_newqKdl;
JntArray m_qdotKdl;
Jacobian* m_jac;
double m_armlength;
KDL::TreeJntToJacSolver* m_jacsolver;
KDL::TreeFkSolverPos_recursive* m_fksolver;
EffectorList m_effectors;
JointConstraintList m_constraints;
JointList m_joints;
void pushQ(CacheTS timestamp);
bool popQ(CacheTS timestamp);
//void pushConstraints(CacheTS timestamp);
//bool popConstraints(CacheTS timestamp);
};
}
#endif /* ARMATURE_HPP_ */
|
