iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C)2013 iCub Facility - Istituto Italiano di Tecnologia
3  * Author: Marco Randazzo
4  * email: marco.randazzo@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #include <yarp/os/Network.h>
20 #include <yarp/os/RFModule.h>
21 #include <yarp/os/Time.h>
22 #include <yarp/os/Log.h>
23 #include <yarp/os/LogStream.h>
24 #include <yarp/os/BufferedPort.h>
25 #include <yarp/sig/Vector.h>
26 #include <yarp/math/Math.h>
27 
28 #include <fstream>
29 #include <iostream>
30 #include <iomanip>
31 #include <string>
32 #include <vector>
33 #include <deque>
34 #include <cmath>
35 #include <mutex>
36 
37 #include "utils.h"
38 
39 // ******************** THE THREAD
40 class BroadcastingThread: public PeriodicThread
41 {
42 private:
43  action_class *actions;
44  robotDriver *driver;
45  BufferedPort<Bottle> port_data_out;
46 
47 public:
48  BroadcastingThread(int period=1): PeriodicThread((double)period/1000.0)
49  {
50  port_data_out.open("/trajectoryPlayer/all_joints_data_out:o");
51  }
52 
54  {
55  port_data_out.interrupt();
56  port_data_out.close();
57  }
58 
60  {
61  if (p) driver=p;
62  }
63 
65  {
66  if (a) actions=a;
67  }
68 
69  bool threadInit()
70  {
71  if (!driver)
72  return false;
73  return true;
74  }
75 
76  void run()
77  {
78  //quick reads the current position
79  double encs[50];
80  if (driver && driver->ienc_ll)
81  {
82  driver->ienc_ll->getEncoders(encs);
83  }
84  else
85  {
86  //invalid driver
87  }
88 
89  //quick reads the pid output
90  double outs[50];
91  if (driver && driver->ipid_ll)
92  {
93  driver->ipid_ll->getPidOutputs(VOCAB_PIDTYPE_POSITION,outs);
94  }
95  else
96  {
97  //invalid driver
98  }
99 
100  //quick reads the pid error
101  double errs[50];
102  if (driver && driver->ipid_ll)
103  {
104  driver->ipid_ll->getPidErrors(VOCAB_PIDTYPE_POSITION,errs);
105  }
106  else
107  {
108  //invalid driver
109  }
110 
111  //quick reads the optical encoders
112  double opts[50];
113  if (driver && driver->imotenc_ll)
114  {
115  driver->imotenc_ll->getMotorEncoders(opts);
116  }
117  else
118  {
119  //invalid driver
120  }
121 
122  int j = actions->current_action;
123 
124  Bottle& bot2 = this->port_data_out.prepare();
125  bot2.clear();
126  bot2.addInt(actions->action_vector[j].counter);
127  bot2.addDouble(actions->action_vector[j].time);
128 
129  int size = this->actions->action_vector[j].get_n_joints();
130  double *ll = actions->action_vector[j].q_joints;
131 
132  bot2.addString("commands:");
133  for (int ix=0;ix<size;ix++)
134  {
135  bot2.addDouble(ll[ix]);
136  }
137  bot2.addString("encoders:");
138  for (int ix=0;ix<size;ix++)
139  {
140  bot2.addDouble(encs[ix]);
141  }
142  bot2.addString("outputs:");
143  for (int ix=0;ix<size;ix++)
144  {
145  bot2.addDouble(outs[ix]);
146  }
147  bot2.addString("optical:");
148  for (int ix=0;ix<size;ix++)
149  {
150  bot2.addDouble(opts[ix]);
151  }
152  bot2.addString("errors:");
153  for (int ix=0;ix<size;ix++)
154  {
155  bot2.addDouble(errs[ix]);
156  }
157  bot2.addString("timestamp:");
158  bot2.addDouble(yarp::os::Time::now());
159  this->port_data_out.write();
160  }
161 };
162 
163 class WorkingThread: public PeriodicThread
164 {
165 private:
166 
167 public:
170  mutex mtx;
171  BufferedPort<Bottle> port_command_out;
172  BufferedPort<Bottle> port_command_joints;
174  double start_time;
175 
176  WorkingThread(double period=5): PeriodicThread((double)period/1000.0)
177  {
178  enable_execute_joint_command = true;
179  //*** open the output port
180  port_command_out.open("/trajectoryPlayer/port_command_out:o");
181  port_command_joints.open("/trajectoryPlayer/port_joints:o");
182  }
183 
185  {
186  port_command_out.interrupt();
187  port_command_out.close();
188  port_command_joints.interrupt();
189  port_command_joints.close();
190  }
191 
193  {
194  if (p) driver=p;
195  }
196 
197  bool threadInit()
198  {
199  if (!driver)
200  return false;
201  return true;
202  }
203 
204  bool execute_joint_command(int action_id)
205  {
206  if (!driver) return false;
207  if (!enable_execute_joint_command) return true;
208 
209  double *ll = actions.action_vector[action_id].q_joints;
210  int nj = actions.action_vector[action_id].get_n_joints();
211 
212  for (int j = 0; j < nj; j++)
213  {
214  driver->setPosition(j, ll[j]);
215  }
216  return true;
217  }
218 
219  void compute_and_send_command(int action_id)
220  {
221  //prepare the output command
222  Bottle& bot = port_command_out.prepare();
223  bot.clear();
224  bot.addInt(actions.action_vector[action_id].counter);
225  bot.addDouble(actions.action_vector[action_id].time);
226  bot.addString(actions.action_vector[action_id].tag.c_str());
227  //@@@ you can add stuff here...
228 
229  //send the output command
230  port_command_out.write();
231  if (!execute_joint_command(action_id))
232  {
233  yError("failed to execute command");
234  }
235 
236  //quick reads the current position
237  double encs[50];
238  if (driver)
239  {
240  int nj= actions.action_vector[action_id].get_n_joints();
241  for (int j = 0; j < nj; j++)
242  {
243  driver->getEncoder(j, &encs[j]);
244  }
245  }
246  else
247  {
248  //invalid driver
249  yError("Critical error: invalid driver");
250  }
251 
252  //send the joints angles on debug port
253  double *ll = actions.action_vector[action_id].q_joints;
254  Bottle& bot2 = this->port_command_joints.prepare();
255  bot2.clear();
256  bot2.addInt(actions.action_vector[action_id].counter);
257  bot2.addDouble(actions.action_vector[action_id].time);
258  int size = this->actions.action_vector[action_id].get_n_joints();
259  bot2.addString("commands:");
260  for (int ix=0;ix<size;ix++)
261  {
262  bot2.addDouble(ll[ix]);
263  }
264  bot2.addString("encoders:");
265  for (int ix=0;ix<size;ix++)
266  {
267  bot2.addDouble(encs[ix]);
268  }
269  this->port_command_joints.write();
270  }
271 
272  void run()
273  {
274  lock_guard<mutex> lck(mtx);
275  double current_time = yarp::os::Time::now();
276  if (actions.current_status==ACTION_IDLE)
277  {
278  // do nothing
279  }
280  else if (actions.current_status == ACTION_STOP)
281  {
282  int nj = actions.action_vector[0].get_n_joints();
283  actions.current_status = ACTION_IDLE;
284  }
285  else if (actions.current_status == ACTION_RESET)
286  {
287  int nj = actions.action_vector[0].get_n_joints();
288  for (int j = 0; j < nj; j++)
289  {
290  driver->setControlMode(j, VOCAB_CM_POSITION);
291  }
292  actions.current_status = ACTION_IDLE;
293  }
294  else if (actions.current_status == ACTION_RUNNING)
295  {
296  size_t last_action = actions.action_vector.size();
297  if (last_action == 0)
298  {
299  yError("sequence empty!");
301  return;
302  }
303 
304  //if it's not the last action
305  if (actions.current_action < last_action-1)
306  {
307  //if enough time is passed from the previous action
308  //double duration = actions.action_vector[actions.current_action+1].time -
309  // actions.action_vector[actions.current_action].time;
310  double elapsed_time = actions.action_vector[actions.current_action].time;
311  if (current_time-start_time > elapsed_time)
312  {
313  //printf("%d %+6.6f \n", actions.current_action, current_time-start_time);
314  //last_time = current_time;
315  actions.current_action++;
316  compute_and_send_command(actions.current_action);
317  yDebug("Executing action: %4zd/%4zd", actions.current_action , last_action);
318  //printf("EXECUTING %d, elapsed_time:%.5f requested_time:%.5f\n", actions.current_action, current_time-last_time, duration);
319  }
320  else
321  {
322  //printf("WAITING %d, elapsed_time:%.5f requested_time:%.5f\n", actions.current_action, current_time-last_time, duration);
323  }
324  }
325  else
326  {
327  if (actions.forever==false)
328  {
329  yInfo("sequence completed in: %f s",yarp::os::Time::now()-start_time);
331  }
332  else
333  {
334  yInfo("sequence completed in: %f s, restarting", yarp::os::Time::now() - start_time);
335  actions.current_action=0;
336  start_time = yarp::os::Time::now();
337  }
338  }
339  }
340  else if (actions.current_status==ACTION_START)
341  {
342  if (actions.action_vector.size()>0)
343  {
344  double *ll = actions.action_vector[0].q_joints;
345  int nj = actions.action_vector[0].get_n_joints();
346  for (int j = 0; j < nj; j++)
347  {
348  driver->setControlMode(j, VOCAB_CM_POSITION);
349  }
350  yarp::os::Time::delay(0.1);
351  for (int j = 0; j < nj; j++)
352  {
353  driver->positionMove(j, ll[j]);
354  }
355 
356  yInfo() << "going to home position";
357  double enc[50];
358  int loops = 100;
359  bool check = true;
360  do
361  {
362  check = true;
363  for (int j = 0; j < nj; j++)
364  {
365  driver->getEncoder(j, &enc[j]);
366  double err = fabs(enc[j] - ll[j]);
367  check &= (err < 2.0);
368  }
369  yarp::os::Time::delay(0.1);
370  loops--;
371  } while (!check && loops>0);
372 
373  if (check)
374  {
375  yInfo() << "done";
376 
377  for (int j = 0; j <nj; j++)
378  {
379  driver->setControlMode(j, VOCAB_CM_POSITION_DIRECT);
380  }
381  yarp::os::Time::delay(0.1);
382  compute_and_send_command(0);
383 
384  actions.current_status = ACTION_RUNNING;
385  start_time = yarp::os::Time::now();
386  }
387  else
388  {
389  yError() << "unable to reach start position!";
390  if (0)
391  {
392  //very strict behavior! if your are controlling fingers, you will probably end here
393  actions.current_status = ACTION_STOP;
394  }
395  else
396  {
397  for (int j = 0; j <nj; j++)
398  {
399  driver->setControlMode(j, VOCAB_CM_POSITION_DIRECT);
400  }
401  yarp::os::Time::delay(0.1);
402  compute_and_send_command(0);
403 
404  actions.current_status = ACTION_RUNNING;
405  start_time = yarp::os::Time::now();
406  }
407  }
408  }
409  else
410  {
411  yWarning("no sequence in memory");
412  actions.current_status=ACTION_STOP;
413  }
414  }
415  else
416  {
417  yError() << "unknown current_status";
418  }
419  }
420 };
421 
422 // ******************** THE MODULE
423 class scriptModule: public RFModule
424 {
425 protected:
426  Port rpcPort;
427  string name;
428  bool verbose;
432 
433 public:
435  {
436  verbose=true;
437  }
438 
439  virtual bool configure(ResourceFinder &rf)
440  {
441  if (rf.check("name"))
442  name=string("/")+rf.find("name").asString().c_str();
443  else
444  name="/trajectoryPlayer";
445 
446  rpcPort.open((name+"/rpc").c_str());
447  attach(rpcPort);
448 
449  Property portProp;
450  portProp.put("robot", rf.find("robot"));
451  portProp.put("part", rf.find("part"));
452 
453  //*** start the robot driver
454  if (!robot.configure(portProp))
455  {
456  yError() << "Error configuring position controller, check parameters";
457  return false;
458  }
459 
460  if (!robot.init())
461  {
462  yError() << "Error cannot connect to remote ports" ;
463  return false;
464  }
465 
466  //*** attach the robot driver to the thread and start it
467  w_thread.attachRobotDriver(&robot);
468  b_thread.attachRobotDriver(&robot);
469  if (!w_thread.start())
470  {
471  yError() << "Working thread did not start, queue will not work";
472  }
473  else
474  {
475  yInfo() << "Working thread started";
476  }
477 
478  if (rf.check("execute")==true)
479  {
480  yInfo() << "Enabling iPid->setReference() controller";
481  w_thread.enable_execute_joint_command = true;
482  }
483  else
484  {
485  yInfo() << "Not using iPid->setReference() controller";
486  w_thread.enable_execute_joint_command = false;
487  }
488 
489  if (rf.check("period")==true)
490  {
491  int period = rf.find("period").asInt();
492  yInfo() << "Thread period set to " << period << "ms";
493  w_thread.setPeriod((double)period/1000.0);
494  }
495  yInfo() << "Using parameters:" << rf.toString();
496 
497  //*** open the position file
498  yInfo() << "opening file...";
499  if (rf.check("filename")==true)
500  {
501  string filename = rf.find("filename").asString().c_str();
502  int req_joints = 0; //default value
503  if (rf.check("joints"))
504  {
505  req_joints = rf.find("joints").asInt();}
506  else
507  {
508  yError() << "Missing parameter 'joints' (number of joints to control)";
509  return false;
510  }
511 
512  if (req_joints > robot.n_joints)
513  {
514  yError() << "Requested number of joints exceeds the number of available joints on the robot!";
515  return false;
516  }
517 
518  if (req_joints < robot.n_joints)
519  {
520  yWarning() << "changing n joints from" << robot.n_joints << "to" << req_joints;
521  robot.n_joints = req_joints;
522  }
523 
524  if (rf.check("jointsMap"))
525  {
526  Bottle* b = rf.find("jointsMap").asList();
527  if (!b) { yError() << "Error parsing parameter jointsMap"; return false; }
528  if (b->size() != req_joints) { yError() << "invalid size of jointsMap parameter"; return false; }
529  for (int i = 0; i < b->size(); i++)
530  {
531  robot.joints_map[i] = b->get(i).asInt();
532  }
533  }
534  else
535  {
536  //create a default joint map
537  for (int i = 0; i < req_joints; i++)
538  {
539  robot.joints_map[i] = i;
540  }
541  }
542 
543  if (!w_thread.actions.openFile(filename,robot.n_joints))
544  {
545  yError() << "Unable to parse file " << filename;
546  return false;
547  };
548  }
549  else
550  {
551  yWarning() << "no sequence files load.";
552  }
553 
554  yInfo() << "module successfully configured. ready.";
555  return true;
556  }
557 
558  virtual bool respond(const Bottle &command, Bottle &reply)
559  {
560  lock_guard<mutex> lck(this->w_thread.mtx);
561  bool ret=true;
562 
563  if (command.size()!=0)
564  {
565  string cmdstring = command.get(0).asString().c_str();
566  {
567  if (cmdstring == "help")
568  {
569  cout << "Available commands:" << endl;
570  cout << "start" << endl;
571  cout << "stop" << endl;
572  cout << "reset" << endl;
573  cout << "clear" << endl;
574  cout << "add" << endl;
575  cout << "load" << endl;
576  cout << "forever" << endl;
577  cout << "list" << endl;
578  reply.addVocab(Vocab::encode("many"));
579  reply.addVocab(Vocab::encode("ack"));
580  reply.addString("Available commands:");
581  reply.addString("start");
582  reply.addString("stop");
583  reply.addString("reset");
584  reply.addString("clear");
585  reply.addString("add");
586  reply.addString("load");
587  reply.addString("forever");
588  reply.addString("list");
589  }
590  else if (cmdstring == "start")
591  {
592  if (this->w_thread.actions.current_action == 0)
593  this->w_thread.actions.current_status = ACTION_START;
594  else
595  this->w_thread.actions.current_status = ACTION_RUNNING;
596  this->w_thread.actions.forever = false;
597 
598  this->b_thread.attachActions(&w_thread.actions);
599  if (this->b_thread.isRunning()==false) b_thread.start();
600 
601  reply.addVocab(Vocab::encode("ack"));
602  }
603  else if (cmdstring == "forever")
604  {
605  if (this->w_thread.actions.current_action == 0)
606  this->w_thread.actions.current_status = ACTION_START;
607  else
608  this->w_thread.actions.current_status = ACTION_RUNNING;
609  w_thread.actions.forever = true;
610  reply.addVocab(Vocab::encode("ack"));
611  }
612  else if (cmdstring == "stop")
613  {
614  this->w_thread.actions.current_status = ACTION_STOP;
615  if (this->b_thread.isRunning()==true) b_thread.askToStop();
616 
617  reply.addVocab(Vocab::encode("ack"));
618  }
619  else if (cmdstring == "clear")
620  {
621  this->w_thread.actions.clear();
622  reply.addVocab(Vocab::encode("ack"));
623  }
624  else if (cmdstring == "add")
625  {
626  if(!w_thread.actions.parseCommandLine(command.get(1).asString().c_str(), -1, robot.n_joints))
627  {
628  yError() << "Unable to parse command";
629  reply.addVocab(Vocab::encode("ERROR Unable to parse file"));
630  }
631  else
632  {
633  yInfo() << "Command added";
634  reply.addVocab(Vocab::encode("ack"));
635  }
636  }
637  else if (cmdstring == "load")
638  {
639  string filename = command.get(1).asString().c_str();
640  if (!w_thread.actions.openFile(filename, robot.n_joints))
641  {
642  yError() << "Unable to parse file";
643  reply.addVocab(Vocab::encode("ERROR Unable to parse file"));
644  }
645  else
646  {
647  yInfo() << "File opened";
648  reply.addVocab(Vocab::encode("ack"));
649  }
650  }
651  else if (cmdstring == "reset")
652  {
653  this->w_thread.actions.current_status = ACTION_RESET;
654  this->w_thread.actions.current_action = 0;
655 
656  if (this->b_thread.isRunning()==true) b_thread.askToStop();
657 
658  reply.addVocab(Vocab::encode("ack"));
659  }
660  else if (cmdstring == "list")
661  {
662  this->w_thread.actions.print();
663  reply.addVocab(Vocab::encode("ack"));
664  }
665  else
666  {
667  reply.addVocab(Vocab::encode("nack"));
668  ret = false;
669  }
670  }
671  }
672  else
673  {
674  reply.addVocab(Vocab::encode("nack"));
675  ret = false;
676  }
677 
678  return ret;
679  }
680 
681  virtual bool close()
682  {
683  rpcPort.interrupt();
684  rpcPort.close();
685 
686  return true;
687  }
688 
689  virtual double getPeriod() { return 1.0; }
690  virtual bool updateModule() { return true; }
691 };
692 
693 
694 int main(int argc, char *argv[])
695 {
696  ResourceFinder rf;
697  rf.setDefaultContext("ctpService");
698  rf.configure(argc,argv);
699 
700  if (rf.check("help"))
701  {
702  yInfo() << "Options:";
703  yInfo() << "\t--joints <n>: number of joints, default 6";
704  yInfo() << "\t--jointsMap (1 2 3...) map of the joints to be controller. Size must be equal to --joints";
705  yInfo() << "\t--name <moduleName>: set new module name";
706  yInfo() << "\t--robot <robotname>: robot name";
707  yInfo() << "\t--part <robotname>: part name";
708  yInfo() << "\t--filename <filename>: the positions file";
709  yInfo() << "\t--execute activate the iPid->setReference() control";
710  yInfo() << "\t--period <period>: the period in ms of the internal thread (default 5)";
711  yInfo() << "\t--verbose to display additional infos";
712  return 0;
713  }
714 
715  Network yarp;
716 
717  if (!yarp.checkNetwork())
718  {
719  yError() << "yarp.checkNetwork() failed.";
720  return -1;
721  }
722 
723  scriptModule mod;
724 
725  return mod.runModule(rf);
726 }
727 
728 
729 
BufferedPort< Bottle > port_command_out
Definition: main.cpp:171
void attachRobotDriver(robotDriver *p)
Definition: main.cpp:192
bool setControlMode(const int j, const int mode)
Definition: utils.cpp:321
BroadcastingThread b_thread
Definition: main.cpp:431
bool init()
Definition: utils.cpp:281
virtual bool configure(ResourceFinder &rf)
Definition: main.cpp:439
#define ACTION_START
Definition: utils.h:55
#define ACTION_STOP
Definition: utils.h:57
virtual bool close()
Definition: main.cpp:681
bool parseCommandLine(const char *command_line, int line, int n_joints)
Definition: utils.cpp:203
#define ACTION_RESET
Definition: utils.h:58
size_t current_action
Definition: utils.h:81
BufferedPort< Bottle > port_command_joints
Definition: main.cpp:172
void attachActions(action_class *a)
Definition: main.cpp:64
action_class actions
Definition: main.cpp:169
bool getEncoder(int j, double *v)
Definition: utils.cpp:333
std::map< int, int > joints_map
Definition: utils.h:113
bool threadInit()
Definition: main.cpp:197
Copyright (C) 2008 RobotCub Consortium.
std::deque< action_struct > action_vector
Definition: utils.h:84
void run()
Definition: main.cpp:272
bool configure(const Property &copt)
Definition: utils.cpp:256
bool setPosition(int j, double ref)
Definition: utils.cpp:327
virtual double getPeriod()
Definition: main.cpp:689
int n_joints
Definition: utils.h:112
WorkingThread(double period=5)
Definition: main.cpp:176
#define ACTION_RUNNING
Definition: utils.h:56
bool enable_execute_joint_command
Definition: main.cpp:173
void clear()
Definition: utils.cpp:100
bool forever
Definition: utils.h:83
~WorkingThread()
Definition: main.cpp:184
bool openFile(string filename, int n_joints)
Definition: utils.cpp:124
void compute_and_send_command(int action_id)
Definition: main.cpp:219
mutex mtx
Definition: main.cpp:170
bool positionMove(int j, double ref)
Definition: utils.cpp:339
#define ACTION_IDLE
Definition: utils.h:54
virtual bool updateModule()
Definition: main.cpp:690
virtual bool respond(const Bottle &command, Bottle &reply)
Definition: main.cpp:558
double start_time
Definition: main.cpp:174
int current_status
Definition: utils.h:82
bool execute_joint_command(int action_id)
Definition: main.cpp:204
scriptModule()
Definition: main.cpp:434
BroadcastingThread(int period=1)
Definition: main.cpp:48
void print()
Definition: utils.cpp:113
bool threadInit()
Definition: main.cpp:69
robotDriver robot
Definition: main.cpp:429
void attachRobotDriver(robotDriver *p)
Definition: main.cpp:59
robotDriver * driver
Definition: main.cpp:168
WorkingThread w_thread
Definition: main.cpp:430
int main(int argc, char *argv[])
Definition: main.cpp:31