iCub-main
utils.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/Time.h>
21 #include <yarp/os/Log.h>
22 #include <yarp/os/LogStream.h>
23 #include <yarp/os/BufferedPort.h>
24 #include <yarp/sig/Vector.h>
25 #include <yarp/math/Math.h>
26 
27 #include <yarp/dev/ControlBoardInterfaces.h>
28 #include <yarp/dev/PolyDriver.h>
29 #include <yarp/os/PeriodicThread.h>
30 #include <yarp/os/Thread.h>
31 
32 #include <fstream>
33 #include <iostream>
34 #include <iomanip>
35 #include <string>
36 #include <vector>
37 #include <deque>
38 
39 using namespace std;
40 using namespace yarp::os;
41 using namespace yarp::sig;
42 using namespace yarp::dev;
43 using namespace yarp::math;
44 
45 #include "utils.h"
46 
47 // ******************** ACTION CLASS
48 
50 {
51  return N_JOINTS;
52 }
53 
55 {
56  N_JOINTS = n;
57  q_joints = new double [N_JOINTS];
58  for (int i=0; i<N_JOINTS; i++) q_joints[i]=0.0;
59  tag = "UNKNOWN";
60 }
61 
63 {
64  N_JOINTS = as.N_JOINTS;
65  counter = as.counter;
66  time = as.time;
67  tag = as.tag;
68  q_joints = new double [N_JOINTS];
69  for (int i=0; i< N_JOINTS; i++)
70  {
71  q_joints[i]=as.q_joints[i];
72  }
73 }
74 
76 {
77  if(this == &as)
78  return *this;
79  N_JOINTS = as.N_JOINTS;
80  counter = as.counter;
81  time = as.time;
82  tag = as.tag;
83  q_joints = new double [N_JOINTS];
84  for (int i=0; i< N_JOINTS; i++)
85  {
86  q_joints[i]=as.q_joints[i];
87  }
88  return *this;
89 }
90 
92 {
93  if (q_joints)
94  {
95  delete []q_joints;
96  q_joints = 0;
97  }
98 }
99 
101 {
102  forever = false;
103  current_action = 0;
104  current_status = ACTION_IDLE;
105  action_vector.clear();
106 }
107 
109 {
110  clear();
111 }
112 
114 {
115  for (action_it=action_vector.begin(); action_it<action_vector.end(); action_it++)
116  {
117  yInfo ("%d %f ",action_it->counter,action_it->time);
118  for (int i=0; i< action_it->get_n_joints(); i++)
119  yInfo("%f ", action_it->q_joints[i]);
120  yInfo ("\n");
121  }
122 }
123 
124 bool action_class::openFile(string filename, int n_joints)
125 {
126  bool ret = true;
127  FILE* data_file = 0;
128  data_file = fopen(filename.c_str(),"r");
129  if (data_file!=NULL)
130  {
131  char* bb = 0;
132  int line =0;
133  do
134  {
135  char command_line[1024];
136  bb = fgets (command_line, 1024, data_file);
137  if (bb == 0) break;
138  //if(!parseCommandLine(command_line, line++))
139  // if(!parseCommandLineFixTime(command_line, line++, 1.0/25.0)) //25hz
140  if(!parseCommandLineFixTime(command_line, line++, 1.0/50.0, n_joints)) //50hz
141  {
142  yError ("error parsing file, line %d\n", line);
143  ret = false;
144  break;
145  };
146  }
147  while (1);
148 
149  fclose (data_file);
150  }
151  else
152  {
153  yError("unable to find file\n");
154  ret = false;
155  }
156  return ret;
157 }
158 
159 bool action_class::parseCommandLineFixTime(const char* command_line, int line, double fixTime, int n_joints)
160 {
161  static int count = 0;
162  static double time =0.0;
163  action_struct tmp_action(n_joints);
164 
165  tmp_action.counter = count; count = count + 1;
166  tmp_action.time = time; time = time+fixTime;
167 
168  char* tok;
169  size_t ss = strlen(command_line);
170  char* cline = new char [ss];
171  strcpy(cline,command_line);
172  tok = strtok (cline," ");
173  int i=0;
174  int joints_count = 0;
175  while (tok != 0 && joints_count < n_joints)
176  {
177  sscanf(tok, "%lf", &tmp_action.q_joints[i++]);
178  tok = strtok (NULL," ");
179  joints_count++;
180  }
181 
182  //insertion in the vector based on the timestamp
183  /*for (action_it=action_vector.begin(); action_it<action_vector.end(); action_it++)
184  {
185  if (tmp_action.time > action_it->time) break;
186  }
187  action_vector.insert(action_it,tmp_action);*/
188 
189  if (action_vector.size()==0)
190  {
191  action_vector.push_back(tmp_action);
192  return true;
193  }
194 
195  for (action_it=action_vector.end()-1; action_it>=action_vector.begin(); action_it--)
196  {
197  if (tmp_action.time > action_it->time) break;
198  }
199  action_vector.insert(action_it+1,tmp_action);
200  return true;
201 }
202 
203 bool action_class::parseCommandLine(const char* command_line, int line, int n_joints)
204 {
205  action_struct tmp_action(n_joints);
206  //use strtok for runtime-defined number of entries
207  char command_line_format [1000];
208  sprintf(command_line_format, "%%d %%lf ");
209  for (int i = 0; i < n_joints; i++)
210  {
211  size_t off = strlen(command_line_format);
212  sprintf(command_line_format+off, "%%lf ");
213  }
214 
215  int ret = sscanf(command_line, "%d %lf %lf %lf %lf %lf %lf %lf",
216  &tmp_action.counter,
217  &tmp_action.time,
218 
219  &tmp_action.q_joints[0],
220  &tmp_action.q_joints[1],
221  &tmp_action.q_joints[2],
222  &tmp_action.q_joints[3],
223  &tmp_action.q_joints[4],
224  &tmp_action.q_joints[5]
225  );
226 
227  if (ret == n_joints+2)
228  {
229  //insertion in the vector based on the timestamp
230  for (action_it=action_vector.begin(); action_it<action_vector.end(); action_it++)
231  if (tmp_action.time > action_it->time) break;
232  action_vector.insert(action_it,tmp_action);
233  return true;
234  }
235 
236  return false;
237 }
238 
239 
240 // ******************** ROBOT DRIVER CLASS
242 {
243  drvOptions_ll.clear();
244  drv_ll = 0;
245  ipos_ll = 0;
246  iposdir_ll = 0;
247  ipid_ll = 0;
248  icmd_ll = 0;
249  ienc_ll = 0;
250  n_joints = 0;
251 
252  verbose=1;
253  drv_connected=false;
254 }
255 
256 bool robotDriver::configure(const Property &copt)
257 {
258  bool ret=true;
259  Property &options=const_cast<Property &> (copt);
260 
261  drvOptions_ll.put("device","remote_controlboard");
262 
263  if (!options.check("robot")) { yError() << "Missing robot parameter"; return false; }
264  if (!options.check("part")) { yError() << "Missing part parameter"; return false; }
265 
266  string remote_ll;
267  string local_ll;
268  remote_ll = string("/") + string(options.find("robot").asString()) + string("/") +string(options.find("part").asString());
269  local_ll = string("/") + string("trajectoryPlayer") + string("/") + string(options.find("part").asString());
270  drvOptions_ll.put("remote",remote_ll.c_str());
271  drvOptions_ll.put("local",local_ll.c_str());
272 
273  if (verbose)
274  {
275  yDebug() << "driver options:\n" << drvOptions_ll.toString().c_str();
276  }
277 
278  return ret;
279 }
280 
282 {
283  drv_ll=new PolyDriver(drvOptions_ll);
284 
285  if (drv_ll->isValid())
286  drv_connected = drv_ll->view(ipos_ll) && drv_ll->view(iposdir_ll) && drv_ll->view(ienc_ll) && drv_ll->view(ipid_ll) && drv_ll->view(imotenc_ll) && drv_ll->view(icmd_ll);
287  else
288  drv_connected=false;
289 
290  if (!drv_connected)
291  {
292  if (drv_ll)
293  {
294  delete drv_ll;
295  drv_ll=0;
296  }
297  return false;
298  }
299 
300  //get the number of the joints
301  ienc_ll->getAxes(&n_joints);
302 
303  //set the initial reference speeds
304  double* speeds = new double [n_joints];
305  for (int i=0; i<n_joints; i++) speeds[i] = 20.0;
306  ipos_ll->setRefSpeeds(speeds);
307 
308  //dbg_connected is optional, so it has not to be returned
309  return drv_connected;
310 }
311 
313 {
314  if (drv_ll)
315  {
316  delete drv_ll;
317  drv_ll=0;
318  }
319 }
320 
321 bool robotDriver::setControlMode(const int j, const int mode)
322 {
323  if (!icmd_ll) return false;
324  return icmd_ll->setControlMode(joints_map[j], mode);
325 }
326 
327 bool robotDriver::setPosition(int j, double ref)
328 {
329  if (!iposdir_ll) return false;
330  return iposdir_ll->setPosition(joints_map[j], ref);
331 }
332 
333 bool robotDriver::getEncoder(int j, double *v)
334 {
335  if (!ienc_ll) return false;
336  return ienc_ll->getEncoder(joints_map[j], v);
337 }
338 
339 bool robotDriver::positionMove(int j, double v)
340 {
341  if (!ipos_ll) return false;
342  return ipos_ll->positionMove(joints_map[j], v);
343 }
bool setControlMode(const int j, const int mode)
Definition: utils.cpp:321
bool init()
Definition: utils.cpp:281
bool parseCommandLine(const char *command_line, int line, int n_joints)
Definition: utils.cpp:203
action_struct & operator=(const action_struct &as)
Definition: utils.cpp:75
robotDriver()
Definition: utils.cpp:241
~action_struct()
Definition: utils.cpp:91
degrees time
Definition: sine.m:5
bool getEncoder(int j, double *v)
Definition: utils.cpp:333
STL namespace.
string tag
Definition: utils.h:68
int n
action_struct(int n)
Definition: utils.cpp:54
bool configure(const Property &copt)
Definition: utils.cpp:256
int get_n_joints()
Definition: utils.cpp:49
bool setPosition(int j, double ref)
Definition: utils.cpp:327
static int v
Definition: iCub_Sim.cpp:42
~robotDriver()
Definition: utils.cpp:312
void clear()
Definition: utils.cpp:100
double time
Definition: utils.h:66
double * q_joints
Definition: utils.h:67
bool openFile(string filename, int n_joints)
Definition: utils.cpp:124
bool positionMove(int j, double ref)
Definition: utils.cpp:339
#define ACTION_IDLE
Definition: utils.h:54
int counter
Definition: utils.h:65
fclose(fileID)
void print()
Definition: utils.cpp:113
GLenum mode
Definition: rendering.cpp:48
action_class()
Definition: utils.cpp:108
bool parseCommandLineFixTime(const char *command_line, int line, double fixTime, int n_joints)
Definition: utils.cpp:159