iCub-main
positionDirectThread.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) 2015 Istituto Italiano di Tecnologia - iCub Facility
4 // Author: Marco Randazzo <marco.randazzo@iit.it>
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
7 #include "positionDirectThread.h"
8 #include <cstring>
9 #include <string>
10 #include <cmath>
11 
12 #include <yarp/os/Log.h>
13 #include <yarp/os/LogStream.h>
14 
15 using namespace yarp::dev;
16 using namespace yarp::os;
17 using namespace yarp::sig;
18 
20  yarp::os::PeriodicThread((double)period/1000.0)
21 {
22  control_period = period;
23  suspended = true;
24  control_joints_list = 0;
25  joints_limiter = 2;
26  target_limiter = 1;
27 }
28 
30 {
31 }
32 
34 {
35  double curr_time = yarp::os::Time::now()-t_start;
36 
37  if (getIterations()>100)
38  {
39  yDebug("Thread ran %d times, est period %lf[ms], used %lf[ms]\n",
40  getIterations(),
41  1000.0*getEstimatedPeriod(),
42  1000.0*getEstimatedUsed());
43  resetStat();
44  }
45  std::lock_guard<std::mutex> lck(_mutex);
46 
47  //read the position targets
48  yarp::os::Bottle *bot = command_port.read(false);
49  if(bot!=NULL)
50  {
51  unsigned int botsize = bot->size();
52  if (botsize == control_joints)
53  {
54  prev_targets=targets;
55  for (unsigned int i=0; i< control_joints; i++)
56  {
57  targets[i] = bot->get(i).asFloat64();
58  }
59  }
60  else
61  {
62  yError ("Your bottle does not have the right size: module is configured to control %d joints", control_joints);
63  }
64  }
65  else
66  {
67  return;
68  }
69 
70  //apply the joints limits
71  for (unsigned int i=0; i< control_joints; i++)
72  {
73  if (targets[i]>max_limits[i]) targets[i]=max_limits[i];
74  if (targets[i]<min_limits[i]) targets[i]=min_limits[i];
75  }
76 
77  //get the current position
78  for (unsigned int i=0; i< control_joints; i++)
79  {
80  double val =0;
81  ienc->getEncoder(control_joints_list[i],&val);
82  encoders[i]=val;
83  }
84 
85  //apply a limit on the difference between prev target and current target
86  for (unsigned int i=0; i< control_joints; i++)
87  {
88  double diff = (targets[i]-prev_targets[i]);
89  if (diff > +target_limiter) targets[i]=prev_targets[i]+ target_limiter;
90  else if (diff < -target_limiter) targets[i]=prev_targets[i]- target_limiter;
91  }
92 
93  //slew rate limiter
94  for(unsigned int i = 0; i<control_joints; i++)
95  {
96  double diff = (targets[i]-encoders[i]);
97  if (diff > +joints_limiter) targets[i]=encoders[i]+ joints_limiter;
98  else if (diff < -joints_limiter) targets[i]=encoders[i]- joints_limiter;
99  }
100 
101  //appy the command
102 #if 0
103  for(unsigned int i = 0; i<control_joints; i++)
104  {
105  idir->setPosition(control_joints_list[i],targets[i]);
106  }
107 #endif
108  idir->setPositions(control_joints, control_joints_list, targets.data());
109 
110  yDebug() << curr_time << targets[0] << targets[1] << targets[2];
111 }
112 
114 {
115  suspended=true;
116  t_start = yarp::os::Time::now();
117  return true;
118 }
119 
121 {
122  for(unsigned int i=0; i<control_joints; i++)
123  {
124  imod->setControlMode(control_joints_list[i], VOCAB_CM_POSITION);
125  }
126 
127  suspended = true;
128  command_port.close();
129 }
130 
131 bool positionDirectControlThread::init(PolyDriver *d, std::string moduleName, std::string partName, std::string robotName, Bottle* jointsList)
132 {
134  char tmp[255];
135  sprintf(tmp, "/%s/%s/%s/command:i", moduleName.c_str(), robotName.c_str(), partName.c_str());
136  yInfo("opening port for part %s\n",tmp);
137  command_port.open(tmp);
138 
139  if (d==0)
140  {
141  yError ("Invalid device driver pointer");
142  return false;
143  }
144 
145  driver=d;
146  driver->view(idir);
147  driver->view(ipos);
148  driver->view(ienc);
149  driver->view(imod);
150  driver->view(ilim);
151 
152  if ( (idir==0)||(ienc==0) || (imod==0) || (ipos==0) || (ilim==0))
153  {
154  yError ("Failed to view motor interfaces");
155  return false;
156  }
157 
158  int tmpj=0;
159  ipos->getAxes(&tmpj);
160  part_joints=tmpj;
161  control_joints= jointsList->size();
162  if (control_joints>part_joints)
163  {
164  yError ("you cannot control more of %d joints for this robot part", part_joints);
165  return false;
166  }
167  else if (control_joints<=0)
168  {
169  yError ("invalid number of control joints (%d)", control_joints);
170  return false;
171  }
172  else
173  {
174  control_joints_list = new int [control_joints];
175  for (unsigned int i=0; i< control_joints; i++)
176  {
177  if (jointsList->get(i).isInt32() && jointsList->get(i).asInt32()>=0)
178  {
179  control_joints_list[i] = jointsList->get(i).asInt32();
180  }
181  else
182  {
183  yError ("invalid list of jonts to control");
184  return false;
185  }
186  }
187  }
188  yInfo("part has %d joints, controlling %d joints\n",part_joints, control_joints);
189 
190  Vector speeds;
191  speeds.resize(control_joints);
192  speeds=10.0;
193  for (unsigned int i=0; i<control_joints; i++)
194  {
195  ipos->setRefSpeed(control_joints_list[i],speeds[i]);
196  }
197 
198  encoders.resize(control_joints);
199  targets.resize(control_joints);
200  prev_targets.resize(control_joints);
201  error.resize(control_joints);
202  encoders.zero();
203  targets.zero();
204  prev_targets.zero();
205  error.zero();
206 
207  min_limits.resize(control_joints);
208  max_limits.resize(control_joints);
209  for (unsigned int i=0; i<control_joints; i++)
210  {
211  double min=0;
212  double max=0;
213  ilim->getLimits(control_joints_list[i],&min,&max);
214  min_limits[i]=min;
215  max_limits[i]=max;
216  }
217 
218  for (unsigned int i=0; i<control_joints; i++)
219  {
220  imod->setControlMode(control_joints_list[i],VOCAB_CM_POSITION_DIRECT);
221  }
222 
223  //get the current position
224  for (unsigned int i=0; i< control_joints; i++)
225  {
226  double val =0;
227  ienc->getEncoder(control_joints_list[i],&val);
228  targets[i] = encoders[i] = val;
229  prev_targets[i] = encoders[i];
230  }
231 
232  return true;
233 }
234 
236 {
237  suspended=true;
238  yInfo("Suspended\n");
239 }
240 
242 {
243  suspended=false;
244  yInfo("Run\n");
245 }
246 
247 void positionDirectControlThread::setVel(int i, double vel)
248 {
249  std::lock_guard<std::mutex> lck(_mutex);
250 }
251 
252 void positionDirectControlThread::setGain(int i, double gain)
253 {
254  std::lock_guard<std::mutex> lck(_mutex);
255 }
256 
257 
259 {
260 
261 }
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
positionDirectThread.h
positionDirectControlThread::limitSpeed
void limitSpeed(yarp::sig::Vector &command)
Definition: positionDirectThread.cpp:258
positionDirectControlThread::run
void run()
Definition: positionDirectThread.cpp:33
positionDirectControlThread::init
bool init(yarp::dev::PolyDriver *d, std::string moduleName, std::string partName, std::string robotName, yarp::os::Bottle *jointsList)
Definition: positionDirectThread.cpp:131
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
yarp::dev
Definition: DebugInterfaces.h:52
positionDirectControlThread::threadInit
bool threadInit()
Definition: positionDirectThread.cpp:113
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
positionDirectControlThread::halt
void halt()
Definition: positionDirectThread.cpp:235
positionDirectControlThread::threadRelease
void threadRelease()
Definition: positionDirectThread.cpp:120
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
positionDirectControlThread::go
void go()
Definition: positionDirectThread.cpp:241
positionDirectControlThread::positionDirectControlThread
positionDirectControlThread(int rate)
Definition: positionDirectThread.cpp:19
positionDirectControlThread::~positionDirectControlThread
~positionDirectControlThread()
Definition: positionDirectThread.cpp:29
positionDirectControlThread::setGain
void setGain(int i, double gain)
Definition: positionDirectThread.cpp:252
positionDirectControlThread::joints_limiter
double joints_limiter
Definition: positionDirectThread.h:53
positionDirectControlThread::setVel
void setVel(int i, double vel)
Definition: positionDirectThread.cpp:247
v
static int v
Definition: iCub_Sim.cpp:42
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
positionDirectControlThread::target_limiter
double target_limiter
Definition: positionDirectThread.h:54