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 | /**
* @file KinematicChain.h
*
* @author <a href="mailto:xu@informatik.hu-berlin.de">Xu, Yuan</a>
* Implementation of Kinematic Chain
*/
#ifndef _KINEMATICCHAIN_H
#define _KINEMATICCHAIN_H
#include "Tools/Kinematics/Link.h"
#include "Representations/Infrastructure/JointData.h"
#include "Representations/Infrastructure/Configuration.h"
class KinematicChain : public naoth::Printable
{
public:
enum LinkID
{
Torso,
Neck,
Head,
LShoulder,
LBicep,
LElbow,
LForeArm, // motorized or passive
LHand, // only avaliable when in H25 with motorized LForeArm
LPelvis,
LHip,
LThigh,
LTibia,
LAnkle,
LFoot,
RShoulder,
RBicep,
RElbow,
RForeArm, // motorized or passive
RHand, // only avaliable when in H25 with motorized RForeArm
RPelvis,
RHip,
RThigh,
RTibia,
RAnkle,
RFoot,
Hip,
numOfLinks
};
private:
void buildLinkChains();
/**
* Set the positions of the links relatively to the parent link.
*/
void initLinkPositions();
/**
* Define the axes for the movable links (joints)
*/
void initJointsAxes();
/**
* Create a link between the movable links of the kinematic chain
* and the corresponding values in the JointData (position, velocity, acceleration).
*/
void linkJointData(naoth::JointData& jointData);
/**
* read from config
*/
void initMassesInfo();
public:
Kinematics::Link theLinks[numOfLinks];
Vector3d CoM; // center of mass
inline Kinematics::Link& getLink(LinkID id) {
return theLinks[id];
}
inline const Kinematics::Link& getLink(LinkID id) const {
return theLinks[id];
}
inline Kinematics::Link& getLink(int i) {
assert(i > -1 && i < numOfLinks);
return theLinks[i];
}
KinematicChain();
virtual ~KinematicChain();
static std::string getLinkName(const Kinematics::Link& node);<--- Function 'getLinkName' argument 1 names different: declaration 'node' definition 'link'.
static std::string getLinkName(const Kinematics::Link *node);<--- Function 'getLinkName' argument 1 names different: declaration 'node' definition 'link'.
static std::string getLinkName(LinkID link);<--- Function 'getLinkName' argument 1 names different: declaration 'link' definition 'j'.
std::string test(const Kinematics::Link& node) const;
void init(naoth::JointData& jointData);
void updateCoM();
Vector2d calculateZMP() const;
virtual void print(std::ostream& stream) const;
bool is_initialized() { return initialized; }
private:
double sumMass;
bool initialized;
};
std::ostream & operator<<(std::ostream& os, const Kinematics::Link& node);
// declare two different representations:
// based on the measured and requested joint positions
class KinematicChainSensor: public KinematicChain {};
class KinematicChainMotor: public KinematicChain {};
#endif /* _KINEMATICCHAIN_H */
|