iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium
3  * Author: Lorenzo Natale
4  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5  *
6  */
7 
91 #include <yarp/os/Network.h>
92 #include <yarp/os/RFModule.h>
93 #include <yarp/os/Time.h>
94 #include <yarp/os/BufferedPort.h>
95 #include <yarp/sig/Vector.h>
96 #include <yarp/math/Math.h>
97 
98 #include <yarp/dev/ControlBoardInterfaces.h>
99 #include <yarp/dev/PolyDriver.h>
100 #include <yarp/os/PeriodicThread.h>
101 #include <yarp/os/Thread.h>
102 #include <yarp/os/LogStream.h>
103 
105 
106 #include <mutex>
107 #include <condition_variable>
108 #include <fstream>
109 #include <iostream>
110 #include <iomanip>
111 #include <string>
112 #include <deque>
113 #include <vector>
114 
115 using namespace std;
116 using namespace yarp::os;
117 using namespace yarp::sig;
118 using namespace yarp::dev;
119 using namespace yarp::math;
120 using namespace iCub::ctrl;
121 
122 #define VCTP_TIME yarp::os::createVocab32('t','i','m','e')
123 #define VCTP_OFFSET yarp::os::createVocab32('o','f','f')
124 #define VCTP_CMD_NOW yarp::os::createVocab32('c','t','p','n')
125 #define VCTP_CMD_QUEUE yarp::os::createVocab32('c','t','p','q')
126 #define VCTP_CMD_FILE yarp::os::createVocab32('c','t','p','f')
127 #define VCTP_POSITION yarp::os::createVocab32('p','o','s')
128 #define VCTP_WAIT yarp::os::createVocab32('w','a','i','t')
129 
130 #define VEL_FILT_SIZE 10
131 #define VEL_FILT_THRES 1.0
132 
133 // Class for position control
135 {
136 private:
137  int off;
138  Vector cmd;
139  double time;
140 public:
141  const int &getOffset() const { return off; }
142  int &getOffset() { return off; }
143 
144  const double &getTime() const { return time; }
145  double &getTime() {return time; }
146 
147  const Vector &getCmd() const { return cmd; }
148 
149  Vector &getCmd() { return cmd; }
150 };
151 
152 typedef deque<ActionItem *> ActionList;
153 
154 class Actions
155 {
156  ActionList actions;
157 
158 public:
161 
162  int size()
163  {
164  return actions.size();
165  }
166 
167  void clear()
168  {
169  for(unsigned int k=0; k<actions.size();k++)
170  {
171  ActionItem *ret=actions.at(k);
172  delete ret;
173  }
174  actions.clear();
175  }
176 
178  {
179  if (actions.empty())
180  return 0;
181 
182  ActionItem *ret=actions.at(0);
183  actions.pop_front();
184  return ret;
185  }
186 
188  {
189  actions.push_back(v);
190  }
191 };
192 
194 {
195 public:
196  bool autochange_control_mode_enable = false;
197 protected:
198  bool verbose;
199  bool connected;
200  Property drvOptions;
201  PolyDriver *drv;
202  IControlMode *mode;
203  IPositionControl *pos;
204  IEncoders *enc;
206  mutex mtx;
207 
208  void _send(const ActionItem *x)
209  {
210  if (!connected)
211  {
212  cerr<<"Error: not connected to control board skipping"<<endl;
213  return;
214  }
215 
216  int size=x->getCmd().size();
217  int offset=x->getOffset();
218  double time=x->getTime();
219  int nJoints=0;
220 
221  enc->getAxes(&nJoints);
222  if ((offset+size)>nJoints)
223  {
224  cerr<<"Error: detected possible overflow, skipping"<<endl;
225  cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
226  return;
227  }
228 
229  Vector disp(size);
230 
231  if (time==0)
232  {
233  return;
234  }
235 
236  for (size_t i=0; i<disp.length(); i++)
237  {
238  double q;
239 
240 
241  if (!enc->getEncoder(offset+i,&q))
242  {
243  cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
244  return;
245  }
246 
247  disp[i]=x->getCmd()[i]-q;
248 
249  if (disp[i]<0.0)
250  disp[i]=-disp[i];
251  }
252 
253  // don't blend together the two "for"
254  // since we have to enforce the modes on the whole
255  // prior to commanding the joints
256  std::vector<int> joints;
257  std::vector<int> modes;
258  std::vector<double> speeds;
259  std::vector<double> positions;
260 
261  for (size_t i=0; i<disp.length(); i++)
262  {
263  joints.push_back(offset+i);
264  speeds.push_back(disp[i]/time);
265  modes.push_back(VOCAB_CM_POSITION);
266  positions.push_back(x->getCmd()[i]);
267  }
268 
269  if (autochange_control_mode_enable)
270  {
271  mode->setControlModes(disp.length(), joints.data(), modes.data());
272  }
273 
274  yarp::os::Time::delay(0.01); // give time to update control modes value
275  mode->getControlModes(disp.length(), joints.data(), modes.data());
276  for (size_t i=0; i<disp.length(); i++)
277  {
278  if(modes[i] != VOCAB_CM_POSITION)
279  {
280  yError() << "Joint " << i << " not in position mode";
281  }
282  }
283  pos->setRefSpeeds(disp.length(), joints.data(), speeds.data());
284  pos->positionMove(disp.length(), joints.data(), positions.data());
285 
286  cout << "Script port: " << x->getCmd().toString() << endl;
287  }
288 
289 public:
291  {
292  drvOptions.clear();
293  drv=0;
294  verbose=1;
295  connected=false;
296  }
297 
299  {
300  lock_guard<mutex> lck(mtx);
301  _send(tmp);
302  }
303 
304  bool configure(const Property &options)
305  {
306  bool ret=true;
307  if (options.check("device"))
308  drvOptions.put("device", options.find("device").asString());
309  else
310  drvOptions.put("device","remote_controlboard");
311 
312  string name=string(options.find("name").asString());
313 
314  string remote=string("/")+string(options.find("robot").asString());
315  remote+=string("/")+string(options.find("part").asString());
316 
317  string local=name;
318  local+=string("/local/")+string(options.find("part").asString());
319 
320  drvOptions.put("remote",remote);
321  drvOptions.put("local",local);
322 
323  if (verbose)
324  {
325  cout << "Driver options:\n" << drvOptions.toString();
326  }
327 
328  return ret;
329  }
330 
331  void queue(ActionItem *item)
332  {
333  lock_guard<mutex> lck(mtx);
334  actions.push_back(item);
335  }
336 
337  void sendNow(ActionItem *item)
338  {
339  lock_guard<mutex> lck(mtx);
340  actions.clear();
341  _send(item);
342  }
343 
345  {
346  lock_guard<mutex> lck(mtx);
347  ActionItem *ret=actions.pop();
348  return ret;
349  }
350 
351  bool connect()
352  {
353  drv=new PolyDriver(drvOptions);
354 
355  if (drv->isValid())
356  connected=drv->view(mode) &&
357  drv->view(pos) &&
358  drv->view(enc);
359  else
360  connected=false;
361 
362  if (!connected)
363  {
364  delete drv;
365  drv=0;
366  }
367 
368  return connected;
369  }
370 
372  {
373  if (drv)
374  delete drv;
375  }
376 };
377 
378 class WorkingThread: public PeriodicThread
379 {
380 private:
381  scriptPosPort *posPort;
382 public:
383  WorkingThread(int period=100): PeriodicThread((double)period/1000.0)
384  {}
385 
387  {
388  if (p)
389  posPort=p;
390  }
391 
392  bool threadInit()
393  {
394  if (!posPort)
395  return false;
396  return true;
397  }
398 
399  void run()
400  {
401  ActionItem *tmp=posPort->pop();
402  if (tmp)
403  {
404  posPort->send(tmp);
405  double time=tmp->getTime();
406  delete tmp;
407  Time::delay(time);
408  }
409  }
410 };
411 
412 class VelocityThread: public Thread
413 {
414 private:
415  mutex mtx;
416  condition_variable cv;
417 
418  AWLinEstimator linEst;
419  Port *velPort;
420  Port *velInitPort;
421  ResourceFinder *pRF;
422 
423  ifstream fin;
424  char line[1024];
425  bool closing;
426  bool firstRun;
427  bool velInit;
428 
429  unsigned int filtElemsCnt;
430 
431  bool checkInitConnection(Port *p)
432  {
433 
434  if(p->getOutputCount() > 0)
435  {
436  cout << "***** got a connection" << endl;
437  return true;
438  }
439  else
440  {
441  return false;
442  }
443  }
444 
445  bool initVelCtrl(Port *p, ResourceFinder *rf)
446  {
447  int i = 0;
448  bool cont = true;
449  char numberId[64];
450  bool ret = true;
451 
452  while(cont)
453  {
454  string gainStr = "gain";
455  sprintf(numberId, "%d", i);
456  gainStr = gainStr + numberId;
457 
458  Bottle c,r;
459  c.addString("gain");
460  c.addInt32(i);
461  if (rf->check(gainStr))
462  {
463  c.addFloat64(rf->find(gainStr).asFloat64());
464  p->write(c,r);
465  i++;
466  }
467  else
468  cont = false;
469  }
470  if (i==0)
471  {
472  ret = false;
473  cout << " velCtrl: missing 'gain' parameters" << endl;
474  }
475 
476  i = 0;
477  cont = true;
478  while(cont)
479  {
480  string svelStr = "svel";
481  sprintf(numberId, "%d", i);
482  svelStr = svelStr + numberId;
483 
484  Bottle c,r;
485  c.addString("svel");
486  c.addInt32(i);
487  if (rf->check(svelStr))
488  {
489  c.addFloat64(rf->find(svelStr).asFloat64());
490  p->write(c,r);
491  i++;
492  }
493  else
494  cont = false;
495  }
496  if (i==0)
497  {
498  ret = false;
499  cout << " velCtrl: missing 'svel' parameters" << endl;
500  }
501 
502  return ret;
503  }
504 
505  bool readLine(Vector &v, double &time)
506  {
507  fin.getline(&line[0],sizeof(line),'\n');
508  string str(line);
509 
510  if (str.length()!=0)
511  {
512  Bottle b;
513  b.fromString(str);
514 
515  if (b.size()>2)
516  {
517  time=b.get(1).asFloat64();
518  v.resize(b.size()-2);
519 
520  for (size_t i=0; i<v.length(); i++)
521  v[i]=b.get(i+2).asFloat64();
522 
523  return true;
524  }
525  else
526  return false;
527  }
528  else
529  return false;
530  }
531 
532  Vector formCommand(const Vector &p, const double t)
533  {
534  Vector res;
535 
536  for (size_t i=0; i<p.length(); i++)
537  {
538  res.push_back(i);
539  res.push_back(p[i]);
540  }
541 
542  AWPolyElement el;
543  el.data=p;
544  el.time=t;
545 
546  Vector v=linEst.estimate(el);
547 
548  if (++filtElemsCnt>VEL_FILT_SIZE)
549  {
550  for (size_t i=0; i<p.length(); i++)
551  {
552  res.push_back(1000.0+i);
553  res.push_back(v[i]);
554  }
555  }
556 
557  return res;
558  }
559 
560 public:
562  {
563  velInit=false;
564  velPort=NULL;
565  velInitPort=NULL;
566  closing=false;
567  firstRun=true;
568  }
569 
570  void attachRF(ResourceFinder *attachedRF)
571  {
572  pRF = attachedRF;
573  }
574 
575  void attachVelPort(Port *p)
576  {
577  if (p)
578  velPort=p;
579  }
580 
581  void attachVelInitPort(Port *p)
582  {
583  if (p)
584  velInitPort=p;
585  }
586 
587  bool go(const string &fileName)
588  {
589  if (velPort==NULL)
590  return false;
591 
592  if (fin.is_open())
593  fin.close();
594 
595  fin.open(fileName.c_str());
596  if (fin.is_open())
597  {
598  fin.seekg(0,ios_base::beg);
599  // skip the first two lines
600  fin.getline(&line[0],sizeof(line),'\n');
601  fin.getline(&line[0],sizeof(line),'\n');
602  firstRun=true;
603  cout<<"File loaded"<<endl;
604  cv.notify_one();
605  return true;
606  }
607  else
608  {
609  cout<<"Unable to load file: "<<fileName<<endl;
610  return false;
611  }
612  }
613 
614  void onStop()
615  {
616  closing=true;
617  cv.notify_one();
618  }
619 
621  {
622  if (fin.is_open())
623  fin.close();
624  }
625 
626  void run()
627  {
628  Vector p1,p2;
629  double time1,time2,t;
630  bool send=false;
631 
632  while (!isStopping())
633  {
634  if (!velInit)
635  {
636  if (checkInitConnection(velInitPort))
637  {
638  bool b = initVelCtrl(velInitPort, pRF);
639  if (b)
640  {
641  cout << "velocity control initialized. gain, svel parameters loaded" << endl;
642  }
643  else
644  {
645  cout << "*** velocity control missing init parameters! (gain, svel)" << endl;
646  }
647  velInit = true;
648  }
649  else
650  Time::delay(1.0);
651  }
652  else
653  {
654  if (!closing)
655  {
656  unique_lock<mutex> lck(mtx);
657  cv.wait(lck);
658  }
659 
660  if (firstRun)
661  {
662  Bottle c,r;
663  c.addString("run");
664  velInitPort->write(c,r);
665  send=readLine(p1,time1);
666  linEst.reset();
667  t=0.0;
668  filtElemsCnt=0;
669  firstRun=false;
670  }
671 
672  if (send)
673  {
674  Vector cmd=formCommand(p1,t);
675  velPort->write(cmd);
676  send=false;
677  }
678 
679  if (readLine(p2,time2))
680  {
681  double dt=time2-time1;
682  Time::delay(dt);
683  t+=dt;
684  p1=p2;
685  time1=time2;
686  send=true;
687  cv.notify_one();
688  }
689  else
690  {
691  fin.close();
692  Bottle c,r;
693  c.addString("susp");
694  velInitPort->write(c,r);
695  }
696  }
697  }
698  }
699 };
700 
701 class scriptModule: public RFModule
702 {
703 protected:
704  ResourceFinder *rf;
705  Port rpcPort;
706  string name;
707  bool verbose;
709  Port velPort;
713 
714 public:
716  {
717  verbose=true;
718  }
719 
720  bool handlectpm(const Bottle &cmd, Bottle &reply)
721  {
722  ActionItem *action;
723  bool ret=parsePosCmd(cmd, reply, &action);
724  if (ret)
725  {
726  posPort.sendNow(action);
727  reply.addVocab32("ack");
728  }
729 
730  return ret;
731  }
732 
733  bool handle_ctp_queue(const Bottle &cmd, Bottle &reply)
734  {
735  ActionItem *action;
736  bool ret=parsePosCmd(cmd, reply, &action);
737  if (ret)
738  {
739  posPort.queue(action);
740  reply.addVocab32("ack");
741  }
742  return ret;
743  }
744 
745  bool handle_ctp_file(const Bottle &cmd, Bottle &reply)
746  {
747  if (cmd.size()<2)
748  return false;
749 
750  string fileName=rf->findFile(cmd.get(1).asString());
751  bool ret = velThread.go(fileName);
752  if (ret)
753  {
754  reply.addVocab32("ack");
755  }
756  else
757  {
758  reply.addVocab32("nack");
759  reply.addString("Unable to load file");
760  }
761  return ret;
762  }
763 
764  bool handle_wait(const Bottle &cmd, Bottle &reply)
765  {
766  cerr<<"Warning command not implemented yet"<<endl;
767  reply.addVocab32("ack");
768  return true;
769  //ActionItem *action;
770  //bool ret=parseWaitCmd(cmd, reply, &action);
771  //if (ret)
772  // posPort.queue(action);
773  }
774 
775  bool parsePosCmd(const Bottle &cmd, Bottle &reply, ActionItem **action)
776  {
777  const int expectedMsgSize=7;
778  bool parsed=false;
779  if (cmd.size()!=expectedMsgSize)
780  {
781  return false;
782  }
783 
784  if (cmd.get(1).asVocab32()==VCTP_TIME)
785  {
786  double time=cmd.get(2).asFloat64();
787  int offset=0;
788  Bottle *posCmd=0;
789  if (cmd.get(3).asVocab32()==VCTP_OFFSET)
790  {
791  offset=cmd.get(4).asInt32();
792  if (cmd.get(5).asVocab32()==VCTP_POSITION)
793  {
794  posCmd=cmd.get(6).asList();
795  if (!posCmd)
796  return false;
797 
798  //check posCmd!=0
799  parsed=true;
800  if (verbose)
801  {
802  cout<<"Received command:"<<endl;
803  cout<<"Time: " << time << "offset: " << offset <<" Cmd: " << posCmd->toString()<<endl;
804  }
805  *action=new ActionItem;
806  (*action)->getCmd().resize(posCmd->size());
807  for(int k=0;k<posCmd->size();k++)
808  (*action)->getCmd()[k]=posCmd->get(k).asFloat64();
809 
810  (*action)->getOffset()=offset;
811  (*action)->getTime()=time;
812  return true;
813  }
814  }
815  }
816  return false;
817  }
818 
819  virtual bool configure(ResourceFinder &rf)
820  {
821  this->rf=&rf;
822 
823  if (rf.check("name"))
824  name=string("/")+rf.find("name").asString();
825  else
826  name="/ctpservice";
827 
828  rpcPort.open(name+string("/")+rf.find("part").asString()+"/rpc");
829  attach(rpcPort);
830 
831  Property portProp;
832  portProp.put("name", name);
833  portProp.put("robot", rf.find("robot"));
834  portProp.put("part", rf.find("part"));
835 
836  if (!posPort.configure(portProp))
837  {
838  cerr<<"Error configuring position controller, check parameters"<<endl;
839  return false;
840  }
841 
842  if (!posPort.connect())
843  {
844  cerr<<"Error cannot connect to remote ports"<<endl;
845  return false;
846  }
847 
848  cout << "***** connect to rpc port and type \"help\" for commands list" << endl;
849 
850  thread.attachPosPort(&posPort);
851  if (!thread.start())
852  {
853  cerr<<"Thread did not start, queue will not work"<<endl;
854  }
855 
856  if (rf.check("autochange_control_mode_enable"))
857  {
858  //This value should be false by default.
859  //Otherwise it can be dangerous to activate idle joints (or in hw fault).
860  //The robot could eventually break something!
861  posPort.autochange_control_mode_enable=true;
862  }
863  velPort.open(name+string("/")+rf.find("part").asString()+"/vc:o");
864  velInitPort.open(name+string("/")+rf.find("part").asString()+"/vcInit:o");
865  velThread.attachVelPort(&velPort);
866  velThread.attachVelInitPort(&velInitPort);
867  cout << "Using parameters:" << endl << rf.toString() << endl;
868  velThread.attachRF(&rf);
869  cout << "***** starting the thread" << endl;
870  velThread.start();
871 
872  return true;
873  }
874 
875  virtual bool respond(const Bottle &command, Bottle &reply)
876  {
877  bool ret;
878  if (command.size()!=0)
879  {
880  switch (command.get(0).asVocab32())
881  {
882  case yarp::os::createVocab32('h','e','l','p'):
883  {
884  cout << "Available commands:" << endl;
885  cout<<"Queue command:\n";
886  cout<<"[ctpq] [time] seconds [off] j [pos] (list)\n";
887  cout<<"New command, execute now (erase queue):\n";
888  cout<<"[ctpn] [time] seconds [off] j [pos] (list)\n";
889  cout<<"Load sequence from file:\n";
890  cout<<"[ctpf] filename\n";
891  reply.addVocab32("ack");
892  return true;
893  }
894  case VCTP_CMD_NOW:
895  {
896  ret=handlectpm(command, reply);
897  return ret;
898  }
899  case VCTP_CMD_QUEUE:
900  {
901  ret=handle_ctp_queue(command, reply);
902  return ret;
903  }
904  case VCTP_CMD_FILE:
905  {
906  ret=handle_ctp_file(command, reply);
907  return ret;
908  }
909  case VCTP_WAIT:
910  {
911  ret=handle_wait(command, reply);
912  }
913  default:
914  return RFModule::respond(command,reply);
915  }
916  }
917  else
918  {
919  reply.addVocab32("nack");
920  return false;
921  }
922  }
923 
924  virtual bool close()
925  {
926  rpcPort.interrupt();
927  rpcPort.close();
928  thread.stop();
929 
930  velThread.stop();
931  velPort.interrupt();
932  velPort.close();
933 
934  velInitPort.interrupt();
935  velInitPort.close();
936 
937  return true;
938  }
939 
940  virtual double getPeriod() { return 1.0; }
941  virtual bool updateModule() { return true; }
942 };
943 
944 
945 int main(int argc, char *argv[])
946 {
947  ResourceFinder rf;
948  rf.setDefaultContext("ctpService");
949  rf.configure(argc,argv);
950 
951  if (rf.check("help"))
952  {
953  cout << "Options:" << endl << endl;
954  cout << "\t--name moduleName: set new module name" << endl;
955  cout << "\t--robot robotname: robot name" << endl;
956  cout << "\t--part partname: robot part name" << endl;
957  cout << "\t--from fileName: input configuration file" << endl;
958  cout << "\t--context dirName: resource finder context" << endl;
959  cout << "\t--autochange_control_mode_enable: sets the joints to position mode when trying to execute the command (default off)" << endl;
960 
961  return 0;
962  }
963 
964  Network yarp;
965  if (!yarp.checkNetwork())
966  return 1;
967 
968  scriptModule mod;
969  return mod.runModule(rf);
970 }
971 
972 
973 
VEL_FILT_SIZE
#define VEL_FILT_SIZE
Definition: main.cpp:130
VelocityThread::VelocityThread
VelocityThread()
Definition: main.cpp:561
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
scriptModule::posPort
scriptPosPort posPort
Definition: main.cpp:708
scriptPosPort
Definition: main.cpp:193
scriptPosPort::_send
void _send(const ActionItem *x)
Definition: main.cpp:208
scriptModule::handle_wait
bool handle_wait(const Bottle &cmd, Bottle &reply)
Definition: main.cpp:764
scriptPosPort::configure
bool configure(const Property &options)
Definition: main.cpp:304
scriptPosPort::queue
void queue(ActionItem *item)
Definition: main.cpp:331
ActionItem
Definition: main.cpp:134
pos
const dReal * pos
Definition: iCub_Sim.cpp:62
scriptModule::handle_ctp_queue
bool handle_ctp_queue(const Bottle &cmd, Bottle &reply)
Definition: main.cpp:733
scriptPosPort::mtx
mutex mtx
Definition: main.cpp:206
VelocityThread::attachVelPort
void attachVelPort(Port *p)
Definition: main.cpp:575
VCTP_TIME
#define VCTP_TIME
Definition: main.cpp:122
scriptPosPort::drvOptions
Property drvOptions
Definition: main.cpp:200
cmd
cmd
Definition: dataTypes.h:30
ActionList
deque< ActionItem * > ActionList
Definition: main.cpp:152
scriptPosPort::send
void send(ActionItem *tmp)
Definition: main.cpp:298
main
int main(int argc, char *argv[])
Definition: main.cpp:31
ActionItem::getTime
const double & getTime() const
Definition: main.cpp:144
VelocityThread::run
void run()
Definition: main.cpp:626
offset
degrees offset
Definition: sine.m:4
scriptPosPort::~scriptPosPort
~scriptPosPort()
Definition: main.cpp:371
WorkingThread::attachPosPort
void attachPosPort(scriptPosPort *p)
Definition: main.cpp:386
iCub::ctrl::AWPolyEstimator::estimate
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
Definition: adaptWinPolyEstimator.cpp:100
ActionItem::getCmd
const Vector & getCmd() const
Definition: main.cpp:147
scriptPosPort::mode
IControlMode * mode
Definition: main.cpp:202
scriptPosPort::enc
IEncoders * enc
Definition: main.cpp:204
VelocityThread::attachRF
void attachRF(ResourceFinder *attachedRF)
Definition: main.cpp:570
VCTP_WAIT
#define VCTP_WAIT
Definition: main.cpp:128
yarp::dev
Definition: DebugInterfaces.h:52
p
p
Definition: show_eyes_axes.m:23
VCTP_CMD_FILE
#define VCTP_CMD_FILE
Definition: main.cpp:126
scriptPosPort::actions
Actions actions
Definition: main.cpp:205
WorkingThread
Definition: main.cpp:378
ActionItem::getOffset
int & getOffset()
Definition: main.cpp:142
scriptModule::handle_ctp_file
bool handle_ctp_file(const Bottle &cmd, Bottle &reply)
Definition: main.cpp:745
iCub::ctrl::AWPolyElement
Definition: adaptWinPolyEstimator.h:45
scriptModule::parsePosCmd
bool parsePosCmd(const Bottle &cmd, Bottle &reply, ActionItem **action)
Definition: main.cpp:775
scriptModule
Definition: main.cpp:701
scriptModule::scriptModule
scriptModule()
Definition: main.cpp:715
VelocityThread::threadRelease
void threadRelease()
Definition: main.cpp:620
iCub::ctrl::AWPolyElement::time
double time
Definition: adaptWinPolyEstimator.h:49
mode
GLenum mode
Definition: rendering.cpp:48
scriptModule::rf
ResourceFinder * rf
Definition: main.cpp:704
scriptModule::configure
virtual bool configure(ResourceFinder &rf)
Definition: main.cpp:819
VelocityThread::attachVelInitPort
void attachVelInitPort(Port *p)
Definition: main.cpp:581
ActionItem::getOffset
const int & getOffset() const
Definition: main.cpp:141
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
scriptModule::getPeriod
virtual double getPeriod()
Definition: main.cpp:940
iCub::ctrl::AWPolyEstimator::reset
void reset()
Reinitialize the internal state.
Definition: adaptWinPolyEstimator.cpp:193
scriptPosPort::drv
PolyDriver * drv
Definition: main.cpp:201
VEL_FILT_THRES
#define VEL_FILT_THRES
Definition: main.cpp:131
x
x
Definition: compute_ekf_sym.m:21
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
ActionItem::getCmd
Vector & getCmd()
Definition: main.cpp:149
VCTP_POSITION
#define VCTP_POSITION
Definition: main.cpp:127
scriptPosPort::connected
bool connected
Definition: main.cpp:199
Actions::~Actions
~Actions()
Definition: main.cpp:160
scriptModule::respond
virtual bool respond(const Bottle &command, Bottle &reply)
Definition: main.cpp:875
Actions::clear
void clear()
Definition: main.cpp:167
iCub::ctrl::AWPolyElement::data
yarp::sig::Vector data
Definition: adaptWinPolyEstimator.h:48
scriptModule::updateModule
virtual bool updateModule()
Definition: main.cpp:941
VCTP_CMD_NOW
#define VCTP_CMD_NOW
Definition: main.cpp:124
Actions::size
int size()
Definition: main.cpp:162
Actions::push_back
void push_back(ActionItem *v)
Definition: main.cpp:187
VelocityThread::onStop
void onStop()
Definition: main.cpp:614
scriptPosPort::pop
ActionItem * pop()
Definition: main.cpp:344
scriptModule::verbose
bool verbose
Definition: main.cpp:707
VelocityThread
Definition: main.cpp:412
v
static int v
Definition: iCub_Sim.cpp:42
adaptWinPolyEstimator.h
VCTP_OFFSET
#define VCTP_OFFSET
Definition: main.cpp:123
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
scriptModule::velInitPort
Port velInitPort
Definition: main.cpp:710
scripting.argc
argc
Definition: scripting.py:184
scriptModule::thread
WorkingThread thread
Definition: main.cpp:711
scriptPosPort::sendNow
void sendNow(ActionItem *item)
Definition: main.cpp:337
scriptModule::rpcPort
Port rpcPort
Definition: main.cpp:705
scriptModule::name
string name
Definition: main.cpp:706
ActionItem::getTime
double & getTime()
Definition: main.cpp:145
scriptModule::handlectpm
bool handlectpm(const Bottle &cmd, Bottle &reply)
Definition: main.cpp:720
VCTP_CMD_QUEUE
#define VCTP_CMD_QUEUE
Definition: main.cpp:125
scriptModule::velThread
VelocityThread velThread
Definition: main.cpp:712
scriptModule::close
virtual bool close()
Definition: main.cpp:924
VelocityThread::go
bool go(const string &fileName)
Definition: main.cpp:587
scriptPosPort::pos
IPositionControl * pos
Definition: main.cpp:203
time
degrees time
Definition: sine.m:5
scriptPosPort::scriptPosPort
scriptPosPort()
Definition: main.cpp:290
scriptPosPort::autochange_control_mode_enable
bool autochange_control_mode_enable
Definition: main.cpp:196
Actions::Actions
Actions()
Definition: main.cpp:159
scriptPosPort::verbose
bool verbose
Definition: main.cpp:198
Actions::pop
ActionItem * pop()
Definition: main.cpp:177
WorkingThread::WorkingThread
WorkingThread(int period=100)
Definition: main.cpp:383
scriptPosPort::connect
bool connect()
Definition: main.cpp:351
Actions
Definition: main.cpp:154
WorkingThread::threadInit
bool threadInit()
Definition: main.cpp:392
scriptModule::velPort
Port velPort
Definition: main.cpp:709
WorkingThread::run
void run()
Definition: main.cpp:399