grasp
All Data Structures Namespaces Functions Modules
handIKModule.h
1 /* Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
2  * Authors: Ilaria Gori
3  * email: ilaria.gori@iit.it
4  * Permission is granted to copy, distribute, and/or modify this program
5  * under the terms of the GNU General Public License, version 2 or any
6  * later version published by the Free Software Foundation.
7  *
8  * A copy of the license can be found in the file LICENSE located in the
9  * root directory.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15 */
16 
17 #ifndef __HANDIKMODULE_H__
18 #define __HANDIKMODULE_H__
19 
20 #include <string>
21 #include <stdio.h>
22 #include <stdlib.h>
23 #include <iostream>
24 #include <fstream>
25 #include <sstream>
26 #include <vector>
27 #include <cmath>
28 #include <yarp/os/RFModule.h>
29 #include <yarp/os/BufferedPort.h>
30 #include <yarp/math/Math.h>
31 #include <yarp/os/Time.h>
32 #include <yarp/os/Random.h>
33 #include <yarp/os/Semaphore.h>
34 #include <yarp/sig/all.h>
35 #include <iCub/iKin/iKinFwd.h>
36 #include <iCub/ctrl/math.h>
37 #include "handIK.h"
38 
39 class HandIKModule: public yarp::os::RFModule
40 {
41  int winnerIndex;
42  double bestObjValue;
43  bool done;
44  bool work;
45  std::string hand;
46  std::string portName;
47  std::string tag;
48  yarp::sig::Vector center;
49  yarp::sig::Vector dim;
50  yarp::sig::Matrix rotation;
51  yarp::sig::Matrix rot_tran;
52  std::vector<yarp::sig::Vector> contacts_r;
53  std::vector<yarp::sig::Vector> normals_r;
54  std::vector<yarp::sig::Vector> contacts_o;
55  std::vector<yarp::sig::Vector> normals_o;
56 
57  std::vector<yarp::sig::Vector> combinations;
58 
59  HandIK_Variables bestSolution;
60 
61  yarp::os::Port outputPort;
62  yarp::os::Port rpc;
63 
64 private:
65  void fromRootToObject();
66  bool respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply);
67  void createCombinationVector();
68  bool extractData(const yarp::os::Bottle &data);
69  void fillVectorFromBottle(const yarp::os::Bottle* b, yarp::sig::Vector &v);
70  void fillMatrixFromBottle(const yarp::os::Bottle* b, yarp::sig::Matrix &m, int rows, int cols);
71  void prepareData(yarp::os::Bottle &data);
72  double evaluateFingers(const HandIK_Variables &solution, const int id);
73 
74 public:
75 
76  HandIKModule();
77  bool configure(yarp::os::ResourceFinder &rf);
78  bool close();
79  bool interruptModule();
80  bool updateModule();
81  double getPeriod();
82 };
83 
84 #endif
85