mia_hand_ros_control
rel 1.0.0
|
Go to the documentation of this file.
12 using hardware_interface::JointStateHandle;
13 using hardware_interface::JointHandle;
14 using transmission_interface::JointToActuatorPositionHandle;
15 using transmission_interface::JointToActuatorVelocityHandle;
16 using transmission_interface::ActuatorToJointStateHandle;
32 if (ros::param::has(
"~Mia_COM_"))
39 ros::param::set(
"~Mia_COM_", 1);
51 mia_.switchPosStream(
false);
52 mia_.switchSpeStream(
false);
63 ROS_INFO_NAMED(
"MiaHWInterface",
"Initialization of MiaHWInterface" );
66 const bool load =
false;
76 std::string info_msg =
"/dev/ttyUSB successfully opened.";
78 ROS_INFO_NAMED(
"MiaHWInterface",
"%s\n", info_msg.c_str());
82 std::string info_msg =
"/dev/ttyUSB can not be open.";
84 ROS_ERROR_NAMED(
"MiaHWInterface",
"%s\n", info_msg.c_str());
89 if(!ros::isInitialized())
91 ROS_FATAL_STREAM_NAMED(
"MiaHWInterface",
"Ros has not been initialized");
102 ROS_ERROR_NAMED(
"MiaHWInterface",
"Error parsing URDF in MiaHWInterface, MiaHWInterface not active.\n");
107 urdf::Model urdf_model;
108 const urdf::Model *
const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
113 const ros::NodeHandle joint_limit_nh(
nh_);
158 for(
unsigned int j=0; j < URDF_n_dof_sim_; j++)
163 ROS_WARN_STREAM_NAMED(
"MiaHWInterface",
"URDFTransmission " <<
URDFtransmissions_[j].name_
164 <<
" has no associated joints.");
168 std::vector<std::string> joint_interfaces =
URDFtransmissions_[j].joints_[0].hardware_interfaces_;
170 if (joint_interfaces.empty() &&
177 if (joint_interfaces.empty())
179 ROS_WARN_STREAM_NAMED(
"MiaHWInterface",
"Joint " <<
URDFtransmissions_[j].joints_[0].name_ <<
180 " of URDFtransmission " <<
URDFtransmissions_[j].name_ <<
" does not specify any hardware interface. " <<
181 "Not adding it to the robot hardware simulation.");
191 if (temp_joint_name ==
"j_thumb_fle" )
198 else if (temp_joint_name ==
"j_index_fle" )
206 else if (temp_joint_name ==
"j_mrl_fle" )
213 else if (temp_joint_name ==
"j_ring_fle" )
218 else if (temp_joint_name ==
"j_little_fle" )
223 else if (temp_joint_name ==
"j_thumb_opp" )
257 js_interface_.registerHandle(hardware_interface::JointStateHandle(
261 std::vector< hardware_interface::JointHandle> joint_handle;
262 joint_handle.resize(2);
280 joint_limit_nh, urdf_model_ptr,
288 if( k >= 0 && k <= 2)
341 mia_.switchPosStream(
true);
342 mia_.switchSpeStream(
true);
486 const std::vector<hardware_interface::JointHandle>& joint_handle,
487 const std::vector<ControlMethod> ctrl_method,
488 const ros::NodeHandle& joint_limit_nh,
489 const urdf::Model *
const urdf_model,
490 int *
const joint_type,
double *
const lower_limit,
491 double *
const upper_limit,
double *
const effort_limit)
494 *joint_type = urdf::Joint::UNKNOWN;
495 *lower_limit = -std::numeric_limits<double>::max();
496 *upper_limit = std::numeric_limits<double>::max();
497 *effort_limit = std::numeric_limits<double>::max();
499 joint_limits_interface::JointLimits limits;
500 bool has_limits =
false;
501 joint_limits_interface::SoftJointLimits soft_limits;
502 bool has_soft_limits =
false;
505 if (urdf_model != NULL)
507 const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name);
509 if (urdf_joint != NULL)
511 *joint_type = urdf_joint->type;
514 if (joint_limits_interface::getJointLimits(urdf_joint, limits))
517 if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
518 has_soft_limits =
true;
524 if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits))
530 if (limits.has_position_limits)
532 *lower_limit = limits.min_position;
533 *upper_limit = limits.max_position;
535 if (limits.has_effort_limits)
536 *effort_limit = limits.max_effort;
540 for (
unsigned int cm = 0; cm< ctrl_method.size(); cm++)
542 switch (ctrl_method[cm])
546 const joint_limits_interface::EffortJointSoftLimitsHandle
547 limits_handle(joint_handle[cm], limits, soft_limits);
553 const joint_limits_interface::PositionJointSoftLimitsHandle
554 limits_handle(joint_handle[cm], limits, soft_limits);
560 const joint_limits_interface::VelocityJointSoftLimitsHandle
561 limits_handle(joint_handle[cm], limits, soft_limits);
570 for (
unsigned int cm = 0; cm< ctrl_method.size(); cm++)
572 switch (ctrl_method[cm])
577 const joint_limits_interface::EffortJointSaturationHandle
578 sat_handle(joint_handle[cm], limits);
584 const joint_limits_interface::PositionJointSaturationHandle
585 sat_handle(joint_handle[cm], limits);
591 const joint_limits_interface::VelocityJointSaturationHandle
592 sat_handle(joint_handle[cm], limits);
615 return jThOpp_Target_position;
624 const double last_joint_velocity_command_,
625 const double joint_velocity_command_,
626 const double last_joint_position_command_,
627 const double joint_position_command_)
644 std::string urdf_string;
647 while (urdf_string.empty())
649 std::string search_param_name;
651 if (
nh_.searchParam(param_name, search_param_name))
653 ROS_INFO_ONCE_NAMED(
"MiaHWInterface",
"MiaHWInterface is waiting for model"
654 " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
656 nh_.getParam(search_param_name, urdf_string);
660 ROS_INFO_ONCE_NAMED(
"MiaHWInterface",
"MiaHWInterface is waiting for model"
663 nh_.getParam(param_name, urdf_string);
668 ROS_DEBUG_STREAM_NAMED(
"MiaHWInterface",
"Recieved urdf from param server, parsing...");
std::vector< double > joint_position_command_
Actual position joint command for the Mia hardware.
Class handling for propagating actuator positions to joint positions for a given MiaIndexTransmission...
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.
Class handling for propagating actuator velocity to joint velocity for a given MiaIndexTransmission.
transmission_interface::MiaMrlTransmission MrlTrans
Transmissions class implemented for the Mia hand mrl flexion joint.
int n_dof_sim_
Number of degree of freedom.
void read(const ros::Time &time, const ros::Duration &duration) override
Read state data from the real robot hardware.
std::vector< double > act_effort_state_
Useless.
hardware_interface::VelocityJointInterface vj_interface_
Interface for the real Mia joint velocity commands.
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_
Interface for the real Mia joint velocity saturation limit.
transmission_interface::MiaActuatorToJointPositionInterface index_act_to_jnt_pos_state
Transmission interface for propagating current actuator position state of the Mia index to joint spac...
std::vector< double > act_velocity_command_
Actual velocity actuator command for the Mia hardware.
unsigned int write_counter
Counter of the write method.
std::vector< double > joint_effort_limits_
Effort limit of the joints of the Mia hardware specified in the URDF (fake).
transmission_interface::MiaJointToActuatorVelocityInterface index_jnt_to_act_vel
Transmission interface for propagating joint velocity commands to actuator space.
MiaHWInterface()
Class constructor.
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
Interface for the real Mia joint position saturation limit.
transmission_interface::ActuatorData a_cmd_data[3]
Actuator command data (one for transmission).
CppDriver mia_
Low level driver of the Mia hand.
bool init(ros::NodeHandle &root_nh, ros::NodeHandle &mia_hw_nh) override
Initialize the class hardware interface and the robot hardware.
Class handling for propagating joint positions to actuator positions for a given MiaIndexTransmission...
std::vector< ControlMethod > last_joint_control_methods_
Last control method used for the joints of the Mia hardware.
std::vector< double > joint_velocity_state_
Actual velocity state of the joints of the Mia hardware.
std::vector< double > act_position_command_
Actual position actuator command for the Mia hardware.
std::vector< double > joint_position_state_
Actual position state of the joints of the Mia hardware.
Class handling for propagating joint velocities to actuator velocities for a given MiaIndexTransmissi...
transmission_interface::ActuatorToJointVelocityInterface act_to_jnt_vel_state
Transmission interface for propagating current velocity actuator state to joint space.
std::vector< double > act_effort_command_
Useless.
std::string robot_description_
Generic name of the URDF to look for with #getURDF(std::string param_name).
std::vector< ControlMethod > joint_control_methods_
Actual control method for the joints of the Mia hardware.
transmission_interface::MiaThfleTransmission ThfleTrans
Transmissions class implemented for the Mia hand thumb flexion joint.
std::vector< double > act_velocity_state_
Actual velocity state of the actuators of the Mia hardware.
hardware_interface::EffortJointInterface ej_interface_
unused
int COM_number_
Number of the COM in which the Mia hand is plugged.
void write(const ros::Time &time, const ros::Duration &duration) override
Write data command to the real robot hardware.
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_
unused
hardware_interface::PositionJointInterface pj_interface_
Interface for the real Mia joint position commands.
std::vector< double > joint_effort_state_
Useless.
std::vector< double > joint_effort_command_
Useless.
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_
Interface for the real joint position soft limit.
std::vector< std::vector< ControlMethod > > List_joint_control_methods_
List of control method available for the joints of the Mia hardware.
transmission_interface::JointToActuatorVelocityInterface jnt_to_act_vel
Transmission interface for propagating joint velocity commands to actuator space Transmission interfa...
transmission_interface::JointData j_cmd_data[3]
joint command data (one for transmission).
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_
Interface for the real Mia joint velocity software limit.
mia_hand::thumb_opp_passive_joint MyTh_opp_passiveJoint
Class used to evaluate the target position of the Mia simulated thumb opposition joint.
std::vector< double > joint_lower_limits_
Lower position limits of the joints of the Mia hardware (as specified in the URDF)
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_
unused
hardware_interface::JointStateInterface js_interface_
Interface for the real Mia joint state.
std::vector< double > joint_upper_limits_
Upper position limits of the joints of the Mia hardware (as specified in the URDF).
std::vector< double > last_joint_velocity_command_
Last joint velocity command received for the Mia hardware.
~MiaHWInterface()
Class distructor.
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.
int read_counter
Counter for the read() funtion.
std::vector< std::string > trasmission_names_
Actual transmissions name.
transmission_interface::MiaActuatorToJointVelocityInterface index_act_to_jnt_vel_state
Transmission interface for propagating current actuator velocity state of the Mia index to joint spac...
std::vector< double > last_joint_position_command_
Last joint position command received for the Mia hardware.
ros::NodeHandle nh_
ROS node handle.
std::vector< transmission_interface::Transmission * > MiaTrasmissions
Transmissions vector to handle the transmissions implemented for the real Mia hand.
transmission_interface::MiaJointToActuatorPositionInterface index_jnt_to_act_pos
Transmission interface for propagating joint position commands to actuator space.
transmission_interface::ActuatorData a_state_data[3]
Actuator state data (one for transmission).
std::vector< double > act_position_state_
Actual position state of the actuators of the Mia hardware.
transmission_interface::MiaIndexTransmission IndexTrans
Transmissions class implemented for the Mia hand mrl flexion joint.
std::vector< std::string > joint_names_
Name of the joints as specified in the URDF.
std::vector< int > joint_types_
Type of the joints specified in the URDF.
std::vector< transmission_interface::TransmissionInfo > URDFtransmissions_
Transmissions declared in the URDF.
Joint_index_mapping joints_ii
Joint_index_mapping struct.
transmission_interface::ActuatorToJointPositionInterface act_to_jnt_pos_state
Transmission interface for propagating current actuator position state to joint space.
std::vector< double > old_act_position_state_
Last position state of the actuator returned by the Mia hardware in the read() funtion.
void InitBkLastCommands()
Initialization of the arrays saving the last command received.
std::vector< double > joint_velocity_command_
Actual velocity joint command for the Mia hardware.
double GetThumbOppPosition()
Evaluate the position of the Mia thumb opposition joint.
transmission_interface::JointToActuatorPositionInterface jnt_to_act_pos
Transmission interface for propagating joint position commands to actuator space.
transmission_interface::JointData j_state_data[3]
Joint state data (one for transmission).
std::string getURDF(std::string param_name) const
Get the name of the URDF.
bool parseTransmissionsFromURDF(const std::string &urdf_string)
Get the transmission names and type from the loaded URDF Get the trasnsmission names specified in the...
ControlMethod
Joints control methods.