1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* François Bleibel, |
4 |
|
|
* Olivier Stasse, |
5 |
|
|
* |
6 |
|
|
* CNRS/AIST |
7 |
|
|
* |
8 |
|
|
*/ |
9 |
|
|
|
10 |
|
|
#define ENABLE_RT_LOG |
11 |
|
|
|
12 |
|
|
#include <dynamic-graph/all-commands.h> |
13 |
|
|
#include <dynamic-graph/real-time-logger.h> |
14 |
|
|
|
15 |
|
|
#include <sot/core/debug.hh> |
16 |
|
|
#include <sot/core/factory.hh> |
17 |
|
|
#include <sot/core/gripper-control.hh> |
18 |
|
|
#include <sot/core/macros-signal.hh> |
19 |
|
|
|
20 |
|
|
using namespace dynamicgraph::sot; |
21 |
|
|
using namespace dynamicgraph; |
22 |
|
|
|
23 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GripperControlPlugin, "GripperControl"); |
24 |
|
|
|
25 |
|
|
/* --- PLUGIN --------------------------------------------------------------- */ |
26 |
|
|
/* --- PLUGIN --------------------------------------------------------------- */ |
27 |
|
|
/* --- PLUGIN --------------------------------------------------------------- */ |
28 |
|
|
|
29 |
|
|
#define SOT_FULL_TO_REDUCED(sotName) \ |
30 |
|
|
sotName##FullSizeSIN(NULL, "GripperControl(" + name + \ |
31 |
|
|
")::input(vector)::" + #sotName + "FullIN"), \ |
32 |
|
|
sotName##ReduceSOUT( \ |
33 |
|
|
SOT_INIT_SIGNAL_2(GripperControlPlugin::selector, \ |
34 |
|
|
sotName##FullSizeSIN, dynamicgraph::Vector, \ |
35 |
|
|
selectionSIN, Flags), \ |
36 |
|
|
"GripperControl(" + name + ")::input(vector)::" + #sotName + \ |
37 |
|
|
"ReducedOUT") |
38 |
|
|
|
39 |
|
|
const double GripperControl::OFFSET_DEFAULT = 0.9; |
40 |
|
|
|
41 |
|
|
// TODO: hard coded |
42 |
|
|
const double DT = 0.005; |
43 |
|
|
|
44 |
|
|
GripperControl::GripperControl(void) |
45 |
|
|
: offset(GripperControl::OFFSET_DEFAULT), factor() {} |
46 |
|
|
|
47 |
|
|
GripperControlPlugin::GripperControlPlugin(const std::string &name) |
48 |
|
|
: Entity(name), |
49 |
|
|
calibrationStarted(false), |
50 |
|
|
positionSIN(NULL, |
51 |
|
|
"GripperControl(" + name + ")::input(vector)::position"), |
52 |
|
|
positionDesSIN( |
53 |
|
|
NULL, "GripperControl(" + name + ")::input(vector)::positionDes"), |
54 |
|
|
torqueSIN(NULL, "GripperControl(" + name + ")::input(vector)::torque"), |
55 |
|
|
torqueLimitSIN( |
56 |
|
|
NULL, "GripperControl(" + name + ")::input(vector)::torqueLimit"), |
57 |
|
|
selectionSIN(NULL, "GripperControl(" + name + ")::input(vector)::selec") |
58 |
|
|
|
59 |
|
|
, |
60 |
|
|
SOT_FULL_TO_REDUCED(position), |
61 |
|
|
SOT_FULL_TO_REDUCED(torque), |
62 |
|
|
SOT_FULL_TO_REDUCED(torqueLimit), |
63 |
|
|
desiredPositionSOUT( |
64 |
|
|
SOT_MEMBER_SIGNAL_4(GripperControl::computeDesiredPosition, |
65 |
|
|
positionSIN, dynamicgraph::Vector, positionDesSIN, |
66 |
|
|
dynamicgraph::Vector, torqueSIN, |
67 |
|
|
dynamicgraph::Vector, torqueLimitSIN, |
68 |
|
|
dynamicgraph::Vector), |
69 |
|
|
"GripperControl(" + name + ")::output(vector)::reference") { |
70 |
|
|
sotDEBUGIN(5); |
71 |
|
|
|
72 |
|
|
positionSIN.plug(&positionReduceSOUT); |
73 |
|
|
torqueSIN.plug(&torqueReduceSOUT); |
74 |
|
|
torqueLimitSIN.plug(&torqueLimitReduceSOUT); |
75 |
|
|
|
76 |
|
|
signalRegistration( |
77 |
|
|
positionSIN << positionDesSIN << torqueSIN << torqueLimitSIN |
78 |
|
|
<< selectionSIN << desiredPositionSOUT << positionFullSizeSIN |
79 |
|
|
<< torqueFullSizeSIN << torqueLimitFullSizeSIN); |
80 |
|
|
sotDEBUGOUT(5); |
81 |
|
|
|
82 |
|
|
initCommands(); |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
GripperControlPlugin::~GripperControlPlugin(void) {} |
86 |
|
|
|
87 |
|
|
std::string GripperControlPlugin::getDocString() const { |
88 |
|
|
std::string docstring = "Control of gripper."; |
89 |
|
|
return docstring; |
90 |
|
|
} |
91 |
|
|
|
92 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
93 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
94 |
|
|
/* --- SIGNALS -------------------------------------------------------------- */ |
95 |
|
|
|
96 |
|
|
void GripperControl::computeIncrement( |
97 |
|
|
const dynamicgraph::Vector &torques, |
98 |
|
|
const dynamicgraph::Vector &torqueLimits, |
99 |
|
|
const dynamicgraph::Vector ¤tNormVel) { |
100 |
|
|
const dynamicgraph::Vector::Index SIZE = currentNormVel.size(); |
101 |
|
|
|
102 |
|
|
// initialize factor, if needed. |
103 |
|
|
if (factor.size() != SIZE) { |
104 |
|
|
factor.resize(SIZE); |
105 |
|
|
factor.fill(1.); |
106 |
|
|
} |
107 |
|
|
|
108 |
|
|
// Torque not provided? |
109 |
|
|
if (torques.size() == 0) { |
110 |
|
|
dgRTLOG() << "torque is not provided " << std::endl; |
111 |
|
|
return; |
112 |
|
|
} |
113 |
|
|
|
114 |
|
|
for (int i = 0; i < SIZE; ++i) { |
115 |
|
|
// apply a reduction factor if the torque limits are exceeded |
116 |
|
|
// and the velocity goes in the same way |
117 |
|
|
if ((torques(i) > torqueLimits(i)) && (currentNormVel(i) > 0)) { |
118 |
|
|
factor(i) *= offset; |
119 |
|
|
} else if ((torques(i) < -torqueLimits(i)) && (currentNormVel(i) < 0)) { |
120 |
|
|
factor(i) *= offset; |
121 |
|
|
} |
122 |
|
|
// otherwise, release smoothly the reduction if possible/needed |
123 |
|
|
else { |
124 |
|
|
factor(i) /= offset; |
125 |
|
|
} |
126 |
|
|
|
127 |
|
|
// ensure factor is in )0,1( |
128 |
|
|
factor(i) = std::min(1., std::max(factor(i), 0.)); |
129 |
|
|
} |
130 |
|
|
} |
131 |
|
|
|
132 |
|
|
dynamicgraph::Vector &GripperControl::computeDesiredPosition( |
133 |
|
|
const dynamicgraph::Vector ¤tPos, |
134 |
|
|
const dynamicgraph::Vector &desiredPos, const dynamicgraph::Vector &torques, |
135 |
|
|
const dynamicgraph::Vector &torqueLimits, |
136 |
|
|
dynamicgraph::Vector &referencePos) { |
137 |
|
|
const dynamicgraph::Vector::Index SIZE = currentPos.size(); |
138 |
|
|
// if( (SIZE==torques.size()) ) |
139 |
|
|
// { /* ERROR ... */ } |
140 |
|
|
|
141 |
|
|
// compute the desired velocity |
142 |
|
|
dynamicgraph::Vector velocity = (desiredPos - currentPos) * (1. / DT); |
143 |
|
|
|
144 |
|
|
computeIncrement(torques, torqueLimits, velocity); |
145 |
|
|
|
146 |
|
|
sotDEBUG(25) << " velocity " << velocity << std::endl; |
147 |
|
|
sotDEBUG(25) << " factor " << factor << std::endl; |
148 |
|
|
|
149 |
|
|
// multiply the velocity elmt per elmt |
150 |
|
|
dynamicgraph::Vector weightedVel(SIZE); |
151 |
|
|
weightedVel = velocity * factor; |
152 |
|
|
sotDEBUG(25) << " weightedVel " << weightedVel << std::endl; |
153 |
|
|
|
154 |
|
|
// integrate the desired velocity |
155 |
|
|
referencePos.resize(SIZE); |
156 |
|
|
referencePos = currentPos + weightedVel * DT; |
157 |
|
|
return referencePos; |
158 |
|
|
} |
159 |
|
|
|
160 |
|
|
dynamicgraph::Vector &GripperControl::selector( |
161 |
|
|
const dynamicgraph::Vector &fullsize, const Flags &selec, |
162 |
|
|
dynamicgraph::Vector &desPos) { |
163 |
|
|
int size = 0; |
164 |
|
|
for (int i = 0; i < fullsize.size(); ++i) { |
165 |
|
|
if (selec(i)) size++; |
166 |
|
|
} |
167 |
|
|
|
168 |
|
|
int curs = 0; |
169 |
|
|
desPos.resize(size); |
170 |
|
|
for (int i = 0; i < fullsize.size(); ++i) { |
171 |
|
|
if (selec(i)) desPos(curs++) = fullsize(i); |
172 |
|
|
} |
173 |
|
|
|
174 |
|
|
return desPos; |
175 |
|
|
} |
176 |
|
|
|
177 |
|
|
/* --- COMMANDLINE ---------------------------------------------------------- */ |
178 |
|
|
/* --- COMMANDLINE ---------------------------------------------------------- */ |
179 |
|
|
/* --- COMMANDLINE ---------------------------------------------------------- */ |
180 |
|
|
void GripperControlPlugin::initCommands() { |
181 |
|
|
namespace dc = ::dynamicgraph::command; |
182 |
|
|
addCommand("offset", |
183 |
|
|
dc::makeCommandVoid1(*this, &GripperControlPlugin::setOffset, |
184 |
|
|
"set the offset (should be in )0, 1( ).")); |
185 |
|
|
} |
186 |
|
|
|
187 |
|
|
void GripperControlPlugin::setOffset(const double &value) { |
188 |
|
|
if ((value > 0) && (value < 1)) |
189 |
|
|
offset = value; |
190 |
|
|
else |
191 |
|
|
throw std::invalid_argument("The offset should be in )0, 1(."); |
192 |
|
|
} |