mia_hand_ros_control  rel 1.0.0
mia_hw_interface.h
Go to the documentation of this file.
1  /*********************************************************************************************************
2  Modifyed by Author: Prensilia srl
3  Desc: Hardware interface of the MIA hand
4 
5  version 1.0
6 
7 **********************************************************************************************************/
8 
9 
10 #ifndef MIA_HW_INTERFACE_H
11 #define MIA_HW_INTERFACE_H
12 
13 
14 
15 /*TODO: install all Mia Hand classes under the same folder, such that, to
16  * include them, the macro instruction #include "mia/... will be necessary.
17  "*/
18 
19 #include <hardware_interface/joint_state_interface.h>
20 #include <hardware_interface/joint_command_interface.h>
21 #include <hardware_interface/robot_hw.h>
22 #include <joint_limits_interface/joint_limits.h>
23 #include <joint_limits_interface/joint_limits_interface.h>
24 #include <joint_limits_interface/joint_limits_rosparam.h>
25 #include <joint_limits_interface/joint_limits_urdf.h>
26 #include <controller_manager/controller_manager.h>
27 #include <boost/scoped_ptr.hpp>
28 #include <ros/ros.h>
29 
30 #include <vector>
31 
32 // URDF
33 #include <urdf/model.h>
34 
35 #include <sstream>
36 
37 #include <transmission_interface/transmission_parser.h>
38 #include <transmission_interface/transmission_info.h>
39 #include <transmission_interface/transmission_interface.h>
44 
45 #include "mia_hand_driver/cpp_driver.h"
46 
47 // Passive Joints of the thumb opp and index flexion
48 #include "mia_hand_description/mia_thumb_opp_passivejoints.h"
49 
50 namespace mia_hand
51 {
52 
57  struct Joint_index_mapping
58  {
59  int j_thumb_opp;
60  int j_thumb_flex;
61  int j_index_flex;
62  int j_mrl_flex;
63  int j_mrl_2;
64  int j_mrl_3;
65  };
66 
71  class MiaHWInterface: public hardware_interface::RobotHW
72  {
73  public:
74 
79 
84 
85 
95  bool init(ros::NodeHandle& root_nh, ros::NodeHandle& mia_hw_nh) override;
96 
100  void update(const ros::TimerEvent& e);
101 
108  void read(const ros::Time& time, const ros::Duration& duration) override;
109 
116  void write(const ros::Time& time, const ros::Duration& duration) override;
117 
118 
119  protected:
120 
121 
128  enum ControlMethod {EFFORT, POSITION, VELOCITY}; //enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};
129 
144  void registerJointLimits(const std::string& joint_name,
145  const std::vector<hardware_interface::JointHandle>& joint_handle,
146  const std::vector<ControlMethod> ctrl_method,
147  const ros::NodeHandle& joint_limit_nh,
148  const urdf::Model *const urdf_model,
149  int *const joint_type, double *const lower_limit,
150  double *const upper_limit, double *const effort_limit);
151 
152 
159  void InitBkLastCommands();
160 
173  const double last_joint_velocity_command_,
174  const double joint_velocity_command_,
175  const double last_joint_position_command_,
176  const double joint_position_command_);
177 
178 
189  double GetThumbOppPosition();
190 
198  std::string getURDF(std::string param_name) const;
199 
200 
207  bool parseTransmissionsFromURDF(const std::string& urdf_string);
208 
209 
210  CppDriver mia_;
211  int COM_number_;
212 
213  ros::NodeHandle nh_;
214  // ros::Timer non_realtime_loop_;
215  // ros::Duration control_period_;
216  // ros::Duration elapsed_time_;
217 
221  std::string robot_description_;
222 
226  std::vector<transmission_interface::TransmissionInfo> URDFtransmissions_;
227 
229  // ROS Interfaces
230 
231  hardware_interface::JointStateInterface js_interface_;
232  hardware_interface::PositionJointInterface pj_interface_;
233  hardware_interface::VelocityJointInterface vj_interface_;
234  hardware_interface::EffortJointInterface ej_interface_;
235 
236  joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
237  joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
238  joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
239  joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
240 
241  joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_;
242  joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_;
243 
245  //Software transmissions.
246 
252 
258 
264 
268  std::vector<transmission_interface::Transmission*> MiaTrasmissions;
269 
270  std::vector<std::string> trasmission_names_;
271 
272 
277  transmission_interface::ActuatorToJointPositionInterface act_to_jnt_pos_state;
278 
283  transmission_interface::ActuatorToJointVelocityInterface act_to_jnt_vel_state;
284 
289  transmission_interface::JointToActuatorPositionInterface jnt_to_act_pos;
290 
295  transmission_interface::JointToActuatorVelocityInterface jnt_to_act_vel;
296 
302 
308 
314 
320 
321 
322  // Actuator and joint space variables: wrappers around raw data
323 
324  transmission_interface::ActuatorData a_state_data[3];
325  transmission_interface::ActuatorData a_cmd_data[3];
326 
327  transmission_interface::JointData j_state_data[3];
328  transmission_interface::JointData j_cmd_data[3];
329 
337  mia_hand::thumb_opp_passive_joint MyTh_opp_passiveJoint; // Passivejoint thumb opp and index_fle
338 
340  // Others
341 
349 
350  unsigned int write_counter;
351  int n_dof_sim_;
352  int n_actuators_;
353 
354 
355  std::vector<std::string> joint_names_;
356  std::vector<int> joint_types_;
357 
358  std::vector<double> joint_position_state_;
359  std::vector<double> joint_velocity_state_;
360  std::vector<double> joint_effort_state_;
361 
362  std::vector<double> joint_position_command_;
363  std::vector<double> joint_velocity_command_;
364  std::vector<double> joint_effort_command_;
365 
366  std::vector<double> last_joint_position_command_;
367  std::vector<double> last_joint_velocity_command_;
368 
369  std::vector<double> act_position_state_;
370  std::vector<double> act_velocity_state_;
371  std::vector<double> act_effort_state_;
372 
373  std::vector<double> act_position_command_;
374  std::vector<double> act_velocity_command_;
375  std::vector<double> act_effort_command_;
376 
377 
378  std::vector<ControlMethod> joint_control_methods_;
379  std::vector<ControlMethod> last_joint_control_methods_;
380  std::vector<std::vector<ControlMethod>> List_joint_control_methods_;
381 
382  std::vector<double> joint_lower_limits_;
383  std::vector<double> joint_upper_limits_;
384 
385  std::vector<double> joint_effort_limits_;
386 
387  // TO DELETE Needed to fix the problem that velocity returned by MIA is abs
388  std::vector<double> old_act_position_state_;
390 
391 
392 
393  };
394 } // namespace
395 
396 #endif
mia_hand::MiaHWInterface::joint_position_command_
std::vector< double > joint_position_command_
Actual position joint command for the Mia hardware.
Definition: mia_hw_interface.h:368
mia_hand::MiaHWInterface::SelectCtrMethod
enum ControlMethod SelectCtrMethod(enum ControlMethod last_joint_control_methods_, const double last_joint_velocity_command_, const double joint_velocity_command_, const double last_joint_position_command_, const double joint_position_command_)
Select the control method to move the real mia hardware.
Definition: mia_hw_interface.cpp:622
mia_transmission_interface.h
mia_hand::MiaHWInterface::MrlTrans
transmission_interface::MiaMrlTransmission MrlTrans
Transmissions class implemented for the Mia hand mrl flexion joint.
Definition: mia_hw_interface.h:263
mia_hand::MiaHWInterface::n_dof_sim_
int n_dof_sim_
Number of degree of freedom.
Definition: mia_hw_interface.h:357
mia_hand::MiaHWInterface::read
void read(const ros::Time &time, const ros::Duration &duration) override
Read state data from the real robot hardware.
Definition: mia_hw_interface.cpp:351
mia_hand::MiaHWInterface::act_effort_state_
std::vector< double > act_effort_state_
Useless.
Definition: mia_hw_interface.h:377
mia_hand::MiaHWInterface::vj_interface_
hardware_interface::VelocityJointInterface vj_interface_
Interface for the real Mia joint velocity commands.
Definition: mia_hw_interface.h:239
mia_hand::MiaHWInterface::vj_sat_interface_
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_
Interface for the real Mia joint velocity saturation limit.
Definition: mia_hw_interface.h:244
transmission_interface::MiaMrlTransmission
Implementation of the linear Mia mrl Transmission mapping the positions and velocity from the joint s...
Definition: mia_mrl_transmission.h:60
mia_hand::MiaHWInterface::index_act_to_jnt_pos_state
transmission_interface::MiaActuatorToJointPositionInterface index_act_to_jnt_pos_state
Transmission interface for propagating current actuator position state of the Mia index to joint spac...
Definition: mia_hw_interface.h:307
mia_hand::MiaHWInterface::act_velocity_command_
std::vector< double > act_velocity_command_
Actual velocity actuator command for the Mia hardware.
Definition: mia_hw_interface.h:380
mia_hand::MiaHWInterface::write_counter
unsigned int write_counter
Counter of the write method.
Definition: mia_hw_interface.h:356
mia_thfle_transmission.h
mia_hand::Joint_index_mapping::j_mrl_3
int j_mrl_3
Definition: mia_hw_interface.h:76
mia_mrl_transmission.h
mia_hand::MiaHWInterface::joint_effort_limits_
std::vector< double > joint_effort_limits_
Effort limit of the joints of the Mia hardware specified in the URDF (fake).
Definition: mia_hw_interface.h:391
transmission_interface::MiaActuatorToJointPositionInterface
Definition: mia_transmission_interface.h:422
mia_hand::Joint_index_mapping
Struct to store the indexes of each joints in the MiaHWInterface Class Index of each joint depends on...
Definition: mia_hw_interface.h:63
mia_hand::MiaHWInterface::index_jnt_to_act_vel
transmission_interface::MiaJointToActuatorVelocityInterface index_jnt_to_act_vel
Transmission interface for propagating joint velocity commands to actuator space.
Definition: mia_hw_interface.h:325
mia_hand::MiaHWInterface::MiaHWInterface
MiaHWInterface()
Class constructor.
Definition: mia_hw_interface.cpp:23
mia_hand::MiaHWInterface::pj_sat_interface_
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
Interface for the real Mia joint position saturation limit.
Definition: mia_hw_interface.h:242
mia_hand::MiaHWInterface::a_cmd_data
transmission_interface::ActuatorData a_cmd_data[3]
Actuator command data (one for transmission).
Definition: mia_hw_interface.h:331
mia_hand::MiaHWInterface::mia_
CppDriver mia_
Low level driver of the Mia hand.
Definition: mia_hw_interface.h:216
transmission_interface::MiaIndexTransmission
Implementation of the no-linear Mia Index Transmission.
Definition: mia_index_transmission.h:34
mia_hand::Joint_index_mapping::j_mrl_flex
int j_mrl_flex
Definition: mia_hw_interface.h:74
mia_hand::MiaHWInterface::init
bool init(ros::NodeHandle &root_nh, ros::NodeHandle &mia_hw_nh) override
Initialize the class hardware interface and the robot hardware.
Definition: mia_hw_interface.cpp:60
mia_hand::MiaHWInterface::last_joint_control_methods_
std::vector< ControlMethod > last_joint_control_methods_
Last control method used for the joints of the Mia hardware.
Definition: mia_hw_interface.h:385
mia_hand::MiaHWInterface::joint_velocity_state_
std::vector< double > joint_velocity_state_
Actual velocity state of the joints of the Mia hardware.
Definition: mia_hw_interface.h:365
mia_hand::MiaHWInterface::act_position_command_
std::vector< double > act_position_command_
Actual position actuator command for the Mia hardware.
Definition: mia_hw_interface.h:379
mia_hand::MiaHWInterface::joint_position_state_
std::vector< double > joint_position_state_
Actual position state of the joints of the Mia hardware.
Definition: mia_hw_interface.h:364
mia_hand::MiaHWInterface::act_to_jnt_vel_state
transmission_interface::ActuatorToJointVelocityInterface act_to_jnt_vel_state
Transmission interface for propagating current velocity actuator state to joint space.
Definition: mia_hw_interface.h:289
mia_hand::MiaHWInterface::act_effort_command_
std::vector< double > act_effort_command_
Useless.
Definition: mia_hw_interface.h:381
mia_index_transmission.h
mia_hand::MiaHWInterface::robot_description_
std::string robot_description_
Generic name of the URDF to look for with #getURDF(std::string param_name).
Definition: mia_hw_interface.h:227
mia_hand::Joint_index_mapping::j_index_flex
int j_index_flex
Definition: mia_hw_interface.h:73
mia_hand::MiaHWInterface::joint_control_methods_
std::vector< ControlMethod > joint_control_methods_
Actual control method for the joints of the Mia hardware.
Definition: mia_hw_interface.h:384
transmission_interface::MiaJointToActuatorVelocityInterface
Definition: mia_transmission_interface.h:431
mia_hand::MiaHWInterface::EFFORT
@ EFFORT
Definition: mia_hw_interface.h:134
mia_hand::MiaHWInterface::ThfleTrans
transmission_interface::MiaThfleTransmission ThfleTrans
Transmissions class implemented for the Mia hand thumb flexion joint.
Definition: mia_hw_interface.h:257
mia_hand::MiaHWInterface::act_velocity_state_
std::vector< double > act_velocity_state_
Actual velocity state of the actuators of the Mia hardware.
Definition: mia_hw_interface.h:376
mia_hand::MiaHWInterface::ej_interface_
hardware_interface::EffortJointInterface ej_interface_
unused
Definition: mia_hw_interface.h:240
mia_hand::MiaHWInterface::COM_number_
int COM_number_
Number of the COM in which the Mia hand is plugged.
Definition: mia_hw_interface.h:217
mia_hand::MiaHWInterface::write
void write(const ros::Time &time, const ros::Duration &duration) override
Write data command to the real robot hardware.
Definition: mia_hw_interface.cpp:396
mia_hand::MiaHWInterface::ej_sat_interface_
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_
unused
Definition: mia_hw_interface.h:247
mia_hand::Joint_index_mapping::j_thumb_opp
int j_thumb_opp
Definition: mia_hw_interface.h:71
mia_hand::MiaHWInterface::pj_interface_
hardware_interface::PositionJointInterface pj_interface_
Interface for the real Mia joint position commands.
Definition: mia_hw_interface.h:238
mia_hand::MiaHWInterface::joint_effort_state_
std::vector< double > joint_effort_state_
Useless.
Definition: mia_hw_interface.h:366
mia_hand::MiaHWInterface::joint_effort_command_
std::vector< double > joint_effort_command_
Useless.
Definition: mia_hw_interface.h:370
mia_hand::MiaHWInterface::pj_limits_interface_
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_
Interface for the real joint position soft limit.
Definition: mia_hw_interface.h:243
mia_hand::MiaHWInterface::List_joint_control_methods_
std::vector< std::vector< ControlMethod > > List_joint_control_methods_
List of control method available for the joints of the Mia hardware.
Definition: mia_hw_interface.h:386
mia_hand::MiaHWInterface::jnt_to_act_vel
transmission_interface::JointToActuatorVelocityInterface jnt_to_act_vel
Transmission interface for propagating joint velocity commands to actuator space Transmission interfa...
Definition: mia_hw_interface.h:301
mia_hand::MiaHWInterface::j_cmd_data
transmission_interface::JointData j_cmd_data[3]
joint command data (one for transmission).
Definition: mia_hw_interface.h:334
mia_hand::MiaHWInterface::vj_limits_interface_
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_
Interface for the real Mia joint velocity software limit.
Definition: mia_hw_interface.h:245
mia_hand::MiaHWInterface::MyTh_opp_passiveJoint
mia_hand::thumb_opp_passive_joint MyTh_opp_passiveJoint
Class used to evaluate the target position of the Mia simulated thumb opposition joint.
Definition: mia_hw_interface.h:343
mia_hand::MiaHWInterface::joint_lower_limits_
std::vector< double > joint_lower_limits_
Lower position limits of the joints of the Mia hardware (as specified in the URDF)
Definition: mia_hw_interface.h:388
mia_hand::MiaHWInterface
Class hardware interface of the real Mia hand.
Definition: mia_hw_interface.h:77
mia_hand::MiaHWInterface::ej_limits_interface_
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_
unused
Definition: mia_hw_interface.h:248
mia_hand::MiaHWInterface::js_interface_
hardware_interface::JointStateInterface js_interface_
Interface for the real Mia joint state.
Definition: mia_hw_interface.h:237
mia_hand::MiaHWInterface::joint_upper_limits_
std::vector< double > joint_upper_limits_
Upper position limits of the joints of the Mia hardware (as specified in the URDF).
Definition: mia_hw_interface.h:389
mia_hand::MiaHWInterface::last_joint_velocity_command_
std::vector< double > last_joint_velocity_command_
Last joint velocity command received for the Mia hardware.
Definition: mia_hw_interface.h:373
mia_hand::MiaHWInterface::~MiaHWInterface
~MiaHWInterface()
Class distructor.
Definition: mia_hw_interface.cpp:48
mia_hand::MiaHWInterface::registerJointLimits
void registerJointLimits(const std::string &joint_name, const std::vector< hardware_interface::JointHandle > &joint_handle, const std::vector< ControlMethod > ctrl_method, const ros::NodeHandle &joint_limit_nh, const urdf::Model *const urdf_model, int *const joint_type, double *const lower_limit, double *const upper_limit, double *const effort_limit)
Register the joint limits of the real mia hardware.
Definition: mia_hw_interface.cpp:485
mia_hand::MiaHWInterface::read_counter
int read_counter
Counter for the read() funtion.
Definition: mia_hw_interface.h:395
mia_hand::MiaHWInterface::trasmission_names_
std::vector< std::string > trasmission_names_
Actual transmissions name.
Definition: mia_hw_interface.h:276
mia_hand
Definition: mia_hw_interface.h:50
mia_hand::MiaHWInterface::update
void update(const ros::TimerEvent &e)
Initialize the class hardware interface and the robot hardware.
mia_hand::MiaHWInterface::index_act_to_jnt_vel_state
transmission_interface::MiaActuatorToJointVelocityInterface index_act_to_jnt_vel_state
Transmission interface for propagating current actuator velocity state of the Mia index to joint spac...
Definition: mia_hw_interface.h:313
mia_hand::MiaHWInterface::last_joint_position_command_
std::vector< double > last_joint_position_command_
Last joint position command received for the Mia hardware.
Definition: mia_hw_interface.h:372
mia_hand::MiaHWInterface::nh_
ros::NodeHandle nh_
ROS node handle.
Definition: mia_hw_interface.h:219
mia_hand::MiaHWInterface::MiaTrasmissions
std::vector< transmission_interface::Transmission * > MiaTrasmissions
Transmissions vector to handle the transmissions implemented for the real Mia hand.
Definition: mia_hw_interface.h:274
transmission_interface::MiaJointToActuatorPositionInterface
Definition: mia_transmission_interface.h:429
mia_hand::MiaHWInterface::index_jnt_to_act_pos
transmission_interface::MiaJointToActuatorPositionInterface index_jnt_to_act_pos
Transmission interface for propagating joint position commands to actuator space.
Definition: mia_hw_interface.h:319
mia_hand::MiaHWInterface::a_state_data
transmission_interface::ActuatorData a_state_data[3]
Actuator state data (one for transmission).
Definition: mia_hw_interface.h:330
mia_hand::MiaHWInterface::act_position_state_
std::vector< double > act_position_state_
Actual position state of the actuators of the Mia hardware.
Definition: mia_hw_interface.h:375
mia_hand::MiaHWInterface::IndexTrans
transmission_interface::MiaIndexTransmission IndexTrans
Transmissions class implemented for the Mia hand mrl flexion joint.
Definition: mia_hw_interface.h:269
mia_hand::MiaHWInterface::joint_names_
std::vector< std::string > joint_names_
Name of the joints as specified in the URDF.
Definition: mia_hw_interface.h:361
mia_hand::MiaHWInterface::joint_types_
std::vector< int > joint_types_
Type of the joints specified in the URDF.
Definition: mia_hw_interface.h:362
mia_hand::MiaHWInterface::URDFtransmissions_
std::vector< transmission_interface::TransmissionInfo > URDFtransmissions_
Transmissions declared in the URDF.
Definition: mia_hw_interface.h:232
mia_hand::MiaHWInterface::joints_ii
Joint_index_mapping joints_ii
Joint_index_mapping struct.
Definition: mia_hw_interface.h:354
mia_hand::MiaHWInterface::act_to_jnt_pos_state
transmission_interface::ActuatorToJointPositionInterface act_to_jnt_pos_state
Transmission interface for propagating current actuator position state to joint space.
Definition: mia_hw_interface.h:283
mia_hand::MiaHWInterface::POSITION
@ POSITION
Definition: mia_hw_interface.h:134
mia_hand::MiaHWInterface::old_act_position_state_
std::vector< double > old_act_position_state_
Last position state of the actuator returned by the Mia hardware in the read() funtion.
Definition: mia_hw_interface.h:394
mia_hand::MiaHWInterface::VELOCITY
@ VELOCITY
Definition: mia_hw_interface.h:134
mia_hand::MiaHWInterface::InitBkLastCommands
void InitBkLastCommands()
Initialization of the arrays saving the last command received.
Definition: mia_hw_interface.cpp:466
mia_hand::MiaHWInterface::joint_velocity_command_
std::vector< double > joint_velocity_command_
Actual velocity joint command for the Mia hardware.
Definition: mia_hw_interface.h:369
mia_hand::MiaHWInterface::GetThumbOppPosition
double GetThumbOppPosition()
Evaluate the position of the Mia thumb opposition joint.
Definition: mia_hw_interface.cpp:609
mia_hand::MiaHWInterface::n_actuators_
int n_actuators_
Definition: mia_hw_interface.h:358
mia_hand::MiaHWInterface::jnt_to_act_pos
transmission_interface::JointToActuatorPositionInterface jnt_to_act_pos
Transmission interface for propagating joint position commands to actuator space.
Definition: mia_hw_interface.h:295
transmission_interface::MiaThfleTransmission
Implementation of the linear Mia thumb flexion transmission mapping the positions and velocity from t...
Definition: mia_thfle_transmission.h:60
transmission_interface::MiaActuatorToJointVelocityInterface
Definition: mia_transmission_interface.h:424
mia_hand::MiaHWInterface::j_state_data
transmission_interface::JointData j_state_data[3]
Joint state data (one for transmission).
Definition: mia_hw_interface.h:333
mia_hand::Joint_index_mapping::j_mrl_2
int j_mrl_2
Definition: mia_hw_interface.h:75
mia_hand::Joint_index_mapping::j_thumb_flex
int j_thumb_flex
Definition: mia_hw_interface.h:72
mia_hand::MiaHWInterface::getURDF
std::string getURDF(std::string param_name) const
Get the name of the URDF.
Definition: mia_hw_interface.cpp:642
mia_hand::MiaHWInterface::parseTransmissionsFromURDF
bool parseTransmissionsFromURDF(const std::string &urdf_string)
Get the transmission names and type from the loaded URDF Get the trasnsmission names specified in the...
Definition: mia_hw_interface.cpp:676
mia_hand::MiaHWInterface::ControlMethod
ControlMethod
Joints control methods.
Definition: mia_hw_interface.h:134