d4c
d4c_server.cpp
1 /*
2  * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia
3  * Authors: Ilaria Gori, Ugo Pattacini
4  * email: ilaria.gori@iit.it, ugo.pattacini@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #include <cstdio>
19 #include <cstdarg>
20 #include <algorithm>
21 
22 #include <gsl/gsl_math.h>
23 
24 #include <yarp/math/Math.h>
25 
26 #include <iCub/ctrl/math.h>
27 #include <iCub/d4c/d4c_server.h>
28 #include <iCub/d4c/private/d4c_helpers.h>
29 
30 using namespace std;
31 using namespace yarp::os;
32 using namespace yarp::dev;
33 using namespace yarp::sig;
34 using namespace yarp::math;
35 using namespace iCub::ctrl;
36 using namespace iCub::d4c;
37 
38 
39 /************************************************************************/
40 Item::Item()
41 {
42  name="";
43  type="undefined";
44  active=false;
45  center.resize(3,0.0);
46  orientation.resize(4,0.0);
47  radius.resize(3,0.0);
48  color.resize(3,0.0);
49 }
50 
51 
52 /************************************************************************/
53 bool Item::fromProperty(const Property &options)
54 {
55  if (options.check("name"))
56  name=options.find("name").asString().c_str();
57 
58  if (options.check("active"))
59  active=options.find("active").asString()=="on";
60 
61  extractVector(options,"center",center);
62  extractVector(options,"orientation",orientation);
63  extractVector(options,"radius",radius);
64  extractVector(options,"color",color);
65 
66  return true;
67 }
68 
69 
70 /************************************************************************/
71 Property Item::toProperty() const
72 {
73  Value vcenter; vcenter.fromString(("("+string(center.toString().c_str())+")").c_str());
74  Value vorientation; vorientation.fromString(("("+string(orientation.toString().c_str())+")").c_str());
75  Value vradius; vradius.fromString(("("+string(radius.toString().c_str())+")").c_str());
76  Value vcolor; vcolor.fromString(("("+string(color.toString().c_str())+")").c_str());
77 
78  Property prop;
79  prop.put("name",name.c_str());
80  prop.put("type",type.c_str());
81  prop.put("active",active?"on":"off");
82  prop.put("center",vcenter);
83  prop.put("orientation",vorientation);
84  prop.put("radius",vradius);
85  prop.put("color",vcolor);
86 
87  return prop;
88 }
89 
90 
91 /************************************************************************/
92 Target_MSD::Target_MSD() : K(0.0), D(0.0)
93 {
94  type="target_msd";
95  color[0]=0.0;
96  color[1]=255.0;
97  color[2]=0.0;
98 }
99 
100 
101 /************************************************************************/
102 bool Target_MSD::fromProperty(const Property &options)
103 {
104  // call the father's method
105  Item::fromProperty(options);
106 
107  if (options.check("K"))
108  K=options.find("K").asDouble();
109 
110  if (options.check("D"))
111  D=options.find("D").asDouble();
112 
113  return true;
114 }
115 
116 
117 /************************************************************************/
118 Property Target_MSD::toProperty() const
119 {
120  // call the father's method
121  Property prop=Item::toProperty();
122 
123  prop.put("K",K);
124  prop.put("D",D);
125 
126  return prop;
127 }
128 
129 
130 /************************************************************************/
131 Vector Target_MSD::getField(const Vector &x, const Vector &xdot)
132 {
133  Vector F;
134 
135  if (active)
136  {
137  F=K*(center-getVectorPos(x))-D*getVectorPos(xdot);
138  F.push_back(0.0);
139  F.push_back(0.0);
140  F.push_back(0.0);
141  F.push_back(0.0);
142  }
143  else
144  F.resize(x.length(),0.0);
145 
146  return F;
147 }
148 
149 
150 /************************************************************************/
151 Obstacle_Gaussian::Obstacle_Gaussian() : G(0.0), cut_tails(false)
152 {
153  type="obstacle_gaussian";
154  color[0]=255.0;
155  color[1]=0.0;
156  color[2]=0.0;
157 }
158 
159 
160 /************************************************************************/
161 bool Obstacle_Gaussian::fromProperty(const Property &options)
162 {
163  // call the father's method
164  Item::fromProperty(options);
165 
166  if (options.check("G"))
167  G=options.find("G").asDouble();
168 
169  if (options.check("cut_tails"))
170  cut_tails=options.find("cut_tails").asString()=="on";
171 
172  return true;
173 }
174 
175 
176 /************************************************************************/
177 Property Obstacle_Gaussian::toProperty() const
178 {
179  // call the father's method
180  Property prop=Item::toProperty();
181 
182  prop.put("G",G);
183  prop.put("cut_tails",cut_tails?"on":"off");
184 
185  return prop;
186 }
187 
188 
189 /************************************************************************/
190 Vector Obstacle_Gaussian::getField(const Vector &x, const Vector &xdot)
191 {
192  Vector zeros(x.length(),0.0);
193  if (!active)
194  return zeros;
195 
196  Matrix Hro=axis2dcm(orientation);
197  Hro(0,3)=center[0];
198  Hro(1,3)=center[1];
199  Hro(2,3)=center[2];
200 
201  Vector xr=getVectorPos(x);
202  xr.push_back(1.0);
203 
204  Vector xo=SE3inv(Hro)*xr;
205 
206  xr.pop_back();
207  xo.pop_back();
208 
209  Vector ell=(xo*xo)/(radius*radius);
210  double dist=ell[0]+ell[1]+ell[2];
211 
212  if (cut_tails && (dist>1.0))
213  return zeros;
214 
215  Vector dx=xr-center;
216  Vector F=(G*exp(-dist)/norm(dx))*dx;
217  F.push_back(0.0);
218  F.push_back(0.0);
219  F.push_back(0.0);
220  F.push_back(0.0);
221 
222  return F;
223 }
224 
225 
226 /************************************************************************/
227 GuiReporter::GuiReporter()
228 {
229  server=NULL;
230 }
231 
232 
233 /************************************************************************/
234 void GuiReporter::setServer(D4CServer *server)
235 {
236  this->server=server;
237 }
238 
239 
240 /************************************************************************/
241 void GuiReporter::report(const PortInfo &info)
242 {
243  if ((server!=NULL) && info.created && !info.incoming)
244  server->scheduleInitGuiTrajectory();
245 }
246 
247 
248 /************************************************************************/
249 D4CServer::D4CServer() : RateThread(20), If(0.0,Vector(7)), Iv(0.0,Vector(7))
250 {
251  isOpen=false;
252  fieldEnabled=false;
253  controlEnabled=false;
254  simulationEnabled=false;
255  simulationFirstStep=false;
256  doInitGuiTrajectory=false;
257  offlineMode=false;
258  initIntegration=true;
259  verbosity=0;
260  name="";
261  activeIF="";
262  iCtrlRight=NULL;
263  iCtrlLeft=NULL;
264  itCnt=0;
265  doTrajectoryCnt=0;
266 
267  xdot.resize(7,0.0);
268  x.resize(7,0.0);
269  xhat.resize(7,0.0);
270 
271  toolFrame=invToolFrame=eye(4,4);
272 }
273 
274 
275 /************************************************************************/
276 D4CServer::~D4CServer()
277 {
278  close();
279 }
280 
281 
282 /************************************************************************/
283 void D4CServer::printMessage(const int logtype, const int level,
284  const char *format, ...) const
285 {
286  if (verbosity>=level)
287  {
288  string str;
289  str="*** "+name+": ";
290 
291  va_list arg;
292  char buf[512];
293  va_start(arg,format);
294  vsnprintf(buf,sizeof(buf),format,arg);
295  va_end(arg);
296 
297  str+=buf;
298  switch (logtype)
299  {
300  case log::error:
301  yError(str.c_str());
302  break;
303  case log::warning:
304  yWarning(str.c_str());
305  break;
306  case log::info:
307  yInfo(str.c_str());
308  break;
309  default:
310  printf("%s\n",str.c_str());
311  }
312  }
313 }
314 
315 
316 /************************************************************************/
317 bool D4CServer::open(const Property &options)
318 {
319  verbosity=options.check("verbosity",Value(0)).asInt();
320  device=options.check("device",Value("cartesiancontrollerclient")).asString().c_str();
321  name=options.check("name",Value("d4c_server")).asString().c_str();
322  robot=options.check("robot",Value("icub")).asString().c_str();
323  part=options.check("part",Value("both_arms")).asString().c_str();
324  offlineMode=options.check("offline");
325 
326  if ((part!="right_arm") && (part!="left_arm") && (part!="both_arms"))
327  {
328  printMessage(log::error,1,"server failed to open, unknown part specified");
329  return false;
330  }
331 
332  period=options.check("period",Value(20)).asInt();
333 
334  double Ts=(double)period/1000.0;
335 
336  If.setTs(Ts);
337  Iv.setTs(Ts);
338 
339  If.reset(xdot);
340  Iv.reset(x);
341 
342  reporter.setServer(this);
343  gui.setReporter(reporter);
344 
345  data.open(("/"+name+"/data:o").c_str());
346  gui.open(("/"+name+"/gui:o").c_str());
347  rpc.open(("/"+name+"/rpc").c_str());
348  rpc.setReader(*this);
349 
350  if (!offlineMode)
351  {
352  if ((part=="right_arm") || (part=="both_arms"))
353  {
354  Property optCtrlRight;
355  optCtrlRight.put("device",device.c_str());
356  optCtrlRight.put("remote",("/"+robot+"/cartesianController/right_arm").c_str());
357  optCtrlRight.put("local",("/"+name+"_right/cartesian").c_str());
358 
359  if (dCtrlRight.open(optCtrlRight))
360  dCtrlRight.view(iCtrlRight);
361  else
362  return false;
363  }
364 
365  if ((part=="left_arm") || (part=="both_arms"))
366  {
367  Property optCtrlLeft;
368  optCtrlLeft.put("device",device.c_str());
369  optCtrlLeft.put("remote",("/"+robot+"/cartesianController/left_arm").c_str());
370  optCtrlLeft.put("local",("/"+name+"_left/cartesian").c_str());
371 
372  if (dCtrlLeft.open(optCtrlLeft))
373  dCtrlLeft.view(iCtrlLeft);
374  else
375  return false;
376 
377  }
378 
379  if ((part=="both_arms") || (part=="right_arm"))
380  {
381  iCtrlActive=iCtrlRight;
382  activeIF="right";
383  }
384  else
385  {
386  iCtrlActive=iCtrlLeft;
387  activeIF="left";
388  }
389 
390  Vector dof;
391  iCtrlActive->getDOF(dof);
392  qhat.resize(dof.length(),0.0);
393 
394  t0=Time::now();
395  }
396 
397  // request high resolution scheduling straightaway
398  Time::turboBoost();
399 
400  setRate(period);
401  start();
402 
403  printMessage(log::info,1,"server successfully open%s", offlineMode?" (offline mode)":"");
404 
405  return isOpen=true;
406 }
407 
408 
409 /************************************************************************/
410 void D4CServer::close()
411 {
412  if (isOpen)
413  {
414  // close prior before any other stuff
415  rpc.interrupt();
416  rpc.close();
417 
418  if (isRunning())
419  stop();
420 
421  if (!offlineMode)
422  {
423  if ((part=="right_arm") || (part=="both_arms"))
424  dCtrlRight.close();
425  if ((part=="left_arm") || (part=="both_arms"))
426  dCtrlLeft.close();
427  }
428 
429  mutex.lock();
430 
431  guiQueue.clear();
432 
433  for (map<int,Item*>::iterator it=table.begin(); it!=table.end(); it++)
434  {
435  eraseGuiItem(GuiRequest("erase",it));
436 
437  delete it->second;
438 
439  printMessage(log::no_info,1,"erasing item %d",it->first);
440 
441  Time::delay(0.02);
442  }
443 
444  table.clear();
445 
446  eraseGuiTrajectory();
447 
448  data.interrupt();
449  data.close();
450 
451  gui.interrupt();
452  gui.close();
453 
454  isOpen=false;
455 
456  mutex.unlock();
457 
458  printMessage(log::info,1,"server closed");
459  }
460  else
461  printMessage(log::warning,3,"server is already closed");
462 }
463 
464 
465 /************************************************************************/
466 Item* D4CServer::itemFactory(const Property &options)
467 {
468  Item *item=NULL;
469 
470  if (options.check("type"))
471  {
472  string name=options.check("name",Value("")).asString().c_str();
473  string type=options.find("type").asString().c_str();
474 
475  printMessage(log::no_info,2,"creating \"%s\" of type %s ...",name.c_str(),type.c_str());
476 
477  if (type=="target_msd")
478  item=new Target_MSD;
479  else if (type=="obstacle_gaussian")
480  item=new Obstacle_Gaussian;
481  }
482  else
483  printMessage(log::warning,2,"option \"type\" not found");
484 
485  if (item!=NULL)
486  printMessage(log::no_info,2,"successfully created");
487  else
488  printMessage(log::warning,2,"unknown type");
489 
490  return item;
491 }
492 
493 
494 /************************************************************************/
495 bool D4CServer::addItem(const Property &options, int &item)
496 {
497  if (isOpen)
498  {
499  mutex.lock();
500  printMessage(log::no_info,2,"received request for adding item: %s",
501  options.toString().c_str());
502 
503  bool ret=false;
504  if (Item *pItem=itemFactory(options))
505  {
506  // configure item
507  pItem->fromProperty(options);
508  table[item=itCnt++]=pItem;
509  map<int,Item*>::iterator it=table.find(item);
510  pushUpdateGuiItem(it);
511 
512  printMessage(log::no_info,1,"added item %s",
513  pItem->toProperty().toString().c_str());
514  ret=true;
515  }
516  else
517  printMessage(log::error,1,"wrong request detected!");
518 
519  mutex.unlock();
520  return ret;
521  }
522  else
523  {
524  printMessage(log::warning,1,"server is not open");
525  return false;
526  }
527 }
528 
529 
530 /************************************************************************/
531 bool D4CServer::eraseItem(const int item)
532 {
533  if (isOpen)
534  {
535  mutex.lock();
536  printMessage(log::no_info,2,"received request for erasing item %d",item);
537 
538  bool ret=false;
539  map<int,Item*>::iterator it=table.find(item);
540  if (it!=table.end())
541  {
542  pushEraseGuiItem(it);
543 
544  delete it->second;
545  table.erase(it);
546 
547  printMessage(log::no_info,1,"item %d scheduled for erasing",item);
548  ret=true;
549  }
550  else
551  printMessage(log::warning,1,"item %d not found!",item);
552 
553  mutex.unlock();
554  return ret;
555  }
556  else
557  {
558  printMessage(log::warning,1,"server is not open");
559  return false;
560  }
561 }
562 
563 
564 /************************************************************************/
565 bool D4CServer::clearItems()
566 {
567  if (isOpen)
568  {
569  mutex.lock();
570  for (map<int,Item*>::iterator it=table.begin(); it!=table.end(); it++)
571  {
572  pushEraseGuiItem(it);
573  delete it->second;
574  }
575 
576  table.clear();
577  printMessage(log::no_info,1,"all items have been scheduled for erasing");
578 
579  mutex.unlock();
580  return true;
581  }
582  else
583  {
584  printMessage(log::warning,1,"server is not open");
585  return false;
586  }
587 }
588 
589 
590 /************************************************************************/
591 bool D4CServer::getItems(Bottle &items)
592 {
593  if (isOpen)
594  {
595  mutex.lock();
596  items.clear();
597  for (map<int,Item*>::const_iterator it=table.begin(); it!=table.end(); it++)
598  items.addInt(it->first);
599 
600  printMessage(log::no_info,1,"list of items ids prepared for sending: (%s)",
601  items.toString().c_str());
602 
603  mutex.unlock();
604  return true;
605  }
606  else
607  {
608  printMessage(log::warning,1,"server is not open");
609  return false;
610  }
611 }
612 
613 
614 /************************************************************************/
615 bool D4CServer::setProperty(const int item, const Property &options)
616 {
617  if (isOpen)
618  {
619  mutex.lock();
620  printMessage(log::no_info,2,"received request for setting item %d property: %s",
621  item,options.toString().c_str());
622 
623  bool ret=false;
624  map<int,Item*>::iterator it=table.find(item);
625  if (it!=table.end())
626  {
627  it->second->fromProperty(options);
628  pushUpdateGuiItem(it);
629 
630  printMessage(log::no_info,1,"item %d property successfully updated: %s",
631  item,it->second->toProperty().toString().c_str());
632  ret=true;
633  }
634  else
635  printMessage(log::warning,1,"item %d not found!",item);
636 
637  mutex.unlock();
638  return ret;
639  }
640  else
641  {
642  printMessage(log::warning,1,"server is not open");
643  return false;
644  }
645 }
646 
647 
648 /************************************************************************/
649 bool D4CServer::getProperty(const int item, Property &options)
650 {
651  if (isOpen)
652  {
653  mutex.lock();
654  printMessage(log::no_info,2,"received request for getting item %d property",item);
655 
656  bool ret=false;
657  map<int,Item*>::const_iterator it=table.find(item);
658  if (it!=table.end())
659  {
660  options=it->second->toProperty();
661 
662  printMessage(log::no_info,1,"item %d property successfully retrieved: %s",
663  item,options.toString().c_str());
664  ret=true;
665  }
666  else
667  printMessage(log::warning,1,"item %d not found!",item);
668 
669  mutex.unlock();
670  return ret;
671  }
672  else
673  {
674  printMessage(log::warning,1,"server is not open");
675  return false;
676  }
677 }
678 
679 
680 /************************************************************************/
681 bool D4CServer::enableField()
682 {
683  if (isOpen)
684  {
685  if (initIntegration)
686  {
687  Vector pos,orien;
688  iCtrlActive->getPose(pos,orien);
689  mutex.lock();
690  copyVectorData(pos,this->x);
691  copyVectorData(orien,this->x);
692  Iv.reset(x);
693  initIntegration=false;
694  mutex.unlock();
695  }
696  fieldEnabled=true;
697  printMessage(log::no_info,1,"field enabled");
698  return true;
699  }
700  else
701  {
702  printMessage(log::warning,1,"server is not open");
703  return false;
704  }
705 }
706 
707 
708 /************************************************************************/
709 bool D4CServer::disableField()
710 {
711  if (isOpen)
712  {
713  mutex.lock();
714  if (!offlineMode)
715  iCtrlActive->stopControl();
716 
717  fieldEnabled=false;
718  printMessage(log::no_info,1,"field disabled");
719 
720  mutex.unlock();
721  return true;
722  }
723  else
724  {
725  printMessage(log::warning,1,"server is not open");
726  return false;
727  }
728 }
729 
730 
731 /************************************************************************/
732 bool D4CServer::getFieldStatus(bool &status)
733 {
734  if (isOpen)
735  {
736  status=fieldEnabled;
737  printMessage(log::no_info,1,"field status = %s",status?"on":"off");
738  return true;
739  }
740  else
741  {
742  printMessage(log::warning,1,"server is not open");
743  return false;
744  }
745 }
746 
747 
748 /************************************************************************/
749 bool D4CServer::enableControl()
750 {
751  if (isOpen)
752  {
753  if (!offlineMode)
754  {
755  mutex.lock();
756  controlEnabled=true;
757  printMessage(log::no_info,1,"control enabled");
758 
759  if (simulationEnabled)
760  {
761  simulationEnabled=false;
762  printMessage(log::warning,2,"simulation gets automatically disabled");
763  }
764 
765  mutex.unlock();
766  return true;
767  }
768  else
769  {
770  printMessage(log::warning,1,"it is not possible to enable control in offline mode");
771  return false;
772  }
773  }
774  else
775  {
776  printMessage(log::warning,1,"server is not open");
777  return false;
778  }
779 }
780 
781 
782 /************************************************************************/
783 bool D4CServer::disableControl()
784 {
785  if (isOpen)
786  {
787  if (!offlineMode)
788  {
789  mutex.lock();
790  iCtrlActive->stopControl();
791  controlEnabled=false;
792  printMessage(log::no_info,1,"control disabled");
793  mutex.unlock();
794  return true;
795  }
796  else
797  {
798  printMessage(log::warning,1,"it is not possible to disable control in offline mode");
799  return false;
800  }
801  }
802  else
803  {
804  printMessage(log::warning,1,"server is not open");
805  return false;
806  }
807 }
808 
809 
810 /************************************************************************/
811 bool D4CServer::getControlStatus(bool &status)
812 {
813  if (isOpen)
814  {
815  if (!offlineMode)
816  {
817  status=controlEnabled;
818  printMessage(log::no_info,1,"control status = %s",status?"on":"off");
819  return true;
820  }
821  else
822  {
823  printMessage(log::warning,1,"there is no possibility to enable or disable control in offline mode");
824  return false;
825  }
826  }
827  else
828  {
829  printMessage(log::warning,1,"server is not open");
830  return false;
831  }
832 }
833 
834 
835 /************************************************************************/
836 bool D4CServer::enableSimulation()
837 {
838  if (isOpen)
839  {
840  if (!offlineMode)
841  {
842  mutex.lock();
843  simulationEnabled=true;
844  simulationFirstStep=true;
845  printMessage(log::no_info,1,"simulation enabled");
846 
847  if (controlEnabled)
848  {
849  controlEnabled=false;
850  printMessage(log::warning,2,"control gets automatically disabled");
851  }
852 
853  mutex.unlock();
854  return true;
855  }
856  else
857  {
858  printMessage(log::warning,1,"it is not possible to enable simulation in offline mode");
859  return false;
860  }
861  }
862  else
863  {
864  printMessage(log::warning,1,"server is not open");
865  return false;
866  }
867 }
868 
869 
870 /************************************************************************/
871 bool D4CServer::disableSimulation()
872 {
873  if (isOpen)
874  {
875  if (!offlineMode)
876  {
877  simulationEnabled=false;
878  printMessage(log::no_info,1,"simulation disabled");
879  return true;
880  }
881  else
882  {
883  printMessage(log::warning,1,"it is not possible to disable simulation in offline mode");
884  return false;
885  }
886  }
887  else
888  {
889  printMessage(log::warning,1,"server is not open");
890  return false;
891  }
892 }
893 
894 
895 /************************************************************************/
896 bool D4CServer::getSimulationStatus(bool &status)
897 {
898  if (isOpen)
899  {
900  if (!offlineMode)
901  {
902  status=simulationEnabled;
903  printMessage(log::no_info,1,"simulation status = %s",status?"on":"off");
904  return true;
905  }
906  else
907  {
908  printMessage(log::warning,1,"there is no possibility to enable or disable simulation in offline mode");
909  return false;
910  }
911  }
912  else
913  {
914  printMessage(log::warning,1,"server is not open");
915  return false;
916  }
917 }
918 
919 
920 /************************************************************************/
921 bool D4CServer::setPeriod(const int period)
922 {
923  if (isOpen)
924  {
925  mutex.lock();
926  this->period=period;
927  double Ts=(double)this->period/1000.0;
928 
929  If.setTs(Ts);
930  Iv.setTs(Ts);
931 
932  if (!offlineMode)
933  {
934  setRate(this->period);
935  printMessage(log::no_info,1,"thread period changed to %d [ms]",period);
936  }
937 
938  mutex.unlock();
939  return true;
940  }
941  else
942  {
943  printMessage(log::warning,1,"server is not open");
944  return false;
945  }
946 }
947 
948 
949 /************************************************************************/
950 bool D4CServer::getPeriod(int &period)
951 {
952  if (isOpen)
953  {
954  if (!offlineMode)
955  {
956  period=(int)const_cast<D4CServer*>(this)->getRate();
957  printMessage(log::no_info,1,"thread period is %d [ms]",period);
958  }
959  else
960  {
961  period=this->period;
962  printMessage(log::no_info,1,"integration time is %d [ms]",this->period);
963  }
964 
965  return true;
966  }
967  else
968  {
969  printMessage(log::warning,1,"server is not open");
970  return false;
971  }
972 }
973 
974 
975 /************************************************************************/
976 bool D4CServer::setPointState(const Vector &x, const Vector &o,
977  const Vector &xdot, const Vector &odot)
978 {
979  if (isOpen)
980  {
981  mutex.lock();
982  copyVectorData(x,this->x);
983  copyVectorData(o,this->x);
984  copyVectorData(xdot,this->xdot);
985  copyVectorData(odot,this->xdot);
986 
987  If.reset(this->xdot);
988  Iv.reset(this->x);
989 
990  initIntegration=false;
991 
992  printMessage(log::no_info,1,"point state changed to x = %s; xdot = %s",
993  this->x.toString().c_str(),this->xdot.toString().c_str());
994 
995  mutex.unlock();
996  return true;
997  }
998  else
999  {
1000  printMessage(log::warning,1,"server is not open");
1001  return false;
1002  }
1003 }
1004 
1005 
1006 /************************************************************************/
1007 bool D4CServer::setPointOrientation(const Vector &o, const Vector &odot)
1008 {
1009  if (isOpen)
1010  {
1011  mutex.lock();
1012  copyVectorData(o,this->x);
1013  copyVectorData(odot,this->xdot);
1014 
1015  If.reset(this->xdot);
1016  Iv.reset(this->x);
1017 
1018  printMessage(log::no_info,1,"point state changed to x = %s; xdot = %s",
1019  this->x.toString().c_str(),this->xdot.toString().c_str());
1020 
1021  mutex.unlock();
1022  return true;
1023  }
1024  else
1025  {
1026  printMessage(log::warning,1,"server is not open");
1027  return false;
1028  }
1029 }
1030 
1031 
1032 /************************************************************************/
1033 bool D4CServer::setPointStateToTool()
1034 {
1035  if (isOpen)
1036  {
1037  if (!offlineMode)
1038  {
1039  Vector x,o,xdot,odot;
1040  getTool(x,o);
1041  iCtrlActive->getTaskVelocities(xdot,odot);
1042  odot=0.0;
1043 
1044  mutex.lock();
1045  copyVectorData(x,this->x);
1046  copyVectorData(o,this->x);
1047  copyVectorData(xdot,this->xdot);
1048  copyVectorData(odot,this->xdot);
1049 
1050  If.reset(this->xdot);
1051  Iv.reset(this->x);
1052 
1053  initIntegration=false;
1054 
1055  printMessage(log::no_info,1,"point state changed to x = %s; xdot = %s",
1056  this->x.toString().c_str(),this->xdot.toString().c_str());
1057 
1058  mutex.unlock();
1059  return true;
1060  }
1061  else
1062  {
1063  printMessage(log::warning,1,"no connection with the robot in offline mode");
1064  return false;
1065  }
1066  }
1067  else
1068  {
1069  printMessage(log::warning,1,"server is not open");
1070  return false;
1071  }
1072 }
1073 
1074 
1075 /************************************************************************/
1076 bool D4CServer::attachToolFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
1077 {
1078  if (isOpen)
1079  {
1080  if (!offlineMode)
1081  {
1082  if ((x.length()<3) || (o.length()<4))
1083  {
1084  printMessage(log::error,1,"problem with vector lengths");
1085  return false;
1086  }
1087  else
1088  {
1089  mutex.lock();
1090  toolFrame=axis2dcm(o);
1091  toolFrame(0,3)=x[0];
1092  toolFrame(1,3)=x[1];
1093  toolFrame(2,3)=x[2];
1094 
1095  invToolFrame=SE3inv(toolFrame);
1096  printMessage(log::no_info,1,"attach tool frame = %s",toolFrame.toString().c_str());
1097 
1098  mutex.unlock();
1099  return true;
1100  }
1101  }
1102  else
1103  {
1104  printMessage(log::warning,1,"it is not possible to attach frame in offline mode");
1105  return false;
1106  }
1107  }
1108  else
1109  {
1110  printMessage(log::warning,1,"server is not open");
1111  return false;
1112  }
1113 }
1114 
1115 
1116 /************************************************************************/
1117 bool D4CServer::getToolFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
1118 {
1119  if (isOpen)
1120  {
1121  if (!offlineMode)
1122  {
1123  mutex.lock();
1124  x.resize(3);
1125  x[0]=toolFrame(0,3);
1126  x[1]=toolFrame(1,3);
1127  x[2]=toolFrame(2,3);
1128  o=dcm2axis(toolFrame);
1129 
1130  printMessage(log::no_info,1,"tool frame currently attached is = %s",
1131  toolFrame.toString().c_str());
1132 
1133  mutex.unlock();
1134  return true;
1135  }
1136  else
1137  {
1138  printMessage(log::warning,1,"there is no tool in offline mode");
1139  return false;
1140  }
1141  }
1142  else
1143  {
1144  printMessage(log::warning,1,"server is not open");
1145  return false;
1146  }
1147 }
1148 
1149 
1150 /************************************************************************/
1151 bool D4CServer::removeToolFrame()
1152 {
1153  if (isOpen)
1154  {
1155  if (!offlineMode)
1156  {
1157  mutex.lock();
1158  toolFrame=invToolFrame=eye(4,4);
1159  printMessage(log::no_info,1,"tool frame removed");
1160  mutex.unlock();
1161  return true;
1162  }
1163  else
1164  {
1165  printMessage(log::warning,1,"there is no tool in offline mode");
1166  return false;
1167  }
1168  }
1169  else
1170  {
1171  printMessage(log::warning,1,"server is not open");
1172  return false;
1173  }
1174 }
1175 
1176 
1177 /************************************************************************/
1178 bool D4CServer::getTool(yarp::sig::Vector &x, yarp::sig::Vector &o)
1179 {
1180  if (isOpen)
1181  {
1182  if (!offlineMode)
1183  {
1184  Vector pos,orien;
1185  iCtrlActive->getPose(pos,orien);
1186 
1187  Matrix frame1=axis2dcm(orien);
1188  frame1(0,3)=pos[0];
1189  frame1(1,3)=pos[1];
1190  frame1(2,3)=pos[2];
1191 
1192  Matrix frame2=frame1*toolFrame;
1193  x.resize(3);
1194  x[0]=frame2(0,3);
1195  x[1]=frame2(1,3);
1196  x[2]=frame2(2,3);
1197  o=dcm2axis(frame2);
1198 
1199  printMessage(log::no_info,1,"tool state is = %s",
1200  frame2.toString().c_str());
1201  return true;
1202  }
1203  else
1204  {
1205  printMessage(log::warning,1,"there is no tool in offline mode");
1206  return false;
1207  }
1208  }
1209  else
1210  {
1211  printMessage(log::warning,1,"server is not open");
1212  return false;
1213  }
1214 }
1215 
1216 
1217 /************************************************************************/
1218 bool D4CServer::getPointState(Vector &x, Vector &o, Vector &xdot, Vector &odot)
1219 {
1220  if (isOpen)
1221  {
1222  x=getVectorPos(this->x);
1223  o=getVectorOrien(this->x);
1224  xdot=getVectorPos(this->xdot);
1225  odot=getVectorOrien(this->xdot);
1226 
1227  printMessage(log::no_info,1,"point state is x = %s; xdot = %s",
1228  x.toString().c_str(),xdot.toString().c_str());
1229  return true;
1230  }
1231  else
1232  {
1233  printMessage(log::warning,1,"server is not open");
1234  return false;
1235  }
1236 }
1237 
1238 
1239 /************************************************************************/
1240 bool D4CServer::getField(Vector &field)
1241 {
1242  if (isOpen)
1243  {
1244  field.resize(x.length(),0.0);
1245  for (map<int,Item*>::const_iterator it=table.begin(); it!=table.end(); it++)
1246  field=field+it->second->getField(x,xdot);
1247 
1248  printMessage(log::no_info,1,"field = %s",field.toString().c_str());
1249  return true;
1250  }
1251  else
1252  {
1253  printMessage(log::warning,1,"server is not open");
1254  return false;
1255  }
1256 }
1257 
1258 
1259 /************************************************************************/
1260 bool D4CServer::getSimulation(Vector &xhat, Vector &ohat, Vector &qhat)
1261 {
1262  if (isOpen)
1263  {
1264  if (!offlineMode)
1265  {
1266  xhat=getVectorPos(this->xhat);
1267  ohat=getVectorOrien(this->xhat);
1268  qhat=this->qhat;
1269 
1270  printMessage(log::no_info,1,"simulated end-effector pose is xhat = %s; part configuration is qhat = %s",
1271  xhat.toString().c_str(),qhat.toString().c_str());
1272  return true;
1273  }
1274  else
1275  {
1276  printMessage(log::warning,1,"there is no possibility to enable simulation in offline mode");
1277  return false;
1278  }
1279  }
1280  else
1281  {
1282  printMessage(log::warning,1,"server is not open");
1283  return false;
1284  }
1285 }
1286 
1287 
1288 /************************************************************************/
1289 bool D4CServer::read(ConnectionReader &connection)
1290 {
1291  Bottle cmd, reply;
1292  cmd.read(connection);
1293 
1294  if (cmd.size()==0)
1295  reply.addVocab(D4C_VOCAB_CMD_NACK);
1296  else switch(cmd.get(0).asVocab())
1297  {
1298  //-----------------
1299  case D4C_VOCAB_CMD_PING:
1300  {
1301  reply.addVocab(D4C_VOCAB_CMD_ACK);
1302  break;
1303  }
1304 
1305  //-----------------
1306  case D4C_VOCAB_CMD_ADD:
1307  {
1308  if (cmd.size()<2)
1309  reply.addVocab(D4C_VOCAB_CMD_NACK);
1310  else
1311  {
1312  int item;
1313  if (addItem(extractProperty(cmd.get(1)),item))
1314  {
1315  reply.addVocab(D4C_VOCAB_CMD_ACK);
1316  reply.addInt(item);
1317  }
1318  else
1319  reply.addVocab(D4C_VOCAB_CMD_NACK);
1320  }
1321 
1322  break;
1323  }
1324 
1325  //-----------------
1326  case D4C_VOCAB_CMD_DEL:
1327  {
1328  if (cmd.size()<2)
1329  reply.addVocab(D4C_VOCAB_CMD_NACK);
1330  else
1331  {
1332  if (eraseItem(cmd.get(1).asInt()))
1333  reply.addVocab(D4C_VOCAB_CMD_ACK);
1334  else
1335  reply.addVocab(D4C_VOCAB_CMD_NACK);
1336  }
1337 
1338  break;
1339  }
1340 
1341  //-----------------
1342  case D4C_VOCAB_CMD_SET:
1343  {
1344  if (cmd.size()<3)
1345  reply.addVocab(D4C_VOCAB_CMD_NACK);
1346  else if (setProperty(cmd.get(1).asInt(),extractProperty(cmd.get(2))))
1347  reply.addVocab(D4C_VOCAB_CMD_ACK);
1348  else
1349  reply.addVocab(D4C_VOCAB_CMD_NACK);
1350 
1351  break;
1352  }
1353 
1354  //-----------------
1355  case D4C_VOCAB_CMD_GET:
1356  {
1357  if (cmd.size()<2)
1358  reply.addVocab(D4C_VOCAB_CMD_NACK);
1359  else
1360  {
1361  Property options;
1362  if (getProperty(cmd.get(1).asInt(),options))
1363  {
1364  Value val;
1365  val.fromString(("("+string(options.toString().c_str())+")").c_str());
1366 
1367  reply.addVocab(D4C_VOCAB_CMD_ACK);
1368  reply.add(val);
1369  }
1370  else
1371  reply.addVocab(D4C_VOCAB_CMD_NACK);
1372  }
1373 
1374  break;
1375  }
1376 
1377  //-----------------
1378  case D4C_VOCAB_CMD_LIST:
1379  {
1380  Bottle items;
1381  if (getItems(items))
1382  {
1383  reply.addVocab(D4C_VOCAB_CMD_ACK);
1384  reply.addList().append(items);
1385  }
1386  else
1387  reply.addVocab(D4C_VOCAB_CMD_NACK);
1388 
1389  break;
1390  }
1391 
1392  //-----------------
1393  case D4C_VOCAB_CMD_CLEAR:
1394  {
1395  if (clearItems())
1396  reply.addVocab(D4C_VOCAB_CMD_ACK);
1397  else
1398  reply.addVocab(D4C_VOCAB_CMD_NACK);
1399 
1400  break;
1401  }
1402 
1403  //-----------------
1404  case D4C_VOCAB_CMD_ENFIELD:
1405  {
1406  if (enableField())
1407  reply.addVocab(D4C_VOCAB_CMD_ACK);
1408  else
1409  reply.addVocab(D4C_VOCAB_CMD_NACK);
1410 
1411  break;
1412  }
1413 
1414  //-----------------
1415  case D4C_VOCAB_CMD_DISFIELD:
1416  {
1417  if (disableField())
1418  reply.addVocab(D4C_VOCAB_CMD_ACK);
1419  else
1420  reply.addVocab(D4C_VOCAB_CMD_NACK);
1421 
1422  break;
1423  }
1424 
1425  //-----------------
1426  case D4C_VOCAB_CMD_STATFIELD:
1427  {
1428  bool status;
1429  if (getFieldStatus(status))
1430  {
1431  reply.addVocab(D4C_VOCAB_CMD_ACK);
1432  reply.addString(status?"on":"off");
1433  }
1434  else
1435  reply.addVocab(D4C_VOCAB_CMD_NACK);
1436 
1437  break;
1438  }
1439 
1440  //-----------------
1441  case D4C_VOCAB_CMD_ENCTRL:
1442  {
1443  if (enableControl())
1444  reply.addVocab(D4C_VOCAB_CMD_ACK);
1445  else
1446  reply.addVocab(D4C_VOCAB_CMD_NACK);
1447 
1448  break;
1449  }
1450 
1451  //-----------------
1452  case D4C_VOCAB_CMD_DISCTRL:
1453  {
1454  if (disableControl())
1455  reply.addVocab(D4C_VOCAB_CMD_ACK);
1456  else
1457  reply.addVocab(D4C_VOCAB_CMD_NACK);
1458 
1459  break;
1460  }
1461 
1462  //-----------------
1463  case D4C_VOCAB_CMD_STATCTRL:
1464  {
1465  bool status;
1466  if (getControlStatus(status))
1467  {
1468  reply.addVocab(D4C_VOCAB_CMD_ACK);
1469  reply.addString(status?"on":"off");
1470  }
1471  else
1472  reply.addVocab(D4C_VOCAB_CMD_NACK);
1473 
1474  break;
1475  }
1476 
1477  //-----------------
1478  case D4C_VOCAB_CMD_ENSIM:
1479  {
1480  if (enableSimulation())
1481  reply.addVocab(D4C_VOCAB_CMD_ACK);
1482  else
1483  reply.addVocab(D4C_VOCAB_CMD_NACK);
1484 
1485  break;
1486  }
1487 
1488  //-----------------
1489  case D4C_VOCAB_CMD_DISSIM:
1490  {
1491  if (disableSimulation())
1492  reply.addVocab(D4C_VOCAB_CMD_ACK);
1493  else
1494  reply.addVocab(D4C_VOCAB_CMD_NACK);
1495 
1496  break;
1497  }
1498 
1499  //-----------------
1500  case D4C_VOCAB_CMD_STATSIM:
1501  {
1502  bool status;
1503  if (getSimulationStatus(status))
1504  {
1505  reply.addVocab(D4C_VOCAB_CMD_ACK);
1506  reply.addString(status?"on":"off");
1507  }
1508  else
1509  reply.addVocab(D4C_VOCAB_CMD_NACK);
1510 
1511  break;
1512  }
1513 
1514  //-----------------
1515  case D4C_VOCAB_CMD_SETPER:
1516  {
1517  if (cmd.size()<2)
1518  reply.addVocab(D4C_VOCAB_CMD_NACK);
1519  else if (setPeriod(cmd.get(1).asInt()))
1520  reply.addVocab(D4C_VOCAB_CMD_ACK);
1521  else
1522  reply.addVocab(D4C_VOCAB_CMD_NACK);
1523 
1524  break;
1525  }
1526 
1527  //-----------------
1528  case D4C_VOCAB_CMD_GETPER:
1529  {
1530  int period;
1531  if (getPeriod(period))
1532  {
1533  reply.addVocab(D4C_VOCAB_CMD_ACK);
1534  reply.addInt(period);
1535  }
1536  else
1537  reply.addVocab(D4C_VOCAB_CMD_NACK);
1538 
1539  break;
1540  }
1541 
1542  //-----------------
1543  case D4C_VOCAB_CMD_SETACTIF:
1544  {
1545  if (cmd.size()<2)
1546  reply.addVocab(D4C_VOCAB_CMD_NACK);
1547  else if (setActiveIF(cmd.get(1).asString().c_str()))
1548  reply.addVocab(D4C_VOCAB_CMD_ACK);
1549  else
1550  reply.addVocab(D4C_VOCAB_CMD_NACK);
1551 
1552  break;
1553  }
1554 
1555  //-----------------
1556  case D4C_VOCAB_CMD_GETACTIF:
1557  {
1558  string _activeIF;
1559  if (getActiveIF(_activeIF))
1560  {
1561  reply.addVocab(D4C_VOCAB_CMD_ACK);
1562  reply.addString(_activeIF.c_str());
1563  }
1564  else
1565  reply.addVocab(D4C_VOCAB_CMD_NACK);
1566 
1567  break;
1568  }
1569 
1570  //-----------------
1571  case D4C_VOCAB_CMD_GETTRAJ:
1572  {
1573  Property options=extractProperty(cmd.get(1));
1574  if (options.isNull())
1575  reply.addVocab(D4C_VOCAB_CMD_NACK);
1576  else
1577  {
1578  int maxIterations=options.find("maxIterations").asInt();
1579  double Ts=options.find("Ts").asDouble();
1580  deque<Vector> trajPos;
1581  deque<Vector> trajOrien;
1582  if (getTrajectory(trajPos,trajOrien,maxIterations,Ts))
1583  {
1584  reply.addVocab(D4C_VOCAB_CMD_ACK);
1585 
1586  Bottle &bPos=reply.addList();
1587  bPos.addString("trajPos");
1588  for (unsigned int i=0; i<trajPos.size(); i++)
1589  {
1590  Bottle &point=bPos.addList();
1591  for (size_t j=0; j<trajPos[i].length(); j++)
1592  point.addDouble(trajPos[i][j]);
1593  }
1594 
1595  Bottle &bOrien=reply.addList();
1596  bOrien.addString("trajOrien");
1597  for (unsigned int i=0; i<trajOrien.size(); i++)
1598  {
1599  Bottle &point=bOrien.addList();
1600  for (size_t j=0; j<trajOrien[i].length(); j++)
1601  point.addDouble(trajOrien[i][j]);
1602  }
1603  }
1604  else
1605  reply.addVocab(D4C_VOCAB_CMD_NACK);
1606  }
1607 
1608  break;
1609  }
1610 
1611  //-----------------
1612  case D4C_VOCAB_CMD_EXECTRAJ:
1613  {
1614  Bottle *options=cmd.get(1).asList();
1615  if (options->isNull())
1616  reply.addVocab(D4C_VOCAB_CMD_NACK);
1617  else
1618  {
1619  double trajTime=-1.0;
1620  deque<Vector> trajPos;
1621  deque<Vector> trajOrien;
1622 
1623  if (Bottle *BtrajTime=options->get(0).asList())
1624  if (BtrajTime->get(0).asString()=="trajTime")
1625  trajTime=BtrajTime->get(1).asDouble();
1626 
1627  bool okPos=false;
1628  if (Bottle *BtrajPos=options->get(1).asList())
1629  {
1630  if (BtrajPos->get(0).asString()=="trajPos")
1631  {
1632  for (int i=1; i<BtrajPos->size(); i++)
1633  {
1634  if (Bottle *point=BtrajPos->get(i).asList())
1635  {
1636  Vector pos(point->size());
1637  for (int j=0; j<point->size(); j++)
1638  pos[j]=point->get(j).asDouble();
1639 
1640  trajPos.push_back(pos);
1641  }
1642  }
1643 
1644  okPos=true;
1645  }
1646  }
1647 
1648  bool okOrien=false;
1649  if (Bottle *BtrajOrien=reply.get(2).asList())
1650  {
1651  if (BtrajOrien->get(0).asString()=="trajOrien")
1652  {
1653  for (int i=1; i<BtrajOrien->size(); i++)
1654  {
1655  if (Bottle *point=BtrajOrien->get(i).asList())
1656  {
1657  Vector orien(point->size());
1658  for (int j=0; j<point->size(); j++)
1659  orien[j]=point->get(j).asDouble();
1660 
1661  trajOrien.push_back(orien);
1662  }
1663  }
1664 
1665  okOrien=true;
1666  }
1667  }
1668 
1669  if (!okPos || !okOrien)
1670  reply.addVocab(D4C_VOCAB_CMD_NACK);
1671  else if (executeTrajectory(trajPos,trajOrien,trajTime))
1672  reply.addVocab(D4C_VOCAB_CMD_ACK);
1673  else
1674  reply.addVocab(D4C_VOCAB_CMD_NACK);
1675  }
1676 
1677  break;
1678  }
1679 
1680  //-----------------
1681  case D4C_VOCAB_CMD_SETSTATETOTOOL:
1682  {
1683  if (cmd.size()<1)
1684  reply.addVocab(D4C_VOCAB_CMD_NACK);
1685  else
1686  {
1687  if (setPointStateToTool())
1688  reply.addVocab(D4C_VOCAB_CMD_ACK);
1689  else
1690  reply.addVocab(D4C_VOCAB_CMD_NACK);
1691  }
1692 
1693  break;
1694  }
1695 
1696  //-----------------
1697  case D4C_VOCAB_CMD_SETSTATE:
1698  {
1699  if (cmd.size()<2)
1700  reply.addVocab(D4C_VOCAB_CMD_NACK);
1701  else
1702  {
1703  Property options=extractProperty(cmd.get(1));
1704  if (options.isNull())
1705  reply.addVocab(D4C_VOCAB_CMD_NACK);
1706  else
1707  {
1708  Vector pos,vel;
1709  if (extractVector(options,"x",pos) && extractVector(options,"xdot",vel))
1710  {
1711  if (setPointState(getVectorPos(pos),getVectorOrien(pos),
1712  getVectorPos(vel),getVectorOrien(vel)))
1713  reply.addVocab(D4C_VOCAB_CMD_ACK);
1714  else
1715  reply.addVocab(D4C_VOCAB_CMD_NACK);
1716  }
1717  else
1718  reply.addVocab(D4C_VOCAB_CMD_NACK);
1719  }
1720  }
1721 
1722  break;
1723  }
1724 
1725  //-----------------
1726  case D4C_VOCAB_CMD_SETORIEN:
1727  {
1728  if (cmd.size()<2)
1729  reply.addVocab(D4C_VOCAB_CMD_NACK);
1730  else
1731  {
1732  Property options=extractProperty(cmd.get(1));
1733  if (options.isNull())
1734  reply.addVocab(D4C_VOCAB_CMD_NACK);
1735  else
1736  {
1737  Vector o,odot;
1738  if (extractVector(options,"o",o) && extractVector(options,"odot",odot))
1739  {
1740  if (setPointOrientation(o,odot))
1741  reply.addVocab(D4C_VOCAB_CMD_ACK);
1742  else
1743  reply.addVocab(D4C_VOCAB_CMD_NACK);
1744  }
1745  else
1746  reply.addVocab(D4C_VOCAB_CMD_NACK);
1747  }
1748  }
1749 
1750  break;
1751  }
1752 
1753  //-----------------
1754  case D4C_VOCAB_CMD_GETSTATE:
1755  {
1756  if (isOpen)
1757  {
1758  mutex.lock();
1759  Property state=prepareData();
1760  mutex.unlock();
1761 
1762  Value val_state;
1763  val_state.fromString(("("+string(state.toString().c_str())+")").c_str());
1764 
1765  reply.addVocab(D4C_VOCAB_CMD_ACK);
1766  reply.add(val_state);
1767  }
1768  else
1769  reply.addVocab(D4C_VOCAB_CMD_NACK);
1770 
1771  break;
1772  }
1773 
1774  //-----------------
1775  case D4C_VOCAB_CMD_ATTACHTOOLFRAME:
1776  {
1777  if (cmd.size()<2)
1778  reply.addVocab(D4C_VOCAB_CMD_NACK);
1779  else
1780  {
1781  Vector x,o;
1782  Property options=extractProperty(cmd.get(1));
1783  extractVector(options,"x",x);
1784  extractVector(options,"o",o);
1785 
1786  if (attachToolFrame(x,o))
1787  reply.addVocab(D4C_VOCAB_CMD_ACK);
1788  else
1789  reply.addVocab(D4C_VOCAB_CMD_NACK);
1790  }
1791 
1792  break;
1793  }
1794 
1795  //-----------------
1796  case D4C_VOCAB_CMD_GETTOOLFRAME:
1797  {
1798  Vector x,o;
1799  if (getToolFrame(x,o))
1800  {
1801  Value val_x; val_x.fromString(("("+string(x.toString().c_str())+")").c_str());
1802  Value val_o; val_o.fromString(("("+string(o.toString().c_str())+")").c_str());
1803 
1804  Property options;
1805  options.put("x",val_x);
1806  options.put("o",val_o);
1807 
1808  Value val;
1809  val.fromString(("("+string(options.toString().c_str())+")").c_str());
1810 
1811  reply.addVocab(D4C_VOCAB_CMD_ACK);
1812  reply.add(val);
1813  }
1814  else
1815  reply.addVocab(D4C_VOCAB_CMD_NACK);
1816 
1817  break;
1818  }
1819 
1820  //-----------------
1821  case D4C_VOCAB_CMD_REMOVETOOLFRAME:
1822  {
1823  if (removeToolFrame())
1824  reply.addVocab(D4C_VOCAB_CMD_ACK);
1825  else
1826  reply.addVocab(D4C_VOCAB_CMD_NACK);
1827 
1828  break;
1829  }
1830 
1831  //-----------------
1832  case D4C_VOCAB_CMD_GETTOOL:
1833  {
1834  Vector x,o;
1835  if (getTool(x,o))
1836  {
1837  Value val_x; val_x.fromString(("("+string(x.toString().c_str())+")").c_str());
1838  Value val_o; val_o.fromString(("("+string(o.toString().c_str())+")").c_str());
1839 
1840  Property options;
1841  options.put("x",val_x);
1842  options.put("o",val_o);
1843 
1844  Value val;
1845  val.fromString(("("+string(options.toString().c_str())+")").c_str());
1846 
1847  reply.addVocab(D4C_VOCAB_CMD_ACK);
1848  reply.add(val);
1849  }
1850  else
1851  reply.addVocab(D4C_VOCAB_CMD_NACK);
1852 
1853  break;
1854  }
1855 
1856  //-----------------
1857  default:
1858  reply.addVocab(D4C_VOCAB_CMD_NACK);
1859  }
1860 
1861  ConnectionWriter *returnToSender=connection.getWriter();
1862  if (returnToSender!=NULL)
1863  reply.write(*returnToSender);
1864 
1865  return true;
1866 }
1867 
1868 
1869 /************************************************************************/
1870 Property D4CServer::prepareData()
1871 {
1872  Vector field;
1873  getField(field);
1874 
1875  Value val_field; val_field.fromString(("("+string(field.toString().c_str())+")").c_str());
1876  Value val_xdot; val_xdot.fromString(("("+string(xdot.toString().c_str())+")").c_str());
1877  Value val_x; val_x.fromString(("("+string(x.toString().c_str())+")").c_str());
1878  Value val_xhat; val_xhat.fromString(("("+string(xhat.toString().c_str())+")").c_str());
1879  Value val_qhat; val_qhat.fromString(("("+string(qhat.toString().c_str())+")").c_str());
1880 
1881  Property out;
1882  out.put("field",val_field);
1883  out.put("xdot",val_xdot);
1884  out.put("x",val_x);
1885  out.put("xhat",val_xhat);
1886  out.put("qhat",val_qhat);
1887 
1888  return out;
1889 }
1890 
1891 
1892 /************************************************************************/
1893 void D4CServer::getTargetForCartesianIF(Vector &pos, Vector &orien)
1894 {
1895  pos=getVectorPos(x);
1896  orien=getVectorOrien(x);
1897 
1898  Matrix frame1=axis2dcm(orien);
1899  frame1(0,3)=pos[0];
1900  frame1(1,3)=pos[1];
1901  frame1(2,3)=pos[2];
1902 
1903  Matrix frame2=frame1*invToolFrame;
1904  pos[0]=frame2(0,3);
1905  pos[1]=frame2(1,3);
1906  pos[2]=frame2(2,3);
1907  orien=dcm2axis(frame2);
1908 }
1909 
1910 
1911 /************************************************************************/
1912 bool D4CServer::getActiveIF(string &activeIF)
1913 {
1914  if (isOpen)
1915  {
1916  if (!offlineMode)
1917  {
1918  activeIF=this->activeIF;
1919  printMessage(log::no_info,1,"active interface: %s",activeIF.c_str());
1920  return true;
1921  }
1922  else
1923  {
1924  activeIF="";
1925  printMessage(log::warning,1,"no connection with the robot in offline mode");
1926  return false;
1927  }
1928  }
1929  else
1930  {
1931  printMessage(log::warning,1,"server is not open");
1932  return false;
1933  }
1934 }
1935 
1936 
1937 /************************************************************************/
1938 bool D4CServer::setActiveIF(const string &activeIF)
1939 {
1940  if (isOpen)
1941  {
1942  if (!offlineMode)
1943  {
1944  if (part=="both_arms")
1945  {
1946  if ((activeIF=="right") || (activeIF=="left"))
1947  {
1948  mutex.lock();
1949  iCtrlActive->stopControl();
1950  if (activeIF=="right")
1951  iCtrlActive=iCtrlRight;
1952  else
1953  iCtrlActive=iCtrlLeft;
1954 
1955  this->activeIF=activeIF;
1956  printMessage(log::no_info,1,"active interface successfully set to %s",
1957  activeIF.c_str());
1958 
1959  mutex.unlock();
1960  return true;
1961  }
1962  else
1963  {
1964  printMessage(log::error,1,"wrong value specified, it should be \"right\" or \"left\"");
1965  return false;
1966  }
1967  }
1968  else
1969  {
1970  printMessage(log::warning,1,"cannot swap arm");
1971  return false;
1972  }
1973  }
1974  else
1975  {
1976  printMessage(log::warning,1,"no connection with the robot in offline mode");
1977  return false;
1978  }
1979  }
1980  else
1981  {
1982  printMessage(log::warning,1,"server is not open");
1983  return false;
1984  }
1985 }
1986 
1987 
1988 /************************************************************************/
1989 void D4CServer::scheduleInitGuiTrajectory()
1990 {
1991  doInitGuiTrajectory=true;
1992 }
1993 
1994 
1995 /************************************************************************/
1996 void D4CServer::initGuiTrajectory()
1997 {
1998  if (gui.getOutputCount()>0)
1999  {
2000  Bottle &obj=gui.prepare();
2001  obj.clear();
2002 
2003  obj.addString("trajectory");
2004  obj.addString(part.c_str()); // trajectory identifier
2005  obj.addString(part.c_str()); // trajectory name
2006  obj.addInt(512); // max samples in circular queue
2007  obj.addDouble(5.0); // lifetime of samples
2008  obj.addInt(0); // col R
2009  obj.addInt(0); // col G
2010  obj.addInt(255); // col B
2011  obj.addDouble(0.5); // alpha [0,1]
2012  obj.addDouble(5.0); // line width
2013 
2014  gui.writeStrict();
2015  printMessage(log::no_info,4,"initializing gui trajectory point");
2016  }
2017 }
2018 
2019 
2020 /************************************************************************/
2021 void D4CServer::updateGuiTrajectory()
2022 {
2023  if (gui.getOutputCount()>0)
2024  {
2025  Bottle &obj=gui.prepare();
2026  obj.clear();
2027 
2028  obj.addString("addpoint");
2029  obj.addString(part.c_str()); // trajectory identifier
2030  obj.addDouble(1000.0*x[0]); // posX [mm]
2031  obj.addDouble(1000.0*x[1]); // posY [mm]
2032  obj.addDouble(1000.0*x[2]); // posZ [mm]
2033 
2034  gui.writeStrict();
2035  printMessage(log::no_info,4,"updating gui trajectory point");
2036  }
2037 }
2038 
2039 
2040 /************************************************************************/
2041 void D4CServer::eraseGuiTrajectory()
2042 {
2043  if (gui.getOutputCount()>0)
2044  {
2045  Bottle &obj=gui.prepare();
2046  obj.clear();
2047 
2048  obj.addString("delete");
2049  obj.addString(part.c_str());
2050 
2051  gui.writeStrict();
2052  printMessage(log::no_info,4,"erasing gui tracjectory point");
2053  }
2054 }
2055 
2056 
2057 /************************************************************************/
2058 void D4CServer::updateGuiItem(const GuiRequest &req)
2059 {
2060  if (gui.getOutputCount()>0)
2061  {
2062  map<int,Item*>::iterator it=req.getIter();
2063  string name=req.getName();
2064  int item=it->first;
2065  Item *pItem=it->second;
2066 
2067  Vector rpy=CTRL_RAD2DEG*dcm2rpy(axis2dcm(pItem->orientation));
2068 
2069  Bottle &obj=gui.prepare();
2070  obj.clear();
2071 
2072  obj.addString("object");
2073  obj.addString(name.c_str());
2074  obj.addDouble(1000.0*pItem->radius[0]); // dimX [mm]
2075  obj.addDouble(1000.0*pItem->radius[1]); // dimY [mm]
2076  obj.addDouble(1000.0*pItem->radius[2]); // dimZ [mm]
2077  obj.addDouble(1000.0*pItem->center[0]); // posX [mm]
2078  obj.addDouble(1000.0*pItem->center[1]); // posY [mm]
2079  obj.addDouble(1000.0*pItem->center[2]); // posZ [mm]
2080  obj.addDouble(rpy[0]); // rotX [deg]
2081  obj.addDouble(rpy[1]); // rotY [deg]
2082  obj.addDouble(rpy[2]); // rotZ [deg]
2083  obj.addInt((int)pItem->color[0]); // col R
2084  obj.addInt((int)pItem->color[1]); // col G
2085  obj.addInt((int)pItem->color[2]); // col B
2086  obj.addDouble(pItem->active?1.0:0.25); // alpha [0,1]
2087 
2088  gui.writeStrict();
2089  printMessage(log::no_info,4,"updating gui item %s",name.c_str());
2090  }
2091 }
2092 
2093 
2094 /************************************************************************/
2095 void D4CServer::eraseGuiItem(const GuiRequest &req)
2096 {
2097  if (gui.getOutputCount()>0)
2098  {
2099  Bottle &obj=gui.prepare();
2100  obj.clear();
2101 
2102  obj.addString("delete");
2103  obj.addString(req.getName().c_str());
2104 
2105  gui.write();
2106  printMessage(log::no_info,1,"erasing gui item %s",name.c_str());
2107  }
2108 }
2109 
2110 
2111 /************************************************************************/
2112 void D4CServer::pushUpdateGuiItem(map<int,Item*>::iterator &it)
2113 {
2114  GuiRequest update("update",it);
2115 
2116  for (unsigned int i=0; i<guiQueue.size(); i++)
2117  {
2118  if (guiQueue[i]==update)
2119  {
2120  printMessage(log::warning,5,"gui item %d update is already scheduled",it->first);
2121  return;
2122  }
2123  }
2124 
2125  guiQueue.push_back(update);
2126  printMessage(log::no_info,5,"scheduled request for gui item %d update",it->first);
2127 }
2128 
2129 
2130 /************************************************************************/
2131 void D4CServer::pushEraseGuiItem(map<int,Item*>::iterator &it)
2132 {
2133  int item=it->first;
2134  GuiRequest update("update",it);
2135 
2136  for (unsigned int i=0; i<guiQueue.size(); i++)
2137  {
2138  if (guiQueue[i]==update)
2139  {
2140  guiQueue.erase(guiQueue.begin()+i);
2141  printMessage(log::no_info,5,"remove schedule for gui item %d update",item);
2142  break;
2143  }
2144  }
2145 
2146  guiQueue.push_back(GuiRequest("erase",it));
2147  printMessage(log::no_info,5,"scheduled request for gui item %d erase",item);
2148 }
2149 
2150 
2151 /************************************************************************/
2152 void D4CServer::handleGuiQueue()
2153 {
2154  if (doInitGuiTrajectory)
2155  {
2156  initGuiTrajectory();
2157  doInitGuiTrajectory=false;
2158  }
2159  // doTrajectoryCnt serves to avoid updating continuosly
2160  // the items and delaying the trajectory too much
2161  else if (guiQueue.size() && (doTrajectoryCnt<3))
2162  {
2163  GuiRequest guiReq=guiQueue.front();
2164  guiQueue.pop_front();
2165 
2166  if (guiReq.getType()=="update")
2167  updateGuiItem(guiReq);
2168  else if (guiReq.getType()=="erase")
2169  eraseGuiItem(guiReq);
2170  else
2171  printMessage(log::warning,1,"unknown gui request received!");
2172 
2173  doTrajectoryCnt++;
2174  }
2175  else
2176  {
2177  updateGuiTrajectory();
2178  doTrajectoryCnt=0;
2179  }
2180 }
2181 
2182 
2183 /************************************************************************/
2184 bool D4CServer::getTrajectory(deque<Vector> &trajPos, deque<Vector> &trajOrien,
2185  const unsigned int maxIterations, const double Ts)
2186 {
2187  if (isOpen)
2188  {
2189  mutex.lock();
2190  printMessage(log::no_info,1,"request for trajectory simulation");
2191  Vector xdotOffline=xdot;
2192  Vector xOffline=x;
2193 
2194  double _Ts=(Ts<=D4C_DEFAULT_TS_DISABLED)?(double)period/1000.0:Ts;
2195  Integrator IfOffline(_Ts,xdotOffline);
2196  Integrator IvOffline(_Ts,xOffline);
2197 
2198  unsigned int iteration=0;
2199  while (iteration++<maxIterations)
2200  {
2201  Vector field(x.length(),0.0);
2202  for (map<int,Item*>::const_iterator it=table.begin(); it!=table.end(); it++)
2203  field=field+it->second->getField(xOffline,xdotOffline);
2204 
2205  xdotOffline=IfOffline.integrate(field);
2206  xOffline=IvOffline.integrate(xdotOffline);
2207 
2208  trajPos.push_back(getVectorPos(xOffline));
2209  trajOrien.push_back(getVectorOrien(xOffline));
2210  }
2211 
2212  mutex.unlock();
2213  return true;
2214  }
2215  else
2216  {
2217  printMessage(log::warning,1,"server is not open");
2218  return false;
2219  }
2220 }
2221 
2222 
2223 /************************************************************************/
2224 bool D4CServer::executeTrajectory(const deque<Vector> &trajPos, const deque<Vector> &trajOrien,
2225  const double trajTime)
2226 {
2227  if (isOpen)
2228  {
2229  printMessage(log::no_info,1,"request for trajectory execution");
2230 
2231  if (trajPos.size()!=trajOrien.size())
2232  {
2233  printMessage(log::error,1,"position and orientation data have different size!");
2234  return false;
2235  }
2236  else if (trajTime<0.0)
2237  {
2238  printMessage(log::error,1,"negative trajectory duration provided!");
2239  return false;
2240  }
2241 
2242  // define the tracker thread
2243  class TrackerThread : public RateThread
2244  {
2245  ICartesianControl *ctrl;
2246  const deque<Vector> *trajPos;
2247  const deque<Vector> *trajOrien;
2248  double trajTime;
2249  double t0,t;
2250  int pos,N;
2251  bool threadInit()
2252  {
2253  t=0.0;
2254  t0=Time::now();
2255  return true;
2256  }
2257  void run()
2258  {
2259  t=Time::now()-t0;
2260  int i=std::min(N,(int)((t*N)/trajTime));
2261  if (i>pos)
2262  {
2263  ctrl->goToPose(trajPos->at(i),trajOrien->at(i));
2264  pos=i;
2265  }
2266  }
2267  void threadRelease()
2268  {
2269  // make sure we attempt to reach the last point
2270  ctrl->goToPoseSync(trajPos->back(),trajOrien->back());
2271  ctrl->waitMotionDone();
2272  }
2273  public:
2274  TrackerThread() : RateThread(20), pos(-1) { }
2275  void setInfo(ICartesianControl *ctrl, const deque<Vector> *trajPos,
2276  const deque<Vector> *trajOrien, const double trajTime)
2277  {
2278  this->trajPos=trajPos;
2279  this->trajOrien=trajOrien;
2280  this->trajTime=trajTime;
2281  N=(int)(trajPos->size()-1);
2282  }
2283  void wait()
2284  {
2285  while(t<trajTime)
2286  Time::delay(0.1);
2287  }
2288  } trackerThread;
2289 
2290  trackerThread.setRate((int)getRate());
2291  trackerThread.setInfo(iCtrlActive,&trajPos,&trajOrien,trajTime);
2292 
2293  // lock the main run
2294  mutex.lock();
2295 
2296  // run the tracker
2297  trackerThread.start();
2298  trackerThread.wait();
2299  trackerThread.stop();
2300 
2301  // unlock the main run
2302  mutex.unlock();
2303 
2304  return true;
2305  }
2306  else
2307  {
2308  printMessage(log::warning,1,"server is not open");
2309  return false;
2310  }
2311 }
2312 
2313 
2314 /************************************************************************/
2315 void D4CServer::run()
2316 {
2317  mutex.lock();
2318 
2319  if (!offlineMode)
2320  {
2321  if (fieldEnabled)
2322  {
2323  printMessage(log::no_info,4,"processing %d items",table.size());
2324 
2325  Vector field;
2326  getField(field);
2327 
2328  xdot=If.integrate(field);
2329  x=Iv.integrate(xdot);
2330 
2331  if (controlEnabled)
2332  {
2333  Vector pos,orien;
2334  getTargetForCartesianIF(pos,orien);
2335  iCtrlActive->goToPose(pos,orien);
2336  }
2337 
2338  if (simulationEnabled)
2339  {
2340  Vector pos,orien,xdhat,odhat;
2341  getTargetForCartesianIF(pos,orien);
2342 
2343  if (simulationFirstStep)
2344  {
2345  iCtrlActive->askForPose(pos,orien,xdhat,odhat,qhat);
2346  simulationFirstStep=false;
2347  }
2348  else
2349  {
2350  Vector dof;
2351  iCtrlActive->getDOF(dof);
2352 
2353  Vector q0;
2354  for (size_t i=0; i<dof.length(); i++)
2355  if (dof[i]>0.0)
2356  q0.push_back(qhat[i]);
2357 
2358  iCtrlActive->askForPose(q0,pos,orien,xdhat,odhat,qhat);
2359  }
2360 
2361  copyVectorData(xdhat,xhat);
2362  copyVectorData(odhat,xhat);
2363  }
2364  }
2365  }
2366 
2367  if (data.getOutputCount()>0)
2368  {
2369  data.prepare()=prepareData();
2370  data.write();
2371  }
2372 
2373  // avoid running faster than what is necessary
2374  if (Time::now()-t0>0.03)
2375  {
2376  handleGuiQueue();
2377  t0=Time::now();
2378  }
2379 
2380  mutex.unlock();
2381 }
2382 
2383 
iCub::d4c
Definition: d4c.h:46
Generated on Sun May 31 2015 15:17:45 for d4c by   doxygen 1.8.9