| Line |
Branch |
Exec |
Source |
| 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 |
| 117 |
|
|
|