1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* François Bleibel, |
4 |
|
|
* Olivier Stasse, |
5 |
|
|
* |
6 |
|
|
* CNRS/AIST |
7 |
|
|
* |
8 |
|
|
*/ |
9 |
|
|
|
10 |
|
|
#include <dynamic-graph/factory.h> |
11 |
|
|
#include <sot/dynamic-pinocchio/zmpreffromcom.h> |
12 |
|
|
|
13 |
|
|
#include <sot/core/debug.hh> |
14 |
|
|
using namespace dynamicgraph::sot; |
15 |
|
|
using namespace dynamicgraph; |
16 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ZmprefFromCom, "ZmprefFromCom"); |
17 |
|
|
|
18 |
|
|
const double ZmprefFromCom::DT_DEFAULT = 5e-3; |
19 |
|
|
const double ZmprefFromCom::FOOT_HEIGHT_DEFAULT = .105; |
20 |
|
|
|
21 |
|
|
ZmprefFromCom::ZmprefFromCom(const std::string& name) |
22 |
|
|
: Entity(name), |
23 |
|
|
dt(DT_DEFAULT), |
24 |
|
|
footHeight(FOOT_HEIGHT_DEFAULT), |
25 |
|
|
waistPositionSIN( |
26 |
|
|
NULL, "sotZmprefFromCom(" + name + ")::input(MatrixHomo)::waist"), |
27 |
|
|
comPositionSIN(NULL, |
28 |
|
|
"sotZmprefFromCom(" + name + ")::input(Vector)::com"), |
29 |
|
|
dcomSIN(NULL, "sotZmprefFromCom(" + name + ")::input(Vector)::dcom"), |
30 |
|
|
zmprefSOUT(boost::bind(&ZmprefFromCom::computeZmpref, this, _1, _2), |
31 |
|
|
waistPositionSIN << comPositionSIN << dcomSIN, |
32 |
|
|
"sotZmprefFromCom(" + name + ")::output(RPY)::zmpref") { |
33 |
|
|
sotDEBUGIN(5); |
34 |
|
|
|
35 |
|
|
signalRegistration(waistPositionSIN); |
36 |
|
|
signalRegistration(comPositionSIN); |
37 |
|
|
signalRegistration(dcomSIN); |
38 |
|
|
signalRegistration(zmprefSOUT); |
39 |
|
|
|
40 |
|
|
sotDEBUGOUT(5); |
41 |
|
|
} |
42 |
|
|
|
43 |
|
|
ZmprefFromCom::~ZmprefFromCom(void) { |
44 |
|
|
sotDEBUGIN(5); |
45 |
|
|
|
46 |
|
|
sotDEBUGOUT(5); |
47 |
|
|
return; |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
51 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
52 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
53 |
|
|
dynamicgraph::Vector& ZmprefFromCom::computeZmpref(dynamicgraph::Vector& res, |
54 |
|
|
const int& time) { |
55 |
|
|
sotDEBUGIN(15); |
56 |
|
|
|
57 |
|
|
const dynamicgraph::Vector& com = comPositionSIN(time); |
58 |
|
|
const dynamicgraph::Vector& dcom = dcomSIN(time); |
59 |
|
|
const MatrixHomogeneous& oTw = waistPositionSIN(time); |
60 |
|
|
|
61 |
|
|
MatrixHomogeneous wTo = oTw.inverse(); |
62 |
|
|
dynamicgraph::Vector nextComRef = dcom; |
63 |
|
|
nextComRef *= dt; |
64 |
|
|
nextComRef += com; |
65 |
|
|
|
66 |
|
|
nextComRef(2) = -footHeight; // projection on the ground. |
67 |
|
|
res = wTo.matrix() * nextComRef; |
68 |
|
|
|
69 |
|
|
sotDEBUGOUT(15); |
70 |
|
|
return res; |
71 |
|
|
} |