iCub-main
main.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 // Copyright (C) 2008 RobotCub Consortium
3 // Author: Lorenzo Natale
4 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
5 
110 #include <stdio.h>
111 
112 #include <yarp/os/Network.h>
113 #include <yarp/os/Property.h>
114 #include <yarp/os/Time.h>
115 #include <yarp/dev/PolyDriver.h>
116 #include <yarp/os/BufferedPort.h>
117 #include <yarp/os/RFModule.h>
118 #include "velControlThread.h"
119 #include <yarp/os/Log.h>
120 #include <yarp/os/LogStream.h>
121 #include <string.h>
122 
123 /*default rate for the control loop*/
124 const int CONTROL_RATE=20;
125 
126 using namespace yarp::os;
127 using namespace yarp::dev;
128 
129 class VelControlModule: public RFModule {
130 private:
131  PolyDriver driver;
132  velControlThread *vc;
133  char partName[255];
134 
135  //added by ludovic
136  Port input_port;
138 public:
139 
140  virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
141  {
142  reply.clear();
143  yDebug("receiving command from port\n");
144  int index = 0;
145  int cmdSize = command.size();
146 
147  while(cmdSize>0)
148  {
149  switch(command.get(index).asVocab32())
150  {
151  case yarp::os::createVocab32('s','u','s','p'):
152  reply.addVocab32("ack");
153  vc->halt();
154  cmdSize--;
155  index++;
156  break;
157  case yarp::os::createVocab32('r','u','n'):
158  reply.addVocab32("ack");
159  vc->go();
160  cmdSize--;
161  index++;
162  break;
163  //this set current position reference
164  case yarp::os::createVocab32('s','e','t'):
165  if (command.size()>=3)
166  {
167  int i=command.get(index+1).asInt32();
168  double pos=command.get(index+2).asFloat64();
169  vc->setRef(i, pos);
170  index +=3;
171  cmdSize-=3;
172  }
173  else
174  {
175  cmdSize--;
176  index++;
177  yError("Invalid set message, ignoring\n");
178  }
179  reply.addVocab32("ack");
180  break;
181  //this set maximum velocity (limiter)
182  case yarp::os::createVocab32('s','v','e','l'):
183  if(command.size()>=3)
184  {
185  int i=command.get(index+1).asInt32();
186  double vel = command.get(index+2).asFloat64();
187  vc->setVel(i,vel);
188  index += 3;
189  cmdSize-=3;;
190  reply.addVocab32("ack");
191  }
192  else
193  {
194  cmdSize--;
195  index++;
196  yError("Invalid set vel message, ignoring\n");
197  reply.addVocab32("fail");
198  }
199  break;
200  case yarp::os::createVocab32('g','a','i','n'):
201  if(command.size()>=3)
202  {
203  int i=command.get(index+1).asInt32();
204  double gain = command.get(index+2).asFloat64();
205  vc->setGain(i,gain);
206  index+=3;
207  cmdSize-=3;
208  reply.addVocab32("ack");
209  }
210  else
211  {
212  cmdSize--;
213  index++;
214  yError("Invalid set gain message, ignoring\n");
215  reply.addVocab32("fail");
216  }
217  break;
218  case yarp::os::createVocab32('h','e','l','p'):
219  fprintf(stdout,"VelocityControl module, valid commands are:\n");
220  fprintf(stdout,"- [susp] suspend the controller (command zero velocity)\n");
221  fprintf(stdout,"- [run] start (and resume after being suspended) the controller\n");
222  fprintf(stdout,"- [quit] quit the module (exit)\n");
223  fprintf(stdout,"- [set] <j> <p> move joint j to p (degrees)\n");
224  fprintf(stdout,"- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
225  fprintf(stdout,"- [gain] <j> <k> set P gain for joint j to k\n");
226  fprintf(stdout,"- [help] to get this help\n");
227  fprintf(stdout,"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
228 
229  reply.addVocab32("many");
230  reply.addVocab32("ack");
231  reply.addString("VelocityControl module, valid commands are:");
232  reply.addString("- [susp] suspend the controller (command zero velocity)");
233  reply.addString("- [run] start (and resume after being suspended) the controller");
234  reply.addString("- [quit] quit the module (exit)");
235  reply.addString("- [set] <j> <p> move joint j to p (degrees)");
236  reply.addString("- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
237  reply.addString("- [gain] <j> <k> set P gain for joint j to k");
238  reply.addString("- [help] to get this help");
239  reply.addString("\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
240  break;
241  default:
242  yError("Invalid command, ignoring\n");
243  reply.addVocab32("fail");
244  //cmdSize--;
245  //index++;
246  //return respond(command, reply); // call default
247  break;
248  }
249  return true;
250  }
251 
252  return false;
253  }
254 
255  virtual bool configure(yarp::os::ResourceFinder &rf)
256  {
257  Property options;
258  options.fromString(rf.toString());
259  char robotName[255];
260  options.put("device", "remote_controlboard");
261  if(options.check("robot"))
262  strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
263  else
264  strncpy(robotName, "icub", sizeof(robotName));
265 
266  if(options.check("part"))
267  {
268  char tmp[800];
269  snprintf(tmp, sizeof(tmp), "/%s/vc/%s/client",
270  robotName,
271  options.find("part").asString().c_str());
272  options.put("local",tmp);
273 
274  snprintf(tmp, sizeof(tmp),"/%s/%s",
275  robotName,
276  options.find("part").asString().c_str());
277  options.put("remote", tmp);
278 
279  snprintf(tmp, sizeof(tmp), "/%s/vc/%s/input",
280  robotName,
281  options.find("part").asString().c_str());
282  input_port.open(tmp);
283 
284  options.put("carrier", "mcast");
285 
286  attach(input_port);
287  }
288  else
289  {
290  yError("Please specify part (e.g. --part head)\n");
291  return false;
292  }
294 
295  if (!driver.open(options))
296  {
297  yError("Error opening device, check parameters\n");
298  return false;
299  }
300 
302  int period = CONTROL_RATE;
303  if(options.check("period"))
304  period = options.find("period").asInt32();
305 
306  yInfo("control rate is %d ms",period);
307 
308  if (!options.check("part"))
309  return false;
310 
311  snprintf(partName, sizeof(partName), "%s", options.find("part").asString().c_str());
312 
313  vc=new velControlThread(period);
314  vc->init(&driver, partName,
315  robotName);
316 
317  vc->start();
318  return true;
319  }
320 
321  virtual bool close()
322  {
323  yInfo("Closing module [%s]\n", partName);
324  vc->stop();
325  vc->halt();
326  delete vc;
327  yInfo("Thead [%s] stopped\n", partName);
328 
329  driver.close();
330  input_port.close();
331 
332  yInfo("Module [%s] closed\n", partName);
333  return true;
334  }
335 
337  {
338  return true;
339  }
340 
341 };
342 
343 int main(int argc, char *argv[])
344 {
345  Network yarp;
346  VelControlModule mod;
347  ResourceFinder rf;
348 
349  rf.configure(argc, argv);
350  if (mod.configure(rf)==false) return 1;
351  mod.runModule();
352  yInfo("Main returning\n");
353  return 0;
354 }
355 
velControlThread::setVel
void setVel(int i, double vel)
Definition: velControlThread.cpp:303
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
velControlThread::go
void go()
Definition: velControlThread.cpp:276
velControlThread::setGain
void setGain(int i, double gain)
Definition: velControlThread.cpp:316
VelControlModule::respond
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: main.cpp:140
pos
const dReal * pos
Definition: iCub_Sim.cpp:62
main
int main(int argc, char *argv[])
Definition: main.cpp:31
VelControlModule::updateModule
bool updateModule()
Definition: main.cpp:336
VelControlModule
Definition: main.cpp:28
velControlThread::halt
void halt()
Definition: velControlThread.cpp:267
yarp::dev
Definition: DebugInterfaces.h:52
VelControlModule::close
virtual bool close()
Definition: main.cpp:321
velControlThread::init
bool init(yarp::dev::PolyDriver *d, std::string partName, std::string robotName)
Definition: velControlThread.cpp:191
velControlThread
Definition: velControlThread.h:21
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
scripting.argc
argc
Definition: scripting.py:184
fprintf
fprintf(fid,'\n')
CONTROL_RATE
const int CONTROL_RATE
Definition: main.cpp:124
velControlThread::setRef
void setRef(int i, double pos)
Definition: velControlThread.cpp:294
velControlThread.h
VelControlModule::configure
virtual bool configure(yarp::os::ResourceFinder &rf)
Definition: main.cpp:255