1 |
|
|
/* |
2 |
|
|
* Copyright 2012, |
3 |
|
|
* Florent Lamiraux |
4 |
|
|
* |
5 |
|
|
* CNRS |
6 |
|
|
* |
7 |
|
|
*/ |
8 |
|
|
|
9 |
|
|
#include <dynamic-graph/entity.h> |
10 |
|
|
#include <dynamic-graph/factory.h> |
11 |
|
|
#include <dynamic-graph/linear-algebra.h> |
12 |
|
|
#include <dynamic-graph/signal-ptr.h> |
13 |
|
|
#include <dynamic-graph/signal-time-dependent.h> |
14 |
|
|
|
15 |
|
|
#include <sot/core/matrix-geometry.hh> |
16 |
|
|
#include <sstream> |
17 |
|
|
#include <vector> |
18 |
|
|
|
19 |
|
|
namespace sot { |
20 |
|
|
namespace dynamic { |
21 |
|
|
using dynamicgraph::Entity; |
22 |
|
|
using dynamicgraph::SignalPtr; |
23 |
|
|
using dynamicgraph::SignalTimeDependent; |
24 |
|
|
using dynamicgraph::Vector; |
25 |
|
|
using dynamicgraph::sot::MatrixHomogeneous; |
26 |
|
|
|
27 |
|
|
class ZmpFromForces : public Entity { |
28 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
29 |
|
|
|
30 |
|
|
public: |
31 |
|
|
static const unsigned int footNumber = 2; |
32 |
|
|
ZmpFromForces(const std::string& name) |
33 |
|
|
: Entity(name), zmpSOUT_(CLASS_NAME + "::output(Vector)::zmp") { |
34 |
|
|
zmpSOUT_.setFunction(boost::bind(&ZmpFromForces::computeZmp, this, _1, _2)); |
35 |
|
|
signalRegistration(zmpSOUT_); |
36 |
|
|
for (unsigned int i = 0; i < footNumber; i++) { |
37 |
|
|
std::ostringstream forceName, positionName; |
38 |
|
|
forceName << CLASS_NAME << "::input(vector6)::force_" << i; |
39 |
|
|
positionName << CLASS_NAME << "::input(MatrixHomo)::sensorPosition_" << i; |
40 |
|
|
forcesSIN_[i] = new SignalPtr<Vector, int>(0, forceName.str()); |
41 |
|
|
sensorPositionsSIN_[i] = |
42 |
|
|
new SignalPtr<MatrixHomogeneous, int>(0, positionName.str()); |
43 |
|
|
signalRegistration(*forcesSIN_[i]); |
44 |
|
|
signalRegistration(*sensorPositionsSIN_[i]); |
45 |
|
|
zmpSOUT_.addDependency(*forcesSIN_[i]); |
46 |
|
|
zmpSOUT_.addDependency(*sensorPositionsSIN_[i]); |
47 |
|
|
} |
48 |
|
|
} |
49 |
|
|
virtual std::string getDocString() const { |
50 |
|
|
std::string docstring = |
51 |
|
|
"Compute ZMP from force sensor measures and positions\n" |
52 |
|
|
"\n" |
53 |
|
|
" Takes 4 signals as input:\n" |
54 |
|
|
" - force_0: wrench measured by force sensor 0 as a 6 dimensional " |
55 |
|
|
"vector\n" |
56 |
|
|
" - force_0: wrench measured by force sensor 1 as a 6 dimensional " |
57 |
|
|
"vector\n" |
58 |
|
|
" - sensorPosition_0: position of force sensor 0\n" |
59 |
|
|
" - sensorPosition_1: position of force sensor 1\n" |
60 |
|
|
" \n" |
61 |
|
|
" compute the Zero Momentum Point of the contact forces as measured " |
62 |
|
|
"by the \n" |
63 |
|
|
" input signals under the asumptions that the contact points between " |
64 |
|
|
"the\n" |
65 |
|
|
" robot and the environment are located in the same horizontal " |
66 |
|
|
"plane.\n"; |
67 |
|
|
return docstring; |
68 |
|
|
} |
69 |
|
|
|
70 |
|
|
private: |
71 |
|
|
Vector& computeZmp(Vector& zmp, int time) { |
72 |
|
|
double fnormal = 0; |
73 |
|
|
double sumZmpx = 0; |
74 |
|
|
double sumZmpy = 0; |
75 |
|
|
double sumZmpz = 0; |
76 |
|
|
zmp.resize(3); |
77 |
|
|
|
78 |
|
|
for (unsigned int i = 0; i < footNumber; ++i) { |
79 |
|
|
const Vector& f = forcesSIN_[i]->access(time); |
80 |
|
|
// Check that force is of dimension 6 |
81 |
|
|
if (f.size() != 6) { |
82 |
|
|
zmp.fill(0.); |
83 |
|
|
return zmp; |
84 |
|
|
} |
85 |
|
|
const MatrixHomogeneous& M = sensorPositionsSIN_[i]->access(time); |
86 |
|
|
double fx = M(0, 0) * f(0) + M(0, 1) * f(1) + M(0, 2) * f(2); |
87 |
|
|
double fy = M(1, 0) * f(0) + M(1, 1) * f(1) + M(1, 2) * f(2); |
88 |
|
|
double fz = M(2, 0) * f(0) + M(2, 1) * f(1) + M(2, 2) * f(2); |
89 |
|
|
|
90 |
|
|
if (fz > 0) { |
91 |
|
|
double Mx = M(0, 0) * f(3) + M(0, 1) * f(4) + M(0, 2) * f(5) + |
92 |
|
|
M(1, 3) * fz - M(2, 3) * fy; |
93 |
|
|
double My = M(1, 0) * f(3) + M(1, 1) * f(4) + M(1, 2) * f(5) + |
94 |
|
|
M(2, 3) * fx - M(0, 3) * fz; |
95 |
|
|
fnormal += fz; |
96 |
|
|
sumZmpx -= My; |
97 |
|
|
sumZmpy += Mx; |
98 |
|
|
sumZmpz += fz * M(2, 3); |
99 |
|
|
} |
100 |
|
|
} |
101 |
|
|
if (fnormal != 0) { |
102 |
|
|
zmp(0) = sumZmpx / fnormal; |
103 |
|
|
zmp(1) = sumZmpy / fnormal; |
104 |
|
|
zmp(2) = sumZmpz / fnormal; |
105 |
|
|
} else { |
106 |
|
|
zmp.fill(0.); |
107 |
|
|
} |
108 |
|
|
return zmp; |
109 |
|
|
} |
110 |
|
|
// Force as measured by force sensor on the foot |
111 |
|
|
SignalPtr<Vector, int>* forcesSIN_[footNumber]; |
112 |
|
|
SignalPtr<MatrixHomogeneous, int>* sensorPositionsSIN_[footNumber]; |
113 |
|
|
SignalTimeDependent<Vector, int> zmpSOUT_; |
114 |
|
|
}; // class ZmpFromForces |
115 |
|
|
} // namespace dynamic |
116 |
|
|
} // namespace sot |