17 #ifndef __HANDIKMODULE_H__ 18 #define __HANDIKMODULE_H__ 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> 39 class HandIKModule:
public yarp::os::RFModule
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;
57 std::vector<yarp::sig::Vector> combinations;
59 HandIK_Variables bestSolution;
61 yarp::os::Port outputPort;
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);
77 bool configure(yarp::os::ResourceFinder &rf);
79 bool interruptModule();