iCub-main
fakeMotorDeviceServer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3  * Author: Ugo Pattacini
4  * email: 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 
19 
20 #include <string>
21 #include <stdio.h>
22 
23 using namespace std;
24 using namespace yarp::os;
25 using namespace yarp::dev;
26 using namespace yarp::sig;
27 using namespace iCub::ctrl;
28 
29 /**********************************************************/
31 {
32  motors=NULL;
33  configured=false;
34 }
35 
36 /**********************************************************/
37 bool fakeMotorDeviceServer::open(Searchable &config)
38 {
39  printf("Opening Fake Motor Device Server ...\n");
40 
41  string local=config.check("local",Value("/fakeyServer")).asString();
42  int Ts=config.check("Ts",Value(10)).asInt32();
43 
44  statePort.open(local+"/state:o");
45  cmdPort.open(local+"/cmd:i");
46  rpcPort.open(local+"/rpc");
47  rpcPort.setReader(*this);
48 
49  // the part is composed of three rotational joints
50  // whose bounds are given in degrees just below
51  Matrix lim(3,2);
52  lim(0,0)=-180.0; lim(0,1)=180.0; // joint 0
53  lim(1,0)=-90.0; lim(1,1)=90.0; // joint 1
54  lim(2,0)=-45.0; lim(2,1)=45.0; // joint 2
55 
56  Vector q0;
57  for (int i=0; i<lim.rows(); i++)
58  q0.push_back((lim(i,0)+lim(i,1))/2.0);
59 
60  // the motors themselves are represented
61  // by pure integrators that give back joints
62  // positions when fed with joints velocities
63  motors=new Integrator((double)Ts/1000.0,q0,lim);
64  vel.resize(motors->get().length(),0.0);
65 
66  setPeriod((double)Ts/1000.0);
67  start();
68 
69  configured=true;
70 
71  printf("Fake Motor Device Server successfully open\n");
72  return true;
73 }
74 
75 /**********************************************************/
77 {
78  printf("Closing Fake Motor Device Server ...\n");
79 
80  if (isRunning())
81  stop();
82 
83  mtx.lock();
85  mtx.unlock();
86 
87  statePort.interrupt();
88  cmdPort.interrupt();
89  rpcPort.interrupt();
90 
91  statePort.close();
92  cmdPort.close();
93  rpcPort.close();
94 
95  if (motors!=NULL)
96  {
97  delete motors;
98  motors=NULL;
99  }
100 
101  configured=false;
102 
103  printf("Fake Motor Device Server successfully closed\n");
104  return true;
105 }
106 
107 /**********************************************************/
109 {
110  lock_guard<mutex> lg(mtx);
111  if (Bottle *cmd=cmdPort.read(false))
112  {
113  if ((size_t)cmd->size()>=vel.length())
114  for (size_t i=0; i<vel.length(); i++)
115  vel[i]=cmd->get(i).asFloat64();
116  }
117 
118  statePort.prepare()=motors->integrate(vel);
119  statePort.write();
120 }
121 
122 /**********************************************************/
123 bool fakeMotorDeviceServer::read(ConnectionReader &connection)
124 {
125  Bottle cmd,reply;
126  cmd.read(connection);
127 
128  mtx.lock();
129 
130  int codeIF=cmd.get(0).asVocab32();
131  int codeMethod=cmd.get(1).asVocab32();
132 
133  if (codeIF==Vocab32::encode("lim"))
134  {
135  if (codeMethod==Vocab32::encode("get"))
136  {
137  int axis=cmd.get(2).asInt32();
138  double min,max;
139  if (getLimits(axis,&min,&max))
140  {
141  reply.addVocab32("ack");
142  reply.addFloat64(min);
143  reply.addFloat64(max);
144  }
145  }
146  }
147  else if (codeIF==Vocab32::encode("enc"))
148  {
149  if (codeMethod==Vocab32::encode("axes"))
150  {
151  int ax;
152  if (getAxes(&ax))
153  {
154  reply.addVocab32("ack");
155  reply.addInt32(ax);
156  }
157  }
158  }
159  else if (codeIF==Vocab32::encode("vel"))
160  {
161  if (codeMethod==Vocab32::encode("move"))
162  {
163  int axis=cmd.get(2).asInt32();
164  double sp=cmd.get(3).asFloat64();
165  if (velocityMove(axis,sp))
166  reply.addVocab32("ack");
167  }
168  else if (codeMethod==Vocab32::encode("acc"))
169  {
170  int axis=cmd.get(2).asInt32();
171  double acc=cmd.get(3).asFloat64();
172  if (setRefAcceleration(axis,acc))
173  reply.addVocab32("ack");
174  }
175  else if (codeMethod==Vocab32::encode("stop"))
176  {
177  int axis=cmd.get(2).asInt32();
178  if (stop(axis))
179  reply.addVocab32("ack");
180  }
181  }
182 
183  if (reply.size()==0)
184  reply.addVocab32("nack");
185 
186  mtx.unlock();
187 
188  if (ConnectionWriter *returnToSender=connection.getWriter())
189  reply.write(*returnToSender);
190 
191  return true;
192 }
193 
194 /**********************************************************/
195 bool fakeMotorDeviceServer::getLimits(int axis, double *min, double *max)
196 {
197  if (!configured)
198  return false;
199 
200  if ((axis<(int)motors->get().length()) && (min!=NULL) && (max!=NULL))
201  {
202  Matrix lim=motors->getLim();
203  *min=lim(axis,0); *max=lim(axis,1);
204  return true;
205  }
206  else
207  return false;
208 }
209 
210 /**********************************************************/
212 {
213  if (!configured)
214  return false;
215 
216  if (ax!=NULL)
217  {
218  *ax=(int)motors->get().length();
219  return true;
220  }
221  else
222  return false;
223 }
224 
225 /**********************************************************/
227 {
228  if (!configured)
229  return false;
230 
231  if ((size_t)j<vel.length())
232  {
233  vel[j]=sp;
234  return true;
235  }
236  else
237  return false;
238 }
239 
240 /**********************************************************/
242 {
243  if (configured && ((size_t)j<vel.length()))
244  return true;
245  else
246  return false;
247 }
248 
249 /**********************************************************/
251 {
252  return velocityMove(j,0.0);
253 }
254 
255 
256 
iCub::ctrl::Integrator::integrate
const yarp::sig::Vector & integrate(const yarp::sig::Vector &x)
Executes one-step integration of input vector.
Definition: pids.cpp:115
iCub::ctrl::Integrator
Definition: pids.h:47
fakeMotorDeviceServer::open
bool open(yarp::os::Searchable &config)
Definition: fakeMotorDeviceServer.cpp:37
cmd
cmd
Definition: dataTypes.h:30
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
fakeMotorDeviceServer::cmdPort
yarp::os::BufferedPort< yarp::os::Bottle > cmdPort
Definition: fakeMotorDeviceComponents.h:42
fakeMotorDeviceServer::rpcPort
yarp::os::Port rpcPort
Definition: fakeMotorDeviceComponents.h:43
fakeMotorDeviceServer::getLimits
bool getLimits(int axis, double *min, double *max)
Definition: fakeMotorDeviceServer.cpp:195
stop
static int stop
Definition: iCub_Sim.cpp:41
yarp::dev
Definition: DebugInterfaces.h:52
fakeMotorDeviceServer::velocityMove
bool velocityMove(int j, double sp)
Definition: fakeMotorDeviceServer.cpp:226
fakeMotorDeviceServer::setRefAcceleration
bool setRefAcceleration(int j, double acc)
Definition: fakeMotorDeviceServer.cpp:241
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
fakeMotorDeviceServer::run
void run()
Definition: fakeMotorDeviceServer.cpp:108
iCub::ctrl::Integrator::get
const yarp::sig::Vector & get() const
Returns the current output vector.
Definition: pids.h:154
fakeMotorDeviceServer::fakeMotorDeviceServer
fakeMotorDeviceServer()
Definition: fakeMotorDeviceServer.cpp:30
fakeMotorDeviceServer::stop
bool stop()
Definition: fakeMotorDeviceComponents.h:115
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
iCub::ctrl::Integrator::getLim
const yarp::sig::Matrix & getLim()
Returns the constraints matrix.
Definition: pids.h:141
fakeMotorDeviceServer::statePort
yarp::os::BufferedPort< yarp::sig::Vector > statePort
Definition: fakeMotorDeviceComponents.h:41
fakeMotorDeviceServer::motors
iCub::ctrl::Integrator * motors
Definition: fakeMotorDeviceComponents.h:47
fakeMotorDeviceServer::configured
bool configured
Definition: fakeMotorDeviceComponents.h:49
fakeMotorDeviceServer::getAxes
bool getAxes(int *ax)
Definition: fakeMotorDeviceServer.cpp:211
fakeMotorDeviceServer::read
bool read(yarp::os::ConnectionReader &connection)
This method decodes the requests forwarded by the client and responds with corresponding replies.
Definition: fakeMotorDeviceServer.cpp:123
fakeMotorDeviceComponents.h
acc
_3f_vect_t acc
Definition: dataTypes.h:3
fakeMotorDeviceServer::vel
yarp::sig::Vector vel
Definition: fakeMotorDeviceComponents.h:48
fakeMotorDeviceServer::close
bool close()
Definition: fakeMotorDeviceServer.cpp:76
fakeMotorDeviceServer::mtx
std::mutex mtx
Definition: fakeMotorDeviceComponents.h:45