iCub-main
main.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 
18 #include <yarp/os/all.h>
19 #include <iCub/iKin/iKinSlv.h>
20 #include <fakeMotorDevice.h>
21 
22 #include <iostream>
23 #include <iomanip>
24 #include <string>
25 
26 using namespace std;
27 using namespace yarp::os;
28 using namespace iCub::iKin;
29 
35 {
36 protected:
46  PartDescriptor *getPartDesc(Searchable &options)
47  {
48  if (!options.check("CustomKinFile"))
49  {
50  cout<<"Error: \"CustomKinFile\" option is missing!"<<endl;
51  return NULL;
52  }
53 
54  string robot=options.check("robot",Value("fake_robot")).asString();
55  string part="fake_part";
56 
57  // here we declare everything is required to open up
58  // the device driver to access the fake robot
59  Property optPart;
60  optPart.put("device","fakeyClient");
61  optPart.put("remote","/"+robot+"/"+part);
62  optPart.put("local","/"+slvName+"/"+part);
63  optPart.put("part",part);
64 
65  // we grab info on the fake robot's kinematics
66  Property linksOptions;
67  linksOptions.fromConfigFile(options.find("CustomKinFile").asString());
68  iKinLimb *limb=new iKinLimb(linksOptions);
69  if (!limb->isValid())
70  {
71  cout<<"Error: invalid links parameters!"<<endl;
72  delete limb;
73  return NULL;
74  }
75 
76  // we fill in the descriptor fields
78  p->lmb=limb; // a pointer to the iKinLimb
79  p->chn=limb->asChain(); // the associated iKinChain object
80  p->cns=NULL; // any further (linear) constraints on the joints other than the bounds? This requires some more effort
81  p->prp.push_back(optPart); // attach the options to open the device driver of the fake part
82  p->rvs.push_back(false); // it may happen that the motor commands to be sent are in reversed order wrt the order of kinematics links (e.g. the iCub torso); if so put here "true"
83  p->num=1; // only one device driver for the whole limb (see below)
84 
85  // whenever a limb is actuated resorting to more than one device
86  // (e.g. for iCub: torso+arm), the following applies:
87  //
88  // p->prp.push_back(optDevice_1);
89  // p->prp.push_back(optDevice_2);
90  // p->rvs.push_back(true);
91  // p->rvs.push_back(false);
92  // p->num=2;
93 
94  return p;
95  }
96 
97 public:
98  /**********************************************************/
99  fakeRobotCartesianSolver(const string &name) : CartesianSolver(name) { }
100 };
101 
102 class SolverModule: public RFModule
103 {
104 protected:
106 
107 public:
108  /**********************************************************/
109  SolverModule() : solver(NULL) { }
110 
111  /**********************************************************/
112  bool configure(ResourceFinder &rf)
113  {
114  if (!rf.check("name"))
115  {
116  cout<<"Error: \"name\" option is missing!"<<endl;
117  return false;
118  }
119 
120  string solverName=rf.find("name").asString();
121  string pathToKin=rf.findFile("kinematics_file");
122 
123  Property config;
124  config.fromConfigFile(rf.findFile("from"));
125  config.put("CustomKinFile",pathToKin);
126 
127  solver=new fakeRobotCartesianSolver(solverName);
128  if (!solver->open(config))
129  {
130  delete solver;
131  return false;
132  }
133 
134  return true;
135  }
136 
137  /************************************************************************/
139  {
140  if (solver!=NULL)
141  solver->interrupt();
142 
143  return true;
144  }
145 
146  /**********************************************************/
147  bool close()
148  {
149  delete solver;
150  return true;
151  }
152 
153  /**********************************************************/
154  double getPeriod()
155  {
156  return 1.0;
157  }
158 
159  /**********************************************************/
161  {
162  if (solver->isClosed() || solver->getTimeoutFlag())
163  return false;
164  else
165  return true;
166  }
167 };
168 
169 /**********************************************************/
170 int main(int argc, char *argv[])
171 {
172  Network yarp;
173  if (!yarp.checkNetwork())
174  {
175  cout<<"Error: yarp server does not seem available"<<endl;
176  return 1;
177  }
178 
179  // register here the new yarp devices
180  // for dealing with the fake robot
182 
183  ResourceFinder rf;
184  rf.setDefaultConfigFile("solver.ini");
185  rf.setDefault("kinematics_file","kinematics.ini");
186  rf.configure(argc,argv);
187 
188  SolverModule solver;
189  return solver.runModule(rf);
190 }
191 
192 
fakeRobotCartesianSolver::getPartDesc
PartDescriptor * getPartDesc(Searchable &options)
This particular method serves to describe all the device drivers used by the solver to access the rob...
Definition: main.cpp:46
iKinSlv.h
iCub::iKin::CartesianSolver::getTimeoutFlag
virtual bool & getTimeoutFlag()
To be called to check whether communication timeout has been detected.
Definition: iKinSlv.h:559
main
int main(int argc, char *argv[])
Definition: main.cpp:31
SolverModule::updateModule
bool updateModule()
Definition: main.cpp:160
iCub::iKin::CartesianSolver
Definition: iKinSlv.h:344
SolverModule::close
bool close()
Definition: main.cpp:147
iCub::iKin::CartesianSolver::interrupt
virtual void interrupt()
Interrupt the open() method waiting for motor parts to be ready.
Definition: iKinSlv.cpp:1525
registerFakeMotorDevices
void registerFakeMotorDevices()
Register new yarp devices for fake motor handling.
Definition: fakeMotorDevice.cpp:24
fakeRobotCartesianSolver::fakeRobotCartesianSolver
fakeRobotCartesianSolver(const string &name)
Definition: main.cpp:99
p
p
Definition: show_eyes_axes.m:23
SolverModule::solver
CartesianSolver * solver
Definition: main.cpp:105
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
SolverModule::SolverModule
SolverModule()
Definition: main.cpp:109
iCub::iKin
Definition: iKinFwd.h:71
fakeMotorDevice.h
fakeRobotCartesianSolver
This class inherits from the CartesianSolver super-class implementing the solver.
Definition: main.cpp:34
SolverModule::getPeriod
double getPeriod()
Definition: main.cpp:154
SolverModule
Definition: main.cpp:332
SolverModule::interruptModule
bool interruptModule()
Definition: main.cpp:138
iCub::iKin::PartDescriptor
Definition: iKinSlv.h:328
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
scripting.argc
argc
Definition: scripting.py:184
iCub::iKin::iKinLimb::isValid
bool isValid() const
Checks if the limb has been properly configured.
Definition: iKinFwd.h:998
iCub::iKin::CartesianSolver::isClosed
virtual bool isClosed() const
To be called to check whether the solver has received a [quit] request.
Definition: iKinSlv.h:549
SolverModule::configure
bool configure(ResourceFinder &rf)
Definition: main.cpp:112
iCub::iKin::iKinLimb::asChain
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
iCub::iKin::CartesianSolver::open
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition: iKinSlv.cpp:1270