20 double clamp(
const double val,
const double min_val,
const double max_val)
22 return std::min(std::max(val, min_val), max_val);
32 ros::NodeHandle model_nh,
33 gazebo::physics::ModelPtr parent_model,
34 const urdf::Model *
const urdf_model,
35 std::vector<transmission_interface::TransmissionInfo> transmissions)
38 ROS_INFO_NAMED(
"mia_hw_sim",
"Initialization of Mia Hw_Sim" );
41 const bool load =
false;
46 const ros::NodeHandle joint_limit_nh(model_nh);
50 n_dof_ = transmissions.size();
75 for(
unsigned int j=0; j <
n_dof_; j++)
79 const std::string jname = transmissions[j].joints_[0].name_;
80 if (jname ==
"j_middle_fle" )
86 else if (jname ==
"j_ring_fle" )
92 else if (jname ==
"j_little_fle" )
103 ROS_INFO_STREAM_NAMED(
"mia_hw_sim",
" middle ring and little as indipendent joints ");
107 ROS_INFO_STREAM_NAMED(
"mia_hw_sim",
" middle ring and little as physically united joints ") ;
112 for(
unsigned int j=0; j <
n_dof_; j++)
116 if(transmissions[j].joints_.size() == 0)
118 ROS_WARN_STREAM_NAMED(
"mia_hw_sim",
"Transmission " << transmissions[j].name_
119 <<
" has no associated joints.");
122 else if(transmissions[j].joints_.size() > 1)
124 ROS_WARN_STREAM_NAMED(
"mia_hw_sim",
"Transmission " << transmissions[j].name_
125 <<
" has more than one joint. Currently the default robot hardware simulation "
126 <<
" interface only supports one.");
133 std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
135 if (joint_interfaces.empty() &&
136 !(transmissions[j].actuators_.empty()) &&
137 !(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
139 joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
140 ROS_WARN_STREAM_NAMED(
"mia_hw_sim",
"The <hardware_interface> element of tranmission " <<
141 transmissions[j].name_ <<
" should be nested inside the <joint> element, not <actuator>. " <<
142 "The transmission will be properly loaded, but please update " <<
143 "your robot model to remain compatible with future versions of the plugin.");
145 if (joint_interfaces.empty())
147 ROS_WARN_STREAM_NAMED(
"mia_hw_sim",
"Joint " << transmissions[j].joints_[0].name_ <<
148 " of transmission " << transmissions[j].name_ <<
" does not specify any hardware interface. " <<
149 "Not adding it to the robot hardware simulation.");
152 else if (joint_interfaces.size() > 1)
154 ROS_WARN_STREAM_NAMED(
"mia_hw_sim",
"Joint " << transmissions[j].joints_[0].name_ <<
155 " of transmission " << transmissions[j].name_ <<
" specifies multiple hardware interfaces. " <<
156 "Currently the default robot hardware simulation interface only supports one. Using the first entry!");
164 const std::string temp_joint_name = transmissions[j].joints_[0].name_;
166 if (temp_joint_name ==
"j_thumb_opp" )
170 else if (temp_joint_name ==
"j_thumb_fle" )
174 else if (temp_joint_name ==
"j_index_fle" )
178 else if (temp_joint_name ==
"j_mrl_fle" )
182 else if (temp_joint_name ==
"j_ring_fle" )
186 else if (temp_joint_name ==
"j_little_fle" )
205 const std::string& hardware_interface = joint_interfaces.front();
213 js_interface_.registerHandle(hardware_interface::JointStateHandle(
218 std::vector< hardware_interface::JointHandle> joint_handle;
220 if(hardware_interface ==
"EffortJointInterface" || hardware_interface ==
"hardware_interface/EffortJointInterface" )
222 ROS_INFO_STREAM_NAMED(
"mia_hw_sim",
" Joint " <<
joint_names_[j]<<
" EFFORT" );
224 joint_handle.resize(1);
235 else if(hardware_interface ==
"PositionJointInterface" || hardware_interface ==
"VelocityJointInterface"
236 || hardware_interface ==
"hardware_interface/PositionJointInterface" || hardware_interface ==
"hardware_interface/VelocityJointInterface" )
239 ROS_INFO_STREAM_NAMED(
"mia_hw_sim",
" Joint " <<
joint_names_[j]<<
" POS-VEL" );
240 joint_handle.resize(2);
253 hardware_interface::JointHandle joint_handle_vel;
262 ROS_FATAL_STREAM_NAMED(
"default_robot_hw_sim",
"No matching hardware interface found for '"
263 << hardware_interface );
269 gazebo::physics::JointPtr joint = parent_model->GetJoint(
joint_names_[j]);
273 ROS_ERROR_STREAM(
"This robot has a joint named \"" <<
joint_names_[j]
274 <<
"\" which is not in the gazebo model.");
283 joint_limit_nh, urdf_model,
292 const ros::NodeHandle nh(model_nh,
"/gazebo_ros_control/pid_gains/" +
302 ROS_WARN_STREAM_NAMED(
"mia_hw_sim",
"POSITION_PID control not effective for mia joints. Please use ROS controllers"
303 <<
" to load pid gains");
309 ROS_WARN_STREAM_NAMED(
"mia_hw_sim",
"VELOCITY_PID control not effective for mia joints. Please use ROS controllers"
310 <<
" to load pid gains");
314 ROS_INFO_STREAM_NAMED(
"mia_hw_sim",
" Get PID params !" );
321 #if GAZEBO_MAJOR_VERSION > 2
327 ROS_INFO_STREAM_NAMED(
"mia_hw_sim",
" Set fmax !" );
366 const std::vector<hardware_interface::JointHandle>& joint_handle,
367 const std::vector<ControlMethod> ctrl_method,
368 const ros::NodeHandle& joint_limit_nh,
369 const urdf::Model *
const urdf_model,
370 int *
const joint_type,
double *
const lower_limit,
371 double *
const upper_limit,
double *
const effort_limit)
374 *joint_type = urdf::Joint::UNKNOWN;
375 *lower_limit = -std::numeric_limits<double>::max();
376 *upper_limit = std::numeric_limits<double>::max();
377 *effort_limit = std::numeric_limits<double>::max();
379 joint_limits_interface::JointLimits limits;
380 bool has_limits =
false;
381 joint_limits_interface::SoftJointLimits soft_limits;
382 bool has_soft_limits =
false;
385 if (urdf_model != NULL)
388 const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name);
389 if (urdf_joint != NULL)
391 *joint_type = urdf_joint->type;
393 if (joint_limits_interface::getJointLimits(urdf_joint, limits))
395 if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
396 has_soft_limits =
true;
400 if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits))
406 if (*joint_type == urdf::Joint::UNKNOWN)
410 if (limits.has_position_limits)
412 *joint_type = urdf::Joint::REVOLUTE;
416 if (limits.angle_wraparound)
417 *joint_type = urdf::Joint::CONTINUOUS;
419 *joint_type = urdf::Joint::PRISMATIC;
423 if (limits.has_position_limits)
425 *lower_limit = limits.min_position;
426 *upper_limit = limits.max_position;
428 if (limits.has_effort_limits)
429 *effort_limit = limits.max_effort;
433 for (
unsigned int cm = 0; cm< ctrl_method.size(); cm++)
435 switch (ctrl_method[cm])
439 const joint_limits_interface::EffortJointSoftLimitsHandle
440 limits_handle(joint_handle[cm], limits, soft_limits);
446 const joint_limits_interface::PositionJointSoftLimitsHandle
447 limits_handle(joint_handle[cm], limits, soft_limits);
453 const joint_limits_interface::VelocityJointSoftLimitsHandle
454 limits_handle(joint_handle[cm], limits, soft_limits);
463 for (
unsigned int cm = 0; cm< ctrl_method.size(); cm++)
465 switch (ctrl_method[cm])
470 const joint_limits_interface::EffortJointSaturationHandle
471 sat_handle(joint_handle[cm], limits);
477 const joint_limits_interface::PositionJointSaturationHandle
478 sat_handle(joint_handle[cm], limits);
484 const joint_limits_interface::VelocityJointSaturationHandle
485 sat_handle(joint_handle[cm], limits);
503 for(
unsigned int j=0; j <
n_dof_; j++)
513 #if GAZEBO_MAJOR_VERSION >= 9
523 #if GAZEBO_MAJOR_VERSION >= 9
546 effort =
sim_joints_[j]->GetForce((
unsigned int)(0));
583 for(
unsigned int j=0; j <
n_dof_; j++)
619 const double MaxDiff_mrl = 0.1;
621 for(
unsigned int j=0; j <
n_dof_; j++)
686 else if (*j_index_flex_pos < 0 && *j_index_flex_pos >= -0.0005 &&
j_index_flex_sign == 1)
733 #if GAZEBO_MAJOR_VERSION >= 4
866 case urdf::Joint::REVOLUTE:
873 case urdf::Joint::CONTINUOUS:
882 const double effort = clamp(
pid_controllers_[j].computeCommand(error, period),
883 -effort_limit, effort_limit);
896 double delta = velocity_control * period.toSec();
899 #if GAZEBO_MAJOR_VERSION >= 4
941 else if (*j_index_flex_pos < 0 && *j_index_flex_pos >= -0.0005 &&
j_index_flex_sign == 1)
987 #elif GAZEBO_MAJOR_VERSION <= 2
1147 const double effort = clamp(
pid_controllers_[j].computeCommand(error, period),
1148 -effort_limit, effort_limit);
1163 const double last_joint_velocity_command_,
1164 const double joint_velocity_command_,
1165 const double last_joint_position_command_,
1166 const double joint_position_command_)
1169 if(Default_joint_control_methods_ ==
EFFORT)
1200 -effort_limit, effort_limit);
1214 const double MaxDist = 0.05;
1215 double isStack =
false;
1219 double diff_mr = joint_position_m - joint_position_r;
1220 double diff_ml = joint_position_m - joint_position_l;
1222 if (diff_mr >= MaxDist & flex_direction > 0 )
1228 else if (diff_ml >= MaxDist & flex_direction > 0 )
1234 else if (diff_mr <= -MaxDist & flex_direction < 0 )
1240 else if (diff_ml <= -MaxDist & flex_direction < 0 )
1266 PLUGINLIB_EXPORT_CLASS(
mia::MiaHWSim, gazebo_ros_control::RobotHWSim)