mia_hand_gazebo  rel 1.0.0
Mia_hw_sim.h
Go to the documentation of this file.
1 /*********************************************************************************************************
2  Author: Prensilia srl
3  Desc: Hardware Interface for mia simulated robot in Gazebo
4 
5  version: 1.6
6 **********************************************************************************************************/
7 
8 
9 #ifndef MIA_HW_SIM_H
10 #define MIA_HW_SIM_H
11 
12 // ros_control
13 #include <control_toolbox/pid.h>
14 #include <hardware_interface/joint_command_interface.h>
15 #include <hardware_interface/robot_hw.h>
16 #include <joint_limits_interface/joint_limits.h>
17 #include <joint_limits_interface/joint_limits_interface.h>
18 #include <joint_limits_interface/joint_limits_rosparam.h>
19 #include <joint_limits_interface/joint_limits_urdf.h>
20 
21 // Gazebo
22 #include <gazebo/common/common.hh>
23 #include <gazebo/physics/physics.hh>
24 #include <gazebo/gazebo.hh>
25 
26 // ROS
27 #include <ros/ros.h>
28 #include <angles/angles.h>
29 #include <pluginlib/class_list_macros.h>
30 
31 // gazebo_ros_control
32 #include <gazebo_ros_control/robot_hw_sim.h> // Inheriting the base class
33 
34 // URDF
35 #include <urdf/model.h>
36 
37 // PassiveJoint Thumb_opp + Index_fle
38 #include "mia_hand_description/mia_thumb_opp_passivejoints.h"
39 
40 
41 namespace mia
42 {
43 
48  struct Joint_index_mapping
49  {
50  int j_thumb_opp;
51  int j_thumb_flex;
52  int j_index_flex;
53  int j_mrl_flex;
54  int j_mrl_2;
55  int j_mrl_3;
56  };
57 
58 
66  class MiaHWSim : public gazebo_ros_control::RobotHWSim
67  {
68 
69  public:
70 
82  virtual bool initSim( const std::string& robot_namespace,
83  ros::NodeHandle model_nh,
84  gazebo::physics::ModelPtr parent_model,
85  const urdf::Model *const urdf_model,
86  std::vector<transmission_interface::TransmissionInfo> transmissions);
87 
94  virtual void readSim(ros::Time time, ros::Duration period);
95 
102  virtual void writeSim(ros::Time time, ros::Duration period);
103 
104  virtual void eStopActive(const bool active);
105 
106  protected:
107 
113 
128  void registerJointLimits(const std::string& joint_name,
129  const std::vector<hardware_interface::JointHandle>& joint_handle,
130  const std::vector<ControlMethod> ctrl_method,
131  const ros::NodeHandle& joint_limit_nh,
132  const urdf::Model *const urdf_model,
133  int *const joint_type, double *const lower_limit,
134  double *const upper_limit, double *const effort_limit);
135 
146  double GetThumbOppPosition(ros::Duration period);
147 
148 
162  bool GetMiddlePosition(double joint_position_m, double joint_position_r, double joint_position_l, double flex_direction);
163 
176  enum ControlMethod SelectCtrMethod( enum MiaHWSim::ControlMethod Default_joint_control_methods_,
178  const double last_joint_velocity_command_,
179  const double joint_velocity_command_,
180  const double last_joint_position_command_,
181  const double joint_position_command_);
182 
183 
190  Joint_index_mapping joints_ii;
191 
192  unsigned int n_dof_;
193  unsigned int write_counter;
194 
195 
196  hardware_interface::JointStateInterface js_interface_;
197  hardware_interface::EffortJointInterface ej_interface_;
198  hardware_interface::PositionJointInterface pj_interface_;
199  hardware_interface::VelocityJointInterface vj_interface_;
200 
201  joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_;
202  joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_;
203  joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
204  joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
205  joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
206  joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
207 
208 
209  std::vector<std::string> joint_names_;
210  std::vector<int> joint_types_;
211  std::vector<double> joint_lower_limits_;
212  std::vector<double> joint_upper_limits_;
213  std::vector<double> joint_effort_limits_;
214 
215  std::vector<double> joint_position_;
216  std::vector<double> joint_velocity_;
217  std::vector<double> joint_effort_;
218  std::vector<double> joint_effort_command_;
219  std::vector<double> last_joint_effort_command_;
220  std::vector<double> joint_position_command_;
221  std::vector<double> last_joint_position_command_;
222  std::vector<double> joint_velocity_command_;
223  std::vector<double> last_joint_velocity_command_;
224  std::vector<ControlMethod> last_joint_control_methods_;
225 
226  std::vector<ControlMethod> joint_control_methods_;
227  std::vector<std::vector<ControlMethod>> List_joint_control_methods_;
228 
229  std::vector<control_toolbox::Pid> pid_controllers_;
230  //
231 
233  double j_index_flex_pos_Th;
235  double jThOpp_Target_position;
237  bool mrl_united;
238 
246  mia_hand::thumb_opp_passive_joint MyTh_opp_passiveJoint;
247 
248 
249  std::vector<gazebo::physics::JointPtr> sim_joints_;
250 
251 
253 
254  };
255 
256 typedef boost::shared_ptr<MiaHWSim> MiaHWSimPtr;
257 
258 }
259 
260 
261 #endif
mia::MiaHWSim::SelectCtrMethod
enum ControlMethod SelectCtrMethod(enum MiaHWSim::ControlMethod Default_joint_control_methods_, 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 simulated hardware.
Definition: Mia_hw_sim.cpp:1161
mia::MiaHWSim::vj_interface_
hardware_interface::VelocityJointInterface vj_interface_
Interface for simulated joint velocity commands.
Definition: Mia_hw_sim.h:204
mia::MiaHWSim::n_dof_
unsigned int n_dof_
Number of degree of freedom of the simulated hardware.
Definition: Mia_hw_sim.h:197
mia::MiaHWSim::VELOCITY
@ VELOCITY
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::joints_ii
Joint_index_mapping joints_ii
Joint_index_mapping struct.
Definition: Mia_hw_sim.h:195
mia::MiaHWSim::ej_limits_interface_
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_
Interface for simulated joint effort soft limit.
Definition: Mia_hw_sim.h:207
mia::MiaHWSim::jMiddle_StackTarget_position
double jMiddle_StackTarget_position
Target position of the Mia simulated middle flexion joint when ring and/or little get stuck.
Definition: Mia_hw_sim.h:241
mia::MiaHWSim::jThOpp_Target_position
double jThOpp_Target_position
Target position of the Mia simulated thumb opposition joint evaluated by this class.
Definition: Mia_hw_sim.h:240
mia::MiaHWSim::pid_controllers_
std::vector< control_toolbox::Pid > pid_controllers_
Definition: Mia_hw_sim.h:234
mia::Joint_index_mapping::j_index_flex
int j_index_flex
Index number of the index_fle joint.
Definition: Mia_hw_sim.h:62
mia::MiaHWSim::pj_limits_interface_
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_
Interface for simulated joint position sodt limit.
Definition: Mia_hw_sim.h:209
mia::MiaHWSim::joint_lower_limits_
std::vector< double > joint_lower_limits_
Lower position limits of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:216
mia::MiaHWSim::EFFORT
@ EFFORT
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::writeSim
virtual void writeSim(ros::Time time, ros::Duration period)
Send commands to the simulated robot hardware.
Definition: Mia_hw_sim.cpp:575
mia::MiaHWSim::ej_sat_interface_
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_
Interface for simulated joint effort saturation limit.
Definition: Mia_hw_sim.h:206
mia::MiaHWSim::POSITION_PID
@ POSITION_PID
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::POSITION
@ POSITION
Definition: Mia_hw_sim.h:117
mia::Joint_index_mapping::j_thumb_opp
int j_thumb_opp
Index number of the thumb_opp joint.
Definition: Mia_hw_sim.h:60
mia::MiaHWSim::VELOCITY_PID
@ VELOCITY_PID
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::List_joint_control_methods_
std::vector< std::vector< ControlMethod > > List_joint_control_methods_
List of control method available for the joints of the simulated hardware.
Definition: Mia_hw_sim.h:232
mia::Joint_index_mapping::j_mrl_3
int j_mrl_3
Index number of the mrl_3 (little) joint.
Definition: Mia_hw_sim.h:65
mia::MiaHWSim::last_joint_effort_command_
std::vector< double > last_joint_effort_command_
Last effort command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:224
mia::MiaHWSim::ej_interface_
hardware_interface::EffortJointInterface ej_interface_
Interface for simulated joint effort commands.
Definition: Mia_hw_sim.h:202
mia::MiaHWSim::GetThumbOppPosition
double GetThumbOppPosition(ros::Duration period)
Evaluate the position of the Mia thumb opposition joint.
Definition: Mia_hw_sim.cpp:1186
mia::MiaHWSim::j_index_flex_pos
double * j_index_flex_pos
Pointer to the position status of Mia simulated index flexion joint.
Definition: Mia_hw_sim.h:237
mia::MiaHWSim::vj_sat_interface_
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_
Interface for simulated joint velocity saturation limit.
Definition: Mia_hw_sim.h:210
mia::MiaHWSim::pj_sat_interface_
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
Interface for simulated joint position saturation limit.
Definition: Mia_hw_sim.h:208
mia::MiaHWSim::last_joint_position_command_
std::vector< double > last_joint_position_command_
Last position command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:226
mia::MiaHWSim::joint_effort_command_
std::vector< double > joint_effort_command_
Actual effort command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:223
mia::MiaHWSim::initSim
virtual bool initSim(const std::string &robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model, const urdf::Model *const urdf_model, std::vector< transmission_interface::TransmissionInfo > transmissions)
Initialize the simulated robot hardware.
Definition: Mia_hw_sim.cpp:31
mia::MiaHWSim::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 simulated hardware.
Definition: Mia_hw_sim.cpp:365
mia::MiaHWSim::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_sim.h:251
mia::Joint_index_mapping::j_mrl_2
int j_mrl_2
Index number of the mrl_2 (ring) joint.
Definition: Mia_hw_sim.h:64
mia::MiaHWSim::write_counter
unsigned int write_counter
Counter of the write method.
Definition: Mia_hw_sim.h:198
mia::MiaHWSim::joint_upper_limits_
std::vector< double > joint_upper_limits_
Upper position limits of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:217
mia::MiaHWSim::joint_control_methods_
std::vector< ControlMethod > joint_control_methods_
Actual control method for the joints of the simulated hardware.
Definition: Mia_hw_sim.h:231
mia::MiaHWSimPtr
boost::shared_ptr< MiaHWSim > MiaHWSimPtr
Definition: Mia_hw_sim.h:261
mia::MiaHWSim::joint_position_command_
std::vector< double > joint_position_command_
Actual position command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:225
mia::MiaHWSim::joint_position_
std::vector< double > joint_position_
Actual position state of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:220
mia::MiaHWSim::GetMiddlePosition
bool GetMiddlePosition(double joint_position_m, double joint_position_r, double joint_position_l, double flex_direction)
Evaluate if any of the Mia simulated middle ring or little flexion joint is stuck.
Definition: Mia_hw_sim.cpp:1211
mia::MiaHWSim::last_joint_control_methods_
std::vector< ControlMethod > last_joint_control_methods_
Last control method used for the joints of the simulated hardware.
Definition: Mia_hw_sim.h:229
mia::MiaHWSim::joint_velocity_
std::vector< double > joint_velocity_
Actual velocity state of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:221
mia::MiaHWSim::joint_names_
std::vector< std::string > joint_names_
Name of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:214
mia::MiaHWSim::ControlMethod
ControlMethod
Joints control methods.
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::js_interface_
hardware_interface::JointStateInterface js_interface_
Interface for simulated joint state.
Definition: Mia_hw_sim.h:201
mia::MiaHWSim::joint_effort_
std::vector< double > joint_effort_
Actual effort state of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:222
mia::MiaHWSim::last_joint_velocity_command_
std::vector< double > last_joint_velocity_command_
Last velocity command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:228
mia::MiaHWSim::joint_effort_limits_
std::vector< double > joint_effort_limits_
Effort upper limits of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:218
mia::MiaHWSim::sim_joints_
std::vector< gazebo::physics::JointPtr > sim_joints_
Gazebo joint pointer.
Definition: Mia_hw_sim.h:254
mia::MiaHWSim::joint_types_
std::vector< int > joint_types_
Type of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:215
mia::MiaHWSim::vj_limits_interface_
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_
Interface for simulated joint velocity soft limit.
Definition: Mia_hw_sim.h:211
mia::MiaHWSim::mrl_united
bool mrl_united
If True then middle ring little fingers are physical connected in the URDF, False if they ate indepen...
Definition: Mia_hw_sim.h:242
mia::MiaHWSim::eStopActive
virtual void eStopActive(const bool active)
Unused.
Definition: Mia_hw_sim.cpp:1258
mia::Joint_index_mapping
Struct to store the indexes of each joints in the MiaHWSim Class Index of each joint depends on the o...
Definition: Mia_hw_sim.h:53
mia
Definition: Mia_hw_sim.h:41
mia::MiaHWSim::readSim
virtual void readSim(ros::Time time, ros::Duration period)
Read state data from the simulated robot hardware.
Definition: Mia_hw_sim.cpp:501
mia::Joint_index_mapping::j_thumb_flex
int j_thumb_flex
Index number of the thumb_fle joint.
Definition: Mia_hw_sim.h:61
mia::MiaHWSim::j_index_flex_sign
int j_index_flex_sign
Current sign of the Mia simulated index flexion joint.
Definition: Mia_hw_sim.h:239
mia::MiaHWSim::e_stop_active_
bool e_stop_active_
Definition: Mia_hw_sim.h:257
mia::MiaHWSim::joint_velocity_command_
std::vector< double > joint_velocity_command_
Actual velocity command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:227
mia::MiaHWSim
Class hardware sim interface of the simulated Mia hand.
Definition: Mia_hw_sim.h:71
mia::MiaHWSim::j_index_flex_pos_Th
double j_index_flex_pos_Th
Threshold used for the simulated index flexion position control to switch its sign.
Definition: Mia_hw_sim.h:238
mia::MiaHWSim::last_e_stop_active_
bool last_e_stop_active_
Unused.
Definition: Mia_hw_sim.h:257
mia::MiaHWSim::pj_interface_
hardware_interface::PositionJointInterface pj_interface_
Interface for simulated joint position commands.
Definition: Mia_hw_sim.h:203
mia::Joint_index_mapping::j_mrl_flex
int j_mrl_flex
Index number of the mrl_fle (medium) joint.
Definition: Mia_hw_sim.h:63