1#include "iCubBreatherThread.h"
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)
14 ResourceFinder &rf =
const_cast<ResourceFinder&
>(_rf);
15 groupPartBottle = rf.findGroup(part.c_str());
16 rate=1000.0*getPeriod();
19bool iCubBreatherThread::threadInit()
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());
31 yError(
" could not open %s PolyDriver!",part.c_str());
37 ok = ok && dd.view(ipos);
38 ok = ok && dd.view(iencs);
39 ok = ok && dd.view(imod);
44 yError(
" Problems acquiring %s interfaces!!!!",part.c_str());
48 iencs -> getAxes(&jnts);
49 encs_0.resize(jnts,0.0);
52 if (!groupPartBottle.isNull())
54 Bottle *bns = groupPartBottle.find(
"noiseStds").asList();
55 Bottle *brf = groupPartBottle.find(
"refSpeeds").asList();
57 for (
int i = 0; i < jnts; i++)
59 noiseStDvtns.push_back(bns->get(i).asFloat64());
60 refSpeeds.push_back(brf->get(i).asFloat64());
65 for (
int i = 0; i < jnts; i++)
67 noiseStDvtns.push_back(noiseStd);
68 refSpeeds.push_back(refSpeed);
74 printMessage(1,
"Noise std deviations: ");
75 for (
size_t i = 0; i < noiseStDvtns.size(); i++)
77 printf(
"%g\t",noiseStDvtns[i]);
81 printMessage(1,
"Ref speeds : ");
82 for (
size_t i = 0; i < refSpeeds.size(); i++)
84 printf(
"%g\t",refSpeeds[i]);
93 for (
int i = 0; i < jnts; i++)
95 tmp[i] = refSpeeds[i];
97 ipos->setRefSpeeds(tmp.data());
99 double t0 = yarp::os::Time::now();
100 double seed = 1000.0 * (t0 -(long)t0);
106void iCubBreatherThread::run()
113 if (!iencs->getEncoders(encs_0.data()))
115 yError(
" Error reading encoders, check connectivity with the robot!");
119 yInfo(
"Home position %s",encs_0.toString(3,3).c_str());
124 lock_guard<mutex> lg(mtx);
127 Vector newTarget = computeNewTarget();
128 yInfo(
"New target: %s",newTarget.toString(3,3).c_str());
129 goToTarget(newTarget);
133 double nextRate=rate+randngen.get(0,500.0);
134 setPeriod((
double)nextRate/1000.0);
137Vector iCubBreatherThread::computeNewTarget()
140 Vector noise(jnts,0.0);
141 for (
int i = 0; i < jnts; i++)
143 noise[i] = randngen.get(0,noiseStDvtns[i]);
146 Vector result(jnts,0.0);
148 for (
int i = 0; i < jnts; i++)
150 result[i] = encs_0[i] + noise[i];
156bool iCubBreatherThread::goToTarget(
const Vector &nT)
158 vector<int> jointsToSet;
159 if (!areJointsHealthyAndSet(jointsToSet,
"position"))
162 yError(
" joints are not healthy!");
167 if (setCtrlModes(
"position"))
169 return ipos -> positionMove(nT.data());
173 yError(
" setCtrlModes returned false!");
180bool iCubBreatherThread::areJointsHealthyAndSet(vector<int> &jointsToSet,
const string &_s)
182 vector<int> modes(jnts);
183 imod->getControlModes(modes.data());
185 for (
size_t i=0; i<modes.size(); i++)
187 if ((modes[i]==VOCAB_CM_HW_FAULT) || (modes[i]==VOCAB_CM_IDLE))
192 if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_VELOCITY)
193 jointsToSet.push_back(i);
195 else if (_s==
"position")
197 if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_POSITION)
198 jointsToSet.push_back(i);
206bool iCubBreatherThread::setCtrlModes(
const string &_s)
208 yDebug(
"Setting %s mode for %s joints..",_s.c_str(),part.c_str());
210 if (_s!=
"position" && _s!=
"velocity")
217 for (
int i = 0; i < jnts; i++)
219 modes.push_back(VOCAB_CM_POSITION);
222 else if (_s==
"velocity")
224 for (
int i = 0; i < jnts; i++)
226 modes.push_back(VOCAB_CM_VELOCITY);
230 imod -> setControlModes(modes.data());
237bool iCubBreatherThread::goHome()
239 return goToTarget(encs_0);
243bool iCubBreatherThread::startBreathing()
245 lock_guard<mutex> lg(mtx);
251bool iCubBreatherThread::stopBreathing()
253 lock_guard<mutex> lg(mtx);
258int iCubBreatherThread::printMessage(
const int l,
const char *f, ...)
const
262 fprintf(stdout,
"*** %s: ",name.c_str());
266 int ret=vfprintf(stdout,f,ap);
275void iCubBreatherThread::threadRelease()
277 yInfo(
"Putting %s in home position..",part.c_str());
280 yInfo(
"Closing controllers..");