funny-things
All Modules
iCubBreatherThread.cpp
1#include "iCubBreatherThread.h"
2
3iCubBreatherThread::iCubBreatherThread(int _rate, string _name, string _robot, string _part, bool _autoStart,
4 double _noiseStd, double _refSpeed, int _v, const ResourceFinder &_rf) :
5 PeriodicThread((double)_rate/1000.0), name(_name), robot(_robot),
6 verbosity(_v), part(_part), noiseStd(_noiseStd), refSpeed(_refSpeed)
7{
8 if (_autoStart)
9 isRunning = true;
10 else
11 isRunning = false;
12 onStart = true;
13
14 ResourceFinder &rf = const_cast<ResourceFinder&>(_rf);
15 groupPartBottle = rf.findGroup(part.c_str());
16 rate=1000.0*getPeriod();
17}
18
19bool iCubBreatherThread::threadInit()
20{
21 bool ok = 1;
22 Property Opt;
23 Opt.put("robot", robot.c_str());
24 Opt.put("part", robot.c_str());
25 Opt.put("device", "remote_controlboard");
26 Opt.put("remote",("/"+robot+"/"+part).c_str());
27 Opt.put("local", ("/"+name +"/"+part).c_str());
28
29 if (!dd.open(Opt))
30 {
31 yError(" could not open %s PolyDriver!",part.c_str());
32 return false;
33 }
34
35 if (dd.isValid())
36 {
37 ok = ok && dd.view(ipos);
38 ok = ok && dd.view(iencs);
39 ok = ok && dd.view(imod);
40 }
41
42 if (!ok)
43 {
44 yError(" Problems acquiring %s interfaces!!!!",part.c_str());
45 return false;
46 }
47
48 iencs -> getAxes(&jnts);
49 encs_0.resize(jnts,0.0);
50
51 // Find the standard deviations and the ref speeds
52 if (!groupPartBottle.isNull())
53 {
54 Bottle *bns = groupPartBottle.find("noiseStds").asList();
55 Bottle *brf = groupPartBottle.find("refSpeeds").asList();
56
57 for (int i = 0; i < jnts; i++)
58 {
59 noiseStDvtns.push_back(bns->get(i).asFloat64());
60 refSpeeds.push_back(brf->get(i).asFloat64());
61 }
62 }
63 else
64 {
65 for (int i = 0; i < jnts; i++)
66 {
67 noiseStDvtns.push_back(noiseStd);
68 refSpeeds.push_back(refSpeed);
69 }
70 }
71
72 if (verbosity >= 1)
73 {
74 printMessage(1,"Noise std deviations: ");
75 for (size_t i = 0; i < noiseStDvtns.size(); i++)
76 {
77 printf("%g\t",noiseStDvtns[i]);
78 }
79 printf("\n");
80
81 printMessage(1,"Ref speeds : ");
82 for (size_t i = 0; i < refSpeeds.size(); i++)
83 {
84 printf("%g\t",refSpeeds[i]);
85 }
86 printf("\n");
87 }
88
89
90
91 // Set the ref speeds
92 Vector tmp(jnts,0.0);
93 for (int i = 0; i < jnts; i++)
94 {
95 tmp[i] = refSpeeds[i];
96 }
97 ipos->setRefSpeeds(tmp.data());
98
99 double t0 = yarp::os::Time::now();
100 double seed = 1000.0 * (t0 -(long)t0);
101 randngen.init();
102
103 return true;
104}
105
106void iCubBreatherThread::run()
107{
108 if (onStart)
109 {
110 // This is there only for avoiding some printing issues. Remove it if you want
111 Time::delay(0.1);
112
113 if (!iencs->getEncoders(encs_0.data()))
114 {
115 yError(" Error reading encoders, check connectivity with the robot!");
116 }
117 else
118 {
119 yInfo("Home position %s",encs_0.toString(3,3).c_str());
120 onStart = false;
121 }
122 }
123
124 lock_guard<mutex> lg(mtx);
125 if (isRunning)
126 {
127 Vector newTarget = computeNewTarget();
128 yInfo("New target: %s",newTarget.toString(3,3).c_str());
129 goToTarget(newTarget);
130 }
131
132 // modulate period
133 double nextRate=rate+randngen.get(0,500.0);
134 setPeriod((double)nextRate/1000.0);
135}
136
137Vector iCubBreatherThread::computeNewTarget()
138{
139 // Let's put the simplest stuff and make it complex afterwards
140 Vector noise(jnts,0.0);
141 for (int i = 0; i < jnts; i++)
142 {
143 noise[i] = randngen.get(0,noiseStDvtns[i]);
144 }
145
146 Vector result(jnts,0.0);
147
148 for (int i = 0; i < jnts; i++)
149 {
150 result[i] = encs_0[i] + noise[i];
151 }
152
153 return result;
154}
155
156bool iCubBreatherThread::goToTarget(const Vector &nT)
157{
158 vector<int> jointsToSet;
159 if (!areJointsHealthyAndSet(jointsToSet,"position"))
160 {
161 stopBreathing();
162 yError(" joints are not healthy!");
163 return false;
164 }
165 else
166 {
167 if (setCtrlModes("position"))
168 {
169 return ipos -> positionMove(nT.data());
170 }
171 else
172 {
173 yError(" setCtrlModes returned false!");
174 return false;
175 }
176 }
177 return true;
178}
179
180bool iCubBreatherThread::areJointsHealthyAndSet(vector<int> &jointsToSet,const string &_s)
181{
182 vector<int> modes(jnts);
183 imod->getControlModes(modes.data());
184
185 for (size_t i=0; i<modes.size(); i++)
186 {
187 if ((modes[i]==VOCAB_CM_HW_FAULT) || (modes[i]==VOCAB_CM_IDLE))
188 return false;
189
190 if (_s=="velocity")
191 {
192 if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_VELOCITY)
193 jointsToSet.push_back(i);
194 }
195 else if (_s=="position")
196 {
197 if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_POSITION)
198 jointsToSet.push_back(i);
199 }
200
201 }
202
203 return true;
204}
205
206bool iCubBreatherThread::setCtrlModes(const string &_s)
207{
208 yDebug("Setting %s mode for %s joints..",_s.c_str(),part.c_str());
209
210 if (_s!="position" && _s!="velocity")
211 return false;
212
213 vector<int> modes;
214
215 if (_s=="position")
216 {
217 for (int i = 0; i < jnts; i++)
218 {
219 modes.push_back(VOCAB_CM_POSITION);
220 }
221 }
222 else if (_s=="velocity")
223 {
224 for (int i = 0; i < jnts; i++)
225 {
226 modes.push_back(VOCAB_CM_VELOCITY);
227 }
228 }
229
230 imod -> setControlModes(modes.data());
231
232 Time::delay(0.1);
233
234 return true;
235}
236
237bool iCubBreatherThread::goHome()
238{
239 return goToTarget(encs_0);
240}
241
242
243bool iCubBreatherThread::startBreathing()
244{
245 lock_guard<mutex> lg(mtx);
246 onStart = true;
247 isRunning = true;
248 return true;
249}
250
251bool iCubBreatherThread::stopBreathing()
252{
253 lock_guard<mutex> lg(mtx);
254 isRunning = false;
255 return true;
256}
257
258int iCubBreatherThread::printMessage(const int l, const char *f, ...) const
259{
260 if (verbosity>=l)
261 {
262 fprintf(stdout,"*** %s: ",name.c_str());
263
264 va_list ap;
265 va_start(ap,f);
266 int ret=vfprintf(stdout,f,ap);
267 va_end(ap);
268
269 return ret;
270 }
271 else
272 return -1;
273}
274
275void iCubBreatherThread::threadRelease()
276{
277 yInfo("Putting %s in home position..",part.c_str());
278 goHome();
279
280 yInfo("Closing controllers..");
281 dd.close();
282}
283
284// empty line to make gcc happy