mia_hand_gazebo
rel 1.0.0
|
Go to the documentation of this file.
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>
22 #include <gazebo/common/common.hh>
23 #include <gazebo/physics/physics.hh>
24 #include <gazebo/gazebo.hh>
28 #include <angles/angles.h>
29 #include <pluginlib/class_list_macros.h>
32 #include <gazebo_ros_control/robot_hw_sim.h>
35 #include <urdf/model.h>
38 #include "mia_hand_description/mia_thumb_opp_passivejoints.h"
66 class MiaHWSim :
public gazebo_ros_control::RobotHWSim
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);
94 virtual void readSim(ros::Time time, ros::Duration period);
102 virtual void writeSim(ros::Time time, ros::Duration period);
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);
162 bool GetMiddlePosition(
double joint_position_m,
double joint_position_r,
double joint_position_l,
double flex_direction);
249 std::vector<gazebo::physics::JointPtr>
sim_joints_;
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.
hardware_interface::VelocityJointInterface vj_interface_
Interface for simulated joint velocity commands.
unsigned int n_dof_
Number of degree of freedom of the simulated hardware.
Joint_index_mapping joints_ii
Joint_index_mapping struct.
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_
Interface for simulated joint effort soft limit.
double jMiddle_StackTarget_position
Target position of the Mia simulated middle flexion joint when ring and/or little get stuck.
double jThOpp_Target_position
Target position of the Mia simulated thumb opposition joint evaluated by this class.
std::vector< control_toolbox::Pid > pid_controllers_
int j_index_flex
Index number of the index_fle joint.
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_
Interface for simulated joint position sodt limit.
std::vector< double > joint_lower_limits_
Lower position limits of the joints of the simulated hardware.
virtual void writeSim(ros::Time time, ros::Duration period)
Send commands to the simulated robot hardware.
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_
Interface for simulated joint effort saturation limit.
int j_thumb_opp
Index number of the thumb_opp joint.
std::vector< std::vector< ControlMethod > > List_joint_control_methods_
List of control method available for the joints of the simulated hardware.
int j_mrl_3
Index number of the mrl_3 (little) joint.
std::vector< double > last_joint_effort_command_
Last effort command of the joints of the simulated hardware.
hardware_interface::EffortJointInterface ej_interface_
Interface for simulated joint effort commands.
double GetThumbOppPosition(ros::Duration period)
Evaluate the position of the Mia thumb opposition joint.
double * j_index_flex_pos
Pointer to the position status of Mia simulated index flexion joint.
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_
Interface for simulated joint velocity saturation limit.
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
Interface for simulated joint position saturation limit.
std::vector< double > last_joint_position_command_
Last position command of the joints of the simulated hardware.
std::vector< double > joint_effort_command_
Actual effort command of the joints of the simulated hardware.
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.
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.
mia_hand::thumb_opp_passive_joint MyTh_opp_passiveJoint
Class used to evaluate the target position of the Mia simulated thumb opposition joint.
int j_mrl_2
Index number of the mrl_2 (ring) joint.
unsigned int write_counter
Counter of the write method.
std::vector< double > joint_upper_limits_
Upper position limits of the joints of the simulated hardware.
std::vector< ControlMethod > joint_control_methods_
Actual control method for the joints of the simulated hardware.
boost::shared_ptr< MiaHWSim > MiaHWSimPtr
std::vector< double > joint_position_command_
Actual position command of the joints of the simulated hardware.
std::vector< double > joint_position_
Actual position state of the joints of the simulated hardware.
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.
std::vector< ControlMethod > last_joint_control_methods_
Last control method used for the joints of the simulated hardware.
std::vector< double > joint_velocity_
Actual velocity state of the joints of the simulated hardware.
std::vector< std::string > joint_names_
Name of the joints of the simulated hardware.
ControlMethod
Joints control methods.
hardware_interface::JointStateInterface js_interface_
Interface for simulated joint state.
std::vector< double > joint_effort_
Actual effort state of the joints of the simulated hardware.
std::vector< double > last_joint_velocity_command_
Last velocity command of the joints of the simulated hardware.
std::vector< double > joint_effort_limits_
Effort upper limits of the joints of the simulated hardware.
std::vector< gazebo::physics::JointPtr > sim_joints_
Gazebo joint pointer.
std::vector< int > joint_types_
Type of the joints of the simulated hardware.
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_
Interface for simulated joint velocity soft limit.
bool mrl_united
If True then middle ring little fingers are physical connected in the URDF, False if they ate indepen...
virtual void eStopActive(const bool active)
Unused.
Struct to store the indexes of each joints in the MiaHWSim Class Index of each joint depends on the o...
virtual void readSim(ros::Time time, ros::Duration period)
Read state data from the simulated robot hardware.
int j_thumb_flex
Index number of the thumb_fle joint.
int j_index_flex_sign
Current sign of the Mia simulated index flexion joint.
std::vector< double > joint_velocity_command_
Actual velocity command of the joints of the simulated hardware.
Class hardware sim interface of the simulated Mia hand.
double j_index_flex_pos_Th
Threshold used for the simulated index flexion position control to switch its sign.
bool last_e_stop_active_
Unused.
hardware_interface::PositionJointInterface pj_interface_
Interface for simulated joint position commands.
int j_mrl_flex
Index number of the mrl_fle (medium) joint.