grasp
•All Data Structures Namespaces Functions Modules
precisionGrasp.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 __PRECISION_GRASP_H__
18 #define __PRECISION_GRASP_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/os/ConnectionReader.h>
31 #include <yarp/sig/Vector.h>
32 #include <yarp/sig/Matrix.h>
33 #include <yarp/math/Math.h>
34 #include <yarp/os/Time.h>
35 #include <yarp/os/Random.h>
36 #include <yarp/os/Semaphore.h>
37 #include <yarp/os/Event.h>
38 #include <yarp/dev/CartesianControl.h>
39 #include <yarp/dev/ControlBoardInterfaces.h>
40 #include <yarp/dev/PolyDriver.h>
41 #include <iCub/iKin/iKinFwd.h>
42 #include <pcl/common/common_headers.h>
43 #include <pcl/features/normal_3d.h>
44 #include <pcl/io/ply_io.h>
45 #include <pcl/filters/statistical_outlier_removal.h>
46 #include <pcl/visualization/pcl_visualizer.h>
47 #include <pcl/filters/voxel_grid.h>
48 #include <pcl/octree/octree.h>
49 #include <boost/thread/thread.hpp>
50 #include <iCub/data3D/SurfaceMeshWithBoundingBox.h>
51 #include <iCub/grasp/forceClosure.h>
52 #include <visualizationThread.h>
53 #include <psoThread.h>
54 
55 #ifdef _WIN32
56  #include "custom/dirent.h"
57 #endif
58 
59 #define STATE_WAIT 0
60 #define STATE_ESTIMATE 1
61 #define STATE_IK 3
62 #define STATE_GRASP 2
63 #define RIGHT_HAND "right"
64 #define LEFT_HAND "left"
65 #define NO_HAND "no_hand"
66 
67 class PrecisionGrasp: public yarp::os::RFModule
68 {
69 private:
70  int current_state;
71  int nFile;
72  int counter;
73  int winner_triplet;
74  int context_in_right;
75  int current_context_right;
76  int context_right;
77  int context_in_left;
78  int current_context_left;
79  int context_left;
80  int ov_cones1;
81  int ov_cones2;
82  int ov_cones3;
83  int ov_cones4;
84  int winner_ov_cones;
85  int posx;
86  int posy;
87  int sizex;
88  int sizey;
89  bool clicked;
90  bool fromFile;
91  bool fromFileFinished;
92  bool dont;
93  bool grasped;
94  bool visualize;
95  bool straight;
96  bool readyToGrasp;
97  bool rightBlocked;
98  bool leftBlocked;
99  bool filterCloud;
100  bool writeCloud;
101  bool rightDisabled;
102  bool leftDisabled;
103  double handSize;
104  double radiusSearch;
105  double sampling;
106  double alpha;
107  double bestCost;
108  double bestManipulability;
109  std::string path;
110  std::string filenameTrain;
111  std::string robot;
112  std::string outputFile;
113  ofstream graspFileTrain;
114  std::string winner_hand;
115 
116  std::vector<yarp::sig::Vector> contacts_r1;
117  std::vector<yarp::sig::Vector> normals_r1;
118  std::vector<yarp::sig::Vector> contacts_r2;
119  std::vector<yarp::sig::Vector> normals_r2;
120  std::vector<yarp::sig::Vector> contacts_r3;
121  std::vector<yarp::sig::Vector> normals_r3;
122  std::vector<yarp::sig::Vector> contacts_r4;
123  std::vector<yarp::sig::Vector> normals_r4;
124  yarp::sig::Vector joints_tmp;
125  yarp::sig::Vector winner_joints;
126  yarp::sig::Vector ee_tmp;
127  yarp::sig::Vector winner_ee;
128  yarp::sig::Vector axis_angle_tmp;
129  yarp::sig::Vector winner_axis;
130  yarp::sig::Vector combination_tmp;
131  yarp::sig::Vector winner_combination;
132  yarp::sig::Vector winner_odhat;
133  yarp::sig::Vector winner_xdhat;
134  yarp::sig::Vector center;
135  yarp::sig::Vector dim;
136  yarp::sig::Matrix rotation;
137  yarp::sig::Vector xdhat,odhat;
138  yarp::sig::Vector q;
139  yarp::sig::Vector offsetR;
140  yarp::sig::Vector offsetL;
141  yarp::sig::Vector home_p_l;
142  yarp::sig::Vector home_p_r;
143  yarp::sig::Vector home_o_l;
144  yarp::sig::Vector home_o_r;
145 
146  yarp::dev::PolyDriver dCtrlRight, dCtrlLeft;
147  yarp::dev::ICartesianControl *iCtrlRight, *iCtrlLeft;
148  yarp::dev::IPositionControl *posRight, *posLeft;
149  yarp::dev::IEncoders *encRight, *encLeft;
150  yarp::dev::PolyDriver robotArmRight, robotArmLeft;
151  yarp::dev::PolyDriver robotTorso;
152  yarp::dev::IControlLimits *limTorso, *limArmRight, *limArmLeft;
153 
154  iCub::iKin::iCubArm *armRight, *armLeft;
155  iCub::iKin::iKinChain *chainRight, *chainLeft;
156 
157  yarp::sig::Vector thetaMinRight, thetaMinLeft;
158  yarp::sig::Vector thetaMaxRight, thetaMaxLeft;
159 
160  VisualizationThread *visualizationThread;
161  PsoThread* psoThreadFitness1;
162  PsoThread* psoThreadFitness2;
163  PsoThread* psoThreadFitness3;
164  PsoThread* psoThreadFitness4;
165  DataToShow data;
166 
167  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
168  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudxyz;
169  pcl::PointCloud <pcl::Normal>::Ptr normals;
170 
171  yarp::os::Port reconstructionPort;
172  yarp::os::Port ikPort1r;
173  yarp::os::Port ikPort2r;
174  yarp::os::Port ikPort3r;
175  yarp::os::Port ikPort4r;
176  yarp::os::Port ikPort1l;
177  yarp::os::Port ikPort2l;
178  yarp::os::Port ikPort3l;
179  yarp::os::Port ikPort4l;
180  yarp::os::Port areCmdPort;
181  yarp::os::Port depth2kin;
182  yarp::os::BufferedPort<iCub::data3D::SurfaceMeshWithBoundingBox> meshPort;
183  yarp::os::Port rpc;
184  yarp::os::BufferedPort<yarp::os::Bottle> toMatlab;
185 
186  yarp::os::Semaphore mutex;
187  yarp::os::Semaphore mutex_to_write;
188  yarp::os::Event eventRpc;
189 
190  iCub::data3D::BoundingBox boundingBox;
191 
192  double min(const double a, const double b) {return (a>b)?b:a;};
193  double max(const double a, const double b) {return (a<b)?b:a;};
194 
195  void addPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in);
196  void addPlanePoints();
197  void filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in_filtered, bool second=false);
198  void write(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, const std::string &fileName);
199  bool normalPointingOut(pcl::Normal &normal, pcl::PointXYZ &point);
200  bool respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply);
201  void fromSurfaceMesh(const iCub::data3D::SurfaceMeshWithBoundingBox& msg);
202  bool fillCloudFromFile();
203  void askToGrasp();
204  yarp::os::Bottle prepareData(const iCub::grasp::ContactPoints &triplet, const int c);
205  std::string extractData(const yarp::os::Bottle &data, const int t);
206  void fillVectorFromBottle(const yarp::os::Bottle* b, yarp::sig::Vector &v);
207  double getZDim(const yarp::sig::Vector &vx, const yarp::sig::Vector &vy, const yarp::sig::Vector &vz);
208  void readData(pcl::PointCloud<pcl::PointXYZ>::Ptr c, pcl::PointCloud <pcl::Normal>::Ptr n);
209  void sampleClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr c, pcl::PointCloud <pcl::Normal>::Ptr n);
210  void writeIKresult(const std::vector<yarp::sig::Vector> &contacts_r, const std::vector<yarp::sig::Vector> &normals_r, const int c, const std::string &hand, const int ov_cones);
211  void writeIKBestresult();
212  void write(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud <pcl::Normal>::Ptr n, const std::string &fileName);
213  void associateDim();
214  bool openDevices();
215  void writeToMatlab(const std::vector<yarp::sig::Vector> &contacts_r, const std::vector<yarp::sig::Vector> &normals_r, const std::string &hand);
216  void writeBestSolution();
217  void fillBottleFromVector(const yarp::sig::Vector &vect, yarp::os::Bottle *b);
218 
219 public:
220 
221  PrecisionGrasp();
222  bool configure(yarp::os::ResourceFinder &rf);
223  bool close();
224  bool interruptModule();
225  bool updateModule();
226  double getPeriod();
227 };
228 
229 #endif
230 
iCub::data3D::BoundingBox
The Definition of the BoundingBox class.
Definition: boundingBox.h:61
iCub::grasp::ContactPoints
Definition of the ForceClosure.
Definition: forceClosure.h:60
Generated by   doxygen 1.8.12