Line data Source code
1 : /*
2 : * Copyright (C) 2019 IIT-HHCM
3 : * Author: Luca Muratore
4 : * email: luca.muratore@iit.it
5 : *
6 : * Licensed under the Apache License, Version 2.0 (the "License");
7 : * you may not use this file except in compliance with the License.
8 : * You may obtain a copy of the License at
9 : * http://www.apache.org/licenses/LICENSE-2.0
10 : *
11 : * Unless required by applicable law or agreed to in writing, software
12 : * distributed under the License is distributed on an "AS IS" BASIS,
13 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 : * See the License for the specific language governing permissions and
15 : * limitations under the License.
16 : */
17 :
18 : #ifndef __ROSEE_EE_HAL__
19 : #define __ROSEE_EE_HAL__
20 :
21 :
22 : #include <ros/ros.h>
23 : #include <sensor_msgs/JointState.h>
24 :
25 : #include <string>
26 : #include <vector>
27 : #include <memory>
28 : #include <unordered_map>
29 : #include <fstream>
30 :
31 : #include <Eigen/Dense>
32 : #include <yaml-cpp/yaml.h>
33 : #include <ros_end_effector/Utils.h>
34 : #include <ros_end_effector/UtilsEigen.h>
35 :
36 : #include <ros_end_effector/UtilsYAML.h>
37 :
38 : #include <rosee_msg/HandInfo.h>
39 : #include <rosee_msg/MotorPhalangePressure.h>
40 :
41 : //Macro to be used in each concrete HAL that will define the create_object functions
42 : #define HAL_CREATE_OBJECT(className) \
43 : extern "C" ::ROSEE::EEHal* create_object_##className ( ros::NodeHandle* nh) { \
44 : return new className(nh); \
45 : } \
46 :
47 : namespace ROSEE {
48 :
49 :
50 : /**
51 : * @brief Class representing an end-effector
52 : *
53 : */
54 : class EEHal {
55 :
56 : public:
57 :
58 : typedef std::shared_ptr<EEHal> Ptr;
59 : typedef std::shared_ptr<const EEHal> ConstPtr;
60 :
61 : EEHal ( ros::NodeHandle* nh );
62 41 : virtual ~EEHal() {};
63 :
64 : virtual bool sense() = 0;
65 :
66 : virtual bool move() = 0;
67 :
68 : virtual bool publish_joint_state();
69 :
70 : virtual bool checkCommandPub();
71 :
72 : //virtual bool setMotorPositionReference(std::string motor, double pos) = 0;
73 : //virtual bool getJointPosition(std::string joint, std::vector<double> &pos ) = 0;
74 :
75 : //Matrices needed by Grasp Planner ROSEE
76 : virtual bool getFingersNames(std::vector<std::string> &fingers_names);
77 : virtual bool getMotorsNames(std::vector<std::string> &motors_names);
78 :
79 : virtual bool getMotorStiffness(Eigen::VectorXd &motors_stiffness);
80 : virtual bool getTipsFrictions(Eigen::VectorXd &tips_friction);
81 : virtual bool getMotorTorqueLimits(Eigen::VectorXd &motors_torque_limits);
82 : virtual bool getTipJointToTipFrameX(Eigen::VectorXd &tip_joint_to_tip_frame_x);
83 : virtual bool getTipJointToTipFrameY(Eigen::VectorXd &tip_joint_to_tip_frame_y);
84 :
85 : // These are dependent on hand configuration, hence can not be simply parsed by conf file so
86 : // each derived HAL must implement them with some logic if they want to use the planne
87 0 : virtual bool getTipsJacobiansNormal(std::unordered_map<std::string, Eigen::MatrixXd>& tips_jacobian_normal) {return false;}
88 0 : virtual bool getTipsJacobiansFriction(std::unordered_map<std::string, Eigen::MatrixXd>& tips_jacobian_friction) {return false;}
89 0 : virtual bool getTransmissionMatrix(Eigen::MatrixXd &transmission_matrix) {return false;}
90 0 : virtual bool getTransmissionSquareMatrices(std::unordered_map<std::string, Eigen::MatrixXd>& transmission_square_matrices) {return false;}
91 0 : virtual bool getTransmissionAugmentedMatrices(std::unordered_map<std::string, Eigen::MatrixXd>& transmission_augmented_matrices) {return false;}
92 0 : virtual bool getTipsForcesNormal(Eigen::VectorXd& tips_forces_normal) {return false;}
93 0 : virtual bool getTipsForcesFriction(Eigen::VectorXd& tips_forces_friction) {return false;}
94 :
95 : virtual bool parseHandInfo();
96 :
97 : virtual bool isHandInfoPresent();
98 :
99 : /**
100 : * @brief init the service message with info parsed from yaml file, ie info that will not change according to hand configuration
101 : * A derived HAL can override this if necessary, since this is called by EEHalExecutor
102 : */
103 : virtual bool init_hand_info_response() ;
104 :
105 : /**
106 : * @brief Here service is advertise and callback set: if derived class wants to use different callback,
107 : * they must override this and do :
108 : * _hand_info_server = _nh->advertiseService(_hand_info_service_name, <custom_callback>);
109 : */
110 : virtual bool setHandInfoCallback();
111 :
112 : bool _pressure_active;
113 : bool publish_pressure();
114 :
115 : Eigen::VectorXd getMotorReference() const;
116 : Eigen::VectorXd getJointPosition() const;
117 : Eigen::VectorXd getJointEffort() const;
118 : Eigen::MatrixXd getPressure() const;
119 :
120 :
121 : protected:
122 :
123 : ros::NodeHandle* _nh;
124 :
125 : /**
126 : * The references that must be read in the move() to send to the real/simulated robot
127 : * TODO put private and create a get ? (no set)
128 : */
129 : sensor_msgs::JointState _mr_msg;
130 : ros::Subscriber _motor_reference_sub;
131 :
132 : /**
133 : * The states that must be filled in the sense(), reading info from real/simulated robot
134 : * TODO put private and create a set (no get) ?
135 : */
136 : sensor_msgs::JointState _js_msg;
137 : ros::Publisher _joint_state_pub;
138 :
139 : //Heri specific message
140 : bool initPressureSensing();
141 : rosee_msg::MotorPhalangePressure _pressure_msg;
142 : ros::Publisher _pressure_pub;
143 :
144 :
145 : /**** Hand info matrices***/
146 : std::vector <std::string> fingers_names, motors_names;
147 : Eigen::VectorXd tips_frictions, motors_torque_limits, motors_stiffness,
148 : tip_joint_to_tip_frame_x, tip_joint_to_tip_frame_y;
149 :
150 : ros::ServiceServer _hand_info_server;
151 : rosee_msg::HandInfo::Response _hand_info_response;
152 : std::string _hand_info_service_name;
153 :
154 :
155 : private:
156 :
157 : bool _hand_info_present;
158 :
159 : bool handInfoEEHalCallback(
160 : rosee_msg::HandInfo::Request& request,
161 : rosee_msg::HandInfo::Response& response);
162 :
163 :
164 : void motor_reference_clbk(const sensor_msgs::JointState::ConstPtr& msg);
165 : };
166 : }
167 :
168 : #endif // __ROSEE_EE_HAL__
|
