iCub-main
velControlThread.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 // Copyright (C) 2008 RobotCub Consortium
4 // Author: Lorenzo Natale
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
7 #include "velControlThread.h"
8 #include <string.h>
9 #include <string>
10 #include <math.h>
11 #include <yarp/os/Log.h>
12 #include <yarp/os/LogStream.h>
13 
14 using namespace yarp::dev;
15 using namespace yarp::os;
16 using namespace yarp::sig;
17 
18 const double MAX_SPEED=250;
19 const double AVERAGE=50;
20 const double ACCELERATIONS=1000000;
21 const double MAX_GAIN = 10.0;
22 
23 const int VELOCITY_INDEX_OFFSET=1000;
24 
26  yarp::os::PeriodicThread((double)rate/1000.0)
27 {
28  control_rate = rate;
29  suspended = true;
30 }
31 
33 {}
34 
36 {
37  double t_start = yarp::os::Time::now();
38 
39  if (getIterations()>100)
40  {
41  yDebug("Thread ran %d times, est period %lf[ms], used %lf[ms]\n",
42  getIterations(),
43  1000.0*getEstimatedPeriod(),
44  1000.0*getEstimatedUsed());
45  resetStat();
46  }
47  std::lock_guard<std::mutex> lck(_mutex);
48 
50  //this command receives also feedforward velocities
51  yarp::os::Bottle *bot = command_port.read(false);
52  if(bot!=NULL)
53  {
54  nb_void_loops = 0;
55  //fprintf(stderr, "\n Receiving command: \t");
56  int size = bot->size()/2;
57  for(int i=0;i<size;i++)
58  {
59  int ind = bot->get(2*i).asInt32();
60  if(ind < VELOCITY_INDEX_OFFSET)
61  {//this is a position command
62  targets(ind) = bot->get(2*i+1).asFloat64();
63  }
64  else
65  {//this is a velocity command
66  ffVelocities(ind - VELOCITY_INDEX_OFFSET) = bot->get(2*i+1).asFloat64();
67  }
68  //fprintf(stderr, "for joint *%d, received %f, \t", ind, targets(ind));
69  }
70  } else {
71  nb_void_loops++;
72  if(nb_void_loops > 5) {
73  ffVelocities = 0.0;
74  }
75  }
76 
77  //getting commands from the slow port
78  yarp::os::Bottle *vec = command_port2.read(false);
79  if (vec!=NULL)
80  {
81  for(int k=0;k<nJoints;k++)
82  targets(k)=vec->get(k).asFloat64();
83  }
84 
85  static int count=0;
86  count++;
87 
88  ienc->getEncoders(encoders.data());
89 
90  //ienc->getEncoderSpeeds(encoders_speed.data());
91  //fprintf(stderr, "printing to file \n");
92 #if 0
93  for(int i=0;i<nJoints;i++)
94  fprintf(currentSpeedFile,"%f ",encoders(i));
95  fprintf(currentSpeedFile,"%f\n",t_start-time_watch);
96 #endif
97 
98  Kd=0.0;
99 
100  for(int k=0;k<nJoints;k++)
101  {
102  // printf ("%+6.3f ", targets(k));
103  double current_err = targets(k)-encoders(k);
104  error_d(k) = (current_err - error(k))/((double)control_rate)*1000.0;
105  error(k) = current_err;
106 
107  //we calculate the command Adding the ffVelocities
108  command(k) = Kp(k)*error(k) + Kd(k)*error_d(k) + ffVelocities(k);
109  //printf ("%+3.3f ", command(k));
110  }
111  //printf ("\n");
112  // std::cout << command.toString() << std::endl;
113  // printf ("\n");
114  limitSpeed(command);
115 
116  if (suspended)
117  command=0;
118 
119 #if 0
120  for(int i=0;i<nJoints;i++)
121  fprintf(targetSpeedFile,"%f ",command(i));
122  fprintf(targetSpeedFile,"%f\n",t_start-time_watch);
123 #endif
124  //printf ("%s\n", command.toString().c_str());
125  if(!suspended)
126  {
127  if (0)
128  {
129  //@@@this is used to directly command positions, with no minum jerk tajectory
130  //not to be used (risky!), unless you know wahat you are doing.
131  //ipid->setReferences(this->targets.data());
132  for(int i=0;i<nJoints;i++)
133  {
134  if (fabs(targets(i)-encoders(i))>6.0)
135  ipos->positionMove(i,targets(i));
136  else
137  ipid->setPidReference(yarp::dev::VOCAB_PIDTYPE_POSITION,i,targets(i));
138  }
139  }
140  else
141  {
142  int trials = 0;
143  while(!ivel->velocityMove(command.data()))
144  {
145  trials++;
146  yError("velcontrol ERROR>> velocity move sent false\n");
147  if(trials>10)
148  {
149  yError("velcontrol ERROR>> tried 10 times to velocityMove, halting...\n");
150  this->halt();
151  break;
152  }
153  }
154  }
155  }
156 }
157 
159 {
160  suspended=true;
161  ienc->getEncoders(encoders.data());
162  //ienc->getEncoderSpeeds(encoders_speed.data());
163  targets=encoders;
164  ffVelocities = 0;
165  count = 0;
166  time_watch = Time::now();
167  time_loop = 0.0;
168 
169  return true;
170 }
171 
173 {
174  for(int k=0;k<nJoints;k++)
175  {
176  ivel->stop();
177  for (int k = 0; k < nJoints; k++)
178  imod->setControlMode(k, VOCAB_CM_POSITION);
179  suspended = true;
180  }
181 
182  command_port.close();
183  command_port2.close();
184 
185 #if 0
186  fclose(targetSpeedFile);
187  fclose(currentSpeedFile);
188 #endif
189 }
190 
191 bool velControlThread::init(PolyDriver *d, std::string partName, std::string robotName)
192 {
193  char tmp[255];
194 
195  nb_void_loops = 0;
196 
198  sprintf(tmp,"/%s/vc/%s/fastCommand", robotName.c_str(), partName.c_str());
199  yInfo("opening port for part %s\n",tmp);
200  command_port.open(tmp);
201 
202  std::string tmp2;
203  tmp2="/";
204  tmp2+=robotName.c_str();
205  tmp2+="/vc/";
206  tmp2+=partName.c_str();
207  tmp2+="/command";
208  command_port2.open(tmp2.c_str());
209 
210  if (d==0)
211  return false;
212 
213  driver=d;
214 
215  driver->view(ivel);
216  driver->view(ipos);
217  driver->view(ienc);
218  driver->view(imod);
219  driver->view(ipid);
220 
221  if ( (ivel==0)||(ienc==0) || (imod==0) || (ipid==0) || (ipos==0))
222  return false;
223 
224  ivel->getAxes(&nJoints);
225  yInfo("controlling %d DOFs\n",nJoints);
226 
227  Vector accs;
228  accs.resize(nJoints);
229  Vector speeds;
230  speeds.resize(nJoints);
231  ivel->getRefAccelerations(accs.data());
232  accs=ACCELERATIONS;
233  ivel->setRefAccelerations(accs.data());
234  speeds=50.0;
235  ipos->setRefSpeeds(speeds.data());
236 
237  encoders.resize(nJoints);
238  encoders_speed.resize(nJoints);
239  command.resize(nJoints);
240  targets.resize(nJoints);
241  ffVelocities.resize(nJoints);
242  command=0;
243  targets=0;
244  ffVelocities = 0;
245  Kp.resize(nJoints);
246  Kp=0;
247  Kd.resize(nJoints);
248  Kd=0;
249  error.resize(nJoints);
250  error=0;
251  error_d.resize(nJoints);
252  error_d=0;
253 
254  maxVel.resize(nJoints);
255  maxVel = 0.0;
256 
257 #if 0
258  sprintf(tmp,"%s_target_speed.dat",partName.c_str());
259  targetSpeedFile = fopen(tmp,"w");
260  sprintf(tmp,"%s_current_speed.dat",partName.c_str());
261  currentSpeedFile = fopen(tmp,"w");
262 #endif
263 
264  return true;
265 }
266 
268 {
269  suspended=true;
270  ivel->stop();
271  yInfo("Suspended\n");
272  targets=encoders;
273  ffVelocities = 0;
274 }
275 
277 {
278  suspended=false;
279  int mode=0;
280  for(int k = 0; k < nJoints; k++)
281  {
282  imod->getControlMode(k, &mode);
283  if (mode!=VOCAB_CM_MIXED && mode!=VOCAB_CM_VELOCITY)
284  {
285  std::string s = yarp::os::Vocab32::decode(mode);
286  yWarning("Joint (%d) is in mode (%s) and does not accepts velocty commands. You have first to set either VOCAB_CM_VELOCITY or VOCAB_CM_MIXED control mode\n", k, s.c_str());
287  }
288  }
289  yInfo("Run\n");
290  targets=encoders;
291  ffVelocities = 0;
292 }
293 
294 void velControlThread::setRef(int i, double pos)
295 {
296  yInfo("Setting new target %d to %lf\n", i, pos);
297 
298  std::lock_guard<std::mutex> lck(_mutex);
299  targets(i)=pos;
300  ffVelocities(i)=0;
301 }
302 
303 void velControlThread::setVel(int i, double vel)
304 {
305  std::lock_guard<std::mutex> lck(_mutex);
306 
307  if((vel > 0.0) && (vel < MAX_SPEED) && (i>=0) && (i<nJoints))
308  {
309  maxVel(i) = vel;
310  yInfo("setting max vel of joint %d to %f\n",i,maxVel(i));
311  }
312  else
313  yError("Impossible to set max vel higher than %f\n",MAX_SPEED);
314 }
315 
316 void velControlThread::setGain(int i, double gain)
317 {
318  std::lock_guard<std::mutex> lck(_mutex);
319 
320  if (gain>MAX_GAIN)
321  gain=MAX_GAIN;
322 
323  if((gain >= 0.0) && (i>=0) && (i<nJoints))
324  {
325  Kp(i) = gain;
326  yInfo("Setting gain for joint %d to %lf\n", i, Kp(i));
327  }
328  else
329  yError("Cannot set gain of joint %d\n",i);
330 }
331 
332 
334 {
335  for(int k=0; k<nJoints;k++)
336  {
337  if(command(k)!=command(k))//check not Nan
338  {
339  command(k)=0.0;
340  yWarning("WARNING::Receiving NaN values\n");
341  }
342  if (fabs(command(k))>maxVel(k))
343  {
344  if (command(k)>0)
345  command(k)=maxVel(k);
346  else
347  command(k)=-maxVel(k);
348  }
349  }
350 }
void setGain(int i, double gain)
void setRef(int i, double pos)
velControlThread(int rate)
bool init(yarp::dev::PolyDriver *d, std::string partName, std::string robotName)
void limitSpeed(yarp::sig::Vector &command)
void setVel(int i, double vel)
Copyright (C) 2008 RobotCub Consortium.
fprintf(fid,'\n')
fclose(fileID)
const double AVERAGE
const double MAX_GAIN
const double ACCELERATIONS
const int VELOCITY_INDEX_OFFSET
const double MAX_SPEED