1 |
|
|
#include <dynamic-graph/command-bind.h> |
2 |
|
|
#include <dynamic-graph/command-direct-getter.h> |
3 |
|
|
#include <dynamic-graph/command-direct-setter.h> |
4 |
|
|
#include <dynamic-graph/factory.h> |
5 |
|
|
#include <holonomic-projection.h> |
6 |
|
|
|
7 |
|
|
#include <sot/core/matrix-geometry.hh> |
8 |
|
|
|
9 |
|
|
namespace dynamicgraph { |
10 |
|
|
|
11 |
|
|
HolonomicProjection::HolonomicProjection(const std::string& name) |
12 |
|
|
: Entity(name), |
13 |
|
|
nv_(-1), |
14 |
|
|
basePoseSIN(NULL, "HolonomicProjection(" + name + |
15 |
|
|
")::input(MatrixHomogeneous)::basePose") |
16 |
|
|
|
17 |
|
|
, |
18 |
|
|
leftWheelIdx_(-1), |
19 |
|
|
rightWheelIdx_(-1), |
20 |
|
|
wheelSeparation_(0.0), |
21 |
|
|
wheelRadius_(0.0), |
22 |
|
|
wheelSeparationMultiplierSIN( |
23 |
|
|
NULL, "HolonomicProjection(" + name + |
24 |
|
|
")::input(double)::wheelSeparationMultiplierSIN"), |
25 |
|
|
leftWheelRadiusMultiplierSIN( |
26 |
|
|
NULL, "HolonomicProjection(" + name + |
27 |
|
|
")::input(double)::leftWheelRadiusMultiplierSIN"), |
28 |
|
|
rightWheelRadiusMultiplierSIN( |
29 |
|
|
NULL, "HolonomicProjection(" + name + |
30 |
|
|
")::input(double)::rightWheelRadiusMultiplierSIN") |
31 |
|
|
|
32 |
|
|
, |
33 |
|
|
projectionSOUT( |
34 |
|
|
boost::bind(&HolonomicProjection::computeProjection, this, _1, _2), |
35 |
|
|
wheelSeparationMultiplierSIN << leftWheelRadiusMultiplierSIN |
36 |
|
|
<< rightWheelRadiusMultiplierSIN |
37 |
|
|
<< basePoseSIN, |
38 |
|
|
"HolonomicProjection(" + name + ")::output(Matrix)::projection") { |
39 |
|
|
signalRegistration(basePoseSIN << projectionSOUT |
40 |
|
|
<< wheelSeparationMultiplierSIN |
41 |
|
|
<< leftWheelRadiusMultiplierSIN |
42 |
|
|
<< rightWheelRadiusMultiplierSIN); |
43 |
|
|
|
44 |
|
|
wheelSeparationMultiplierSIN.setConstant(1); |
45 |
|
|
leftWheelRadiusMultiplierSIN.setConstant(1); |
46 |
|
|
rightWheelRadiusMultiplierSIN.setConstant(1); |
47 |
|
|
|
48 |
|
|
// Add commands |
49 |
|
|
addCommand("setSize", command::makeDirectSetter( |
50 |
|
|
*this, &nv_, "Number of DoF of the robot")); |
51 |
|
|
addCommand("getSize", command::makeDirectGetter( |
52 |
|
|
*this, &nv_, "Number of DoF of the robot")); |
53 |
|
|
|
54 |
|
|
addCommand("setLeftWheel", command::makeDirectSetter(*this, &leftWheelIdx_, |
55 |
|
|
"Left wheel index.")); |
56 |
|
|
addCommand("getLeftWheel", command::makeDirectGetter(*this, &leftWheelIdx_, |
57 |
|
|
"Left wheel index.")); |
58 |
|
|
|
59 |
|
|
addCommand("setRightWheel", command::makeDirectSetter(*this, &rightWheelIdx_, |
60 |
|
|
"Right wheel index.")); |
61 |
|
|
addCommand("getRightWheel", command::makeDirectGetter(*this, &rightWheelIdx_, |
62 |
|
|
"Right wheel index.")); |
63 |
|
|
|
64 |
|
|
addCommand("setWheelSeparation", |
65 |
|
|
command::makeDirectSetter( |
66 |
|
|
*this, &wheelSeparation_, |
67 |
|
|
"Wheel separation, wrt the midpoint of the wheel width")); |
68 |
|
|
addCommand("getWheelSeparation", |
69 |
|
|
command::makeDirectGetter( |
70 |
|
|
*this, &wheelSeparation_, |
71 |
|
|
"Wheel separation, wrt the midpoint of the wheel width")); |
72 |
|
|
|
73 |
|
|
addCommand("setWheelRadius", |
74 |
|
|
command::makeDirectSetter(*this, &wheelRadius_, |
75 |
|
|
"Wheel radius (assuming it's the same " |
76 |
|
|
"for the left and right wheels")); |
77 |
|
|
addCommand("getWheelRadius", |
78 |
|
|
command::makeDirectGetter(*this, &wheelRadius_, |
79 |
|
|
"Wheel radius (assuming it's the same " |
80 |
|
|
"for the left and right wheels")); |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
Matrix& HolonomicProjection::computeProjection(Matrix& proj, const int& time) { |
84 |
|
|
if (nv_ < 6) { |
85 |
|
|
SEND_ERROR_STREAM_MSG("Size must be superior to 6."); |
86 |
|
|
proj.resize(0, 0); |
87 |
|
|
return proj; |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
const sot::MatrixHomogeneous& oMb = basePoseSIN.access(time); |
91 |
|
|
sot::MatrixTwist twist; |
92 |
|
|
sot::buildFrom(oMb, twist); |
93 |
|
|
|
94 |
|
|
bool hasWheels = (leftWheelIdx_ >= 0 && rightWheelIdx_ >= 0); |
95 |
|
|
|
96 |
|
|
const int nr = nv_, nc = (hasWheels ? nv_ - 6 : nv_ - 4); |
97 |
|
|
proj = Matrix::Zero(nr, nc); |
98 |
|
|
|
99 |
|
|
// 6 first rows |
100 |
|
|
proj.topLeftCorner<6, 2>() << twist.col(0), twist.col(5); |
101 |
|
|
|
102 |
|
|
// Other rows |
103 |
|
|
if (hasWheels) { |
104 |
|
|
const double ws = wheelSeparationMultiplierSIN(time) * wheelSeparation_; |
105 |
|
|
const double lwr = leftWheelRadiusMultiplierSIN(time) * wheelRadius_; |
106 |
|
|
const double rwr = rightWheelRadiusMultiplierSIN(time) * wheelRadius_; |
107 |
|
|
proj(leftWheelIdx_, 0) = 1. / lwr; |
108 |
|
|
proj(leftWheelIdx_, 1) = -ws / (2.0 * lwr); // Left |
109 |
|
|
proj(rightWheelIdx_, 0) = 1. / rwr; |
110 |
|
|
proj(rightWheelIdx_, 1) = ws / (2.0 * rwr); // Right |
111 |
|
|
int c = 2; |
112 |
|
|
for (int r = 6; r < nv_; ++r) { |
113 |
|
|
if (r == leftWheelIdx_ || r == rightWheelIdx_) continue; |
114 |
|
|
proj(r, c) = 1.; |
115 |
|
|
++c; |
116 |
|
|
} |
117 |
|
|
assert(c == nc); |
118 |
|
|
} else { |
119 |
|
|
proj.bottomRightCorner(nv_ - 6, nv_ - 6).setIdentity(); |
120 |
|
|
} |
121 |
|
|
|
122 |
|
|
return proj; |
123 |
|
|
} |
124 |
|
|
|
125 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(HolonomicProjection, "HolonomicProjection"); |
126 |
|
|
} // namespace dynamicgraph |