iCub-main
module.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 iCub Facility - 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 #ifndef __DEPTH2KIN_MODULE_H__
19 #define __DEPTH2KIN_MODULE_H__
20 
21 #include <mutex>
22 #include <string>
23 #include <deque>
24 
25 #include <opencv2/opencv.hpp>
26 
27 #include <yarp/os/all.h>
28 #include <yarp/sig/all.h>
29 #include <yarp/dev/all.h>
30 
31 #include <iCub/iKin/iKinFwd.h>
32 
33 #include "depth2kin_IDL.h"
34 #include "methods.h"
35 #include "nlp.h"
36 
37 using namespace std;
38 using namespace yarp::os;
39 using namespace yarp::dev;
40 using namespace yarp::sig;
41 using namespace iCub::iKin;
42 
43 // forward declaration
44 class CalibModule;
45 
46 
47 /************************************************************************/
48 class DisparityProcessor : public BufferedPort<ImageOf<PixelMono> >
49 {
50 protected:
52  void onRead(ImageOf<PixelMono> &imgIn);
53 
54 public:
56 };
57 
58 
59 
60 /************************************************************************/
61 class CalibModule : public RFModule, public depth2kin_IDL
62 {
63 protected:
64  ResourceFinder *rf;
65  PolyDriver drvArmL;
66  PolyDriver drvArmR;
67  PolyDriver drvAnalogL;
68  PolyDriver drvAnalogR;
69  PolyDriver drvCartL;
70  PolyDriver drvCartR;
71  PolyDriver drvGaze;
72  IControlMode *imods;
73  IEncoders *iencs;
74  IPositionControl *iposs;
75  IAnalogSensor *ianalog;
76  ICartesianControl *iarm;
77  IGazeControl *igaze;
78 
79  mutex mtx;
82 
84  LocallyWeightedExperts expertsL,expertsR,*experts;
86 
87  string arm;
88  bool useArmL,useArmR;
90  double max_dist;
91  double block_eyes;
95  int roi_side;
96  int nEncs;
97  int test;
98  bool enabled;
99  bool calibrated;
100  bool isSaved;
101  bool closing;
105 
106  deque<Vector> targets,targetsConsumed;
111  motorExplorationStateLog } motorExplorationState;
112 
115 
116  BufferedPort<Bottle> touchInPort;
118  BufferedPort<ImageOf<PixelBgr> > depthOutPort;
119  RpcClient depthRpcPort;
120  RpcServer rpcPort;
121 
122  bool attach(RpcServer &source);
123  bool createTargets(const Vector &c, const Vector &size);
124  bool isTypeValid(const string &type);
125  Calibrator *factory(const string &type);
126  bool factory(Value &v);
127  cv::Rect extractFingerTip(ImageOf<PixelMono> &imgIn, ImageOf<PixelBgr> &imgOut,
128  const Vector &c, Vector &px);
129  double getMinVer() const;
130  bool getGazeParams(const string &eye, const string &type, Matrix &M);
131  bool pushExtrinsics(const string &eye, const Matrix &H);
132  bool getDepth(const Vector &px, Vector &x, Vector &pxr);
133  bool getDepthAveraged(const Vector &px, Vector &x, Vector &pxr, const int maxSamples=5);
134  void openHand(IControlMode *imod, IPositionControl *ipos);
135  void postureHelper(const Vector &gaze_ang, const Matrix &targetL, const Matrix &targetR);
136  void prepareRobot();
137  int removeOutliers();
138  void doMotorExploration();
139  void doTouch(const Vector &xd);
140  void doTest();
141 
142 public:
143  CalibModule();
144  bool configure(ResourceFinder &rf);
145  void onRead(ImageOf<PixelMono> &imgIn);
146  double getPeriod();
147  bool updateModule();
148  void terminate();
149  bool close();
150 
151  // IDL methods
152  int getNumExperts();
153  bool clearExperts();
154  bool load();
155  bool save();
156  bool log(const string &type);
157  bool explore();
158  bool stop();
159  bool setMaxDist(const double max_dist);
160  double getMaxDist();
161  bool setRoi(const int side);
162  int getRoi();
163  bool setBlockEyes(const double block_eyes);
164  double getBlockEyes();
165  bool blockEyes();
166  bool clearEyes();
167  bool setArm(const string &arm);
168  string getArm();
169  bool setCalibrationType(const string &type, const string &extrapolation);
170  string getCalibrationType();
171  Property calibrate(const bool rm_outliers);
172  bool pushCalibrator();
173  bool setTouchWithExperts(const string &sw);
174  string getTouchWithExperts();
175  bool touch(const int u, const int v);
176  PointReq getPoint(const string &arm, const double x, const double y, const double z);
177  vector<PointReq> getPoints(const string &arm, const vector<double> &coordinates);
178  bool setExperiment(const string &exp, const string &v);
179  string getExperiment(const string &exp);
180  Vector getExtrinsics(const string &eye);
181  bool resetExtrinsics(const string &eye);
182  bool setExplorationWait(const double wait);
183  double getExplorationWait();
184  bool setExplorationInTargetTol(const double tol);
185  double getExplorationInTargetTol();
186  bool setTouchInTargetTol(const double tol);
187  double getTouchInTargetTol();
188  bool setExplorationSpace(const double cx, const double cy, const double cz,
189  const double a, const double b);
190  bool setExplorationSpaceDelta(const double dcx, const double dcy, const double dcz,
191  const double da, const double db);
192  Property getExplorationData();
193  bool clearExplorationData();
194  bool posture(const string &type);
195  bool calibrateDepth();
196  bool quit();
197 };
198 
199 
200 #endif
201 
202 
CalibModule::roi_side
int roi_side
Definition: module.h:95
iKinFwd.h
CalibModule::motorExplorationStateTrigger
@ motorExplorationStateTrigger
Definition: module.h:109
iCub::skinManager::quit
@ quit
Definition: rpcSkinManager.h:24
CalibModule::ianalog
IAnalogSensor * ianalog
Definition: module.h:75
H
H
Definition: compute_ekf_fast.m:27
CalibModule::isSaved
bool isSaved
Definition: module.h:100
CalibModule::drvGaze
PolyDriver drvGaze
Definition: module.h:71
CalibModule::rpcPort
RpcServer rpcPort
Definition: module.h:120
CalibModule::context_gaze
int context_gaze
Definition: module.h:114
CalibModule::rf
ResourceFinder * rf
Definition: module.h:64
CalibModule::block_eyes
double block_eyes
Definition: module.h:91
CalibModule::iposs
IPositionControl * iposs
Definition: module.h:74
Calibrator
Definition: methods.h:32
EyeAligner
Definition: nlp.h:36
z
z
Definition: show_eyes_axes.m:22
CalibModule::iarm
ICartesianControl * iarm
Definition: module.h:76
CalibModule::motorExplorationAsyncStop
bool motorExplorationAsyncStop
Definition: module.h:107
CalibModule::touchInPort
BufferedPort< Bottle > touchInPort
Definition: module.h:116
CalibModule::drvCartR
PolyDriver drvCartR
Definition: module.h:70
CalibModule::exp_depth2kin
bool exp_depth2kin
Definition: module.h:102
DisparityProcessor
Definition: module.h:48
iCub::iKin::iCubFinger
Definition: iKinFwd.h:1121
LocallyWeightedExperts
Definition: methods.h:119
CalibModule::igaze
IGazeControl * igaze
Definition: module.h:77
stop
static int stop
Definition: iCub_Sim.cpp:41
CalibModule::touch_intargettol
double touch_intargettol
Definition: module.h:94
CalibModule::motorExplorationStateReach
@ motorExplorationStateReach
Definition: module.h:110
yarp::dev
Definition: DebugInterfaces.h:52
CalibModule::max_dist
double max_dist
Definition: module.h:90
nlp.h
CalibModule::test
int test
Definition: module.h:97
CalibModule::exp_aligneyes
bool exp_aligneyes
Definition: module.h:103
CalibModule::imods
IControlMode * imods
Definition: module.h:72
CalibModule
Definition: module.h:61
CalibModule::arm
string arm
Definition: module.h:87
CalibModule::selectArmEnabled
bool selectArmEnabled
Definition: module.h:89
CalibModule::drvArmL
PolyDriver drvArmL
Definition: module.h:65
CalibModule::depthRpcPort
RpcClient depthRpcPort
Definition: module.h:119
iCub::iKin
Definition: iKinFwd.h:71
CalibModule::useArmR
bool useArmR
Definition: module.h:88
CalibModule::curExplorationCenter
Vector curExplorationCenter
Definition: module.h:81
CalibModule::drvCartL
PolyDriver drvCartL
Definition: module.h:69
CalibModule::enabled
bool enabled
Definition: module.h:98
CalibModule::expertsR
LocallyWeightedExperts expertsR
Definition: module.h:84
CalibModule::mtx
mutex mtx
Definition: module.h:79
depth2kin_IDL.h
CalibModule::context_arm
int context_arm
Definition: module.h:113
CalibModule::motorExplorationStateIdle
@ motorExplorationStateIdle
Definition: module.h:108
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
PointReq
PointReq IDL structure to send/receive points.
Definition: PointReq.h:21
CalibModule::aligner
EyeAligner aligner
Definition: module.h:85
CalibModule::depthInPort
DisparityProcessor depthInPort
Definition: module.h:117
CalibModule::closing
bool closing
Definition: module.h:101
CalibModule::depthOutPort
BufferedPort< ImageOf< PixelBgr > > depthOutPort
Definition: module.h:118
CalibModule::targetsConsumed
deque< Vector > targetsConsumed
Definition: module.h:106
DisparityProcessor::module
CalibModule * module
Definition: module.h:51
v
static int v
Definition: iCub_Sim.cpp:42
depth2kin_IDL
depth2kin_IDL IDL Interface to depth2kin services.
Definition: depth2kin_IDL.h:24
CalibModule::finger
iCubFinger finger
Definition: module.h:80
CalibModule::iencs
IEncoders * iencs
Definition: module.h:73
CalibModule::exploration_wait
double exploration_wait
Definition: module.h:92
CalibModule::touchWithExperts
bool touchWithExperts
Definition: module.h:104
iCub::skinManager::calibrate
@ calibrate
Definition: rpcSkinManager.h:13
CalibModule::drvAnalogL
PolyDriver drvAnalogL
Definition: module.h:67
exp
exp(-x3 *T)]
CalibModule::drvArmR
PolyDriver drvArmR
Definition: module.h:66
CalibModule::calibrated
bool calibrated
Definition: module.h:99
CalibModule::exploration_intargettol
double exploration_intargettol
Definition: module.h:93
methods.h
CalibModule::nEncs
int nEncs
Definition: module.h:96
CalibModule::calibrator
Calibrator * calibrator
Definition: module.h:83
CalibModule::drvAnalogR
PolyDriver drvAnalogR
Definition: module.h:68