default_robot_hw_sim.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Open Source Robotics Foundation
00005  *  Copyright (c) 2013, The Johns Hopkins University
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Open Source Robotics Foundation
00019  *     nor the names of its contributors may be
00020  *     used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *********************************************************************/
00036 
00037 /* Author: Dave Coleman, Jonathan Bohren
00038    Desc:   Hardware Interface for any simulated robot in Gazebo
00039 */
00040 
00041 
00042 #include <gazebo_ros_control/default_robot_hw_sim.h>
00043 
00044 
00045 namespace
00046 {
00047 
00048 double clamp(const double val, const double min_val, const double max_val)
00049 {
00050   return std::min(std::max(val, min_val), max_val);
00051 }
00052 
00053 }
00054 
00055 namespace gazebo_ros_control
00056 {
00057 
00058 
00059 bool DefaultRobotHWSim::initSim(
00060   const std::string& robot_namespace,
00061   ros::NodeHandle model_nh,
00062   gazebo::physics::ModelPtr parent_model,
00063   const urdf::Model *const urdf_model,
00064   std::vector<transmission_interface::TransmissionInfo> transmissions)
00065 {
00066   // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
00067   // parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
00068   const ros::NodeHandle joint_limit_nh(model_nh);
00069 
00070   // Resize vectors to our DOF
00071   n_dof_ = transmissions.size();
00072   joint_names_.resize(n_dof_);
00073   joint_types_.resize(n_dof_);
00074   joint_lower_limits_.resize(n_dof_);
00075   joint_upper_limits_.resize(n_dof_);
00076   joint_effort_limits_.resize(n_dof_);
00077   joint_control_methods_.resize(n_dof_);
00078   pid_controllers_.resize(n_dof_);
00079   joint_position_.resize(n_dof_);
00080   joint_velocity_.resize(n_dof_);
00081   joint_effort_.resize(n_dof_);
00082   joint_effort_command_.resize(n_dof_);
00083   joint_position_command_.resize(n_dof_);
00084   joint_velocity_command_.resize(n_dof_);
00085 
00086   // Initialize values
00087   for(unsigned int j=0; j < n_dof_; j++)
00088   {
00089     // Check that this transmission has one joint
00090     if(transmissions[j].joints_.size() == 0)
00091     {
00092       ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
00093         << " has no associated joints.");
00094       continue;
00095     }
00096     else if(transmissions[j].joints_.size() > 1)
00097     {
00098       ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
00099         << " has more than one joint. Currently the default robot hardware simulation "
00100         << " interface only supports one.");
00101       continue;
00102     }
00103 
00104     std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
00105     if (joint_interfaces.empty() &&
00106         !(transmissions[j].actuators_.empty()) &&
00107         !(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
00108     {
00109       // TODO: Deprecate HW interface specification in actuators in ROS J
00110       joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
00111       ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
00112         transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
00113         "The transmission will be properly loaded, but please update " <<
00114         "your robot model to remain compatible with future versions of the plugin.");
00115     }
00116     if (joint_interfaces.empty())
00117     {
00118       ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
00119         " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
00120         "Not adding it to the robot hardware simulation.");
00121       continue;
00122     }
00123     else if (joint_interfaces.size() > 1)
00124     {
00125       ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
00126         " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
00127         "Currently the default robot hardware simulation interface only supports one. Using the first entry!");
00128       //continue;
00129     }
00130 
00131     // Add data from transmission
00132     joint_names_[j] = transmissions[j].joints_[0].name_;
00133     joint_position_[j] = 1.0;
00134     joint_velocity_[j] = 0.0;
00135     joint_effort_[j] = 1.0;  // N/m for continuous joints
00136     joint_effort_command_[j] = 0.0;
00137     joint_position_command_[j] = 0.0;
00138     joint_velocity_command_[j] = 0.0;
00139 
00140     const std::string& hardware_interface = joint_interfaces.front();
00141 
00142     // Debug
00143     ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
00144       << "' of type '" << hardware_interface << "'");
00145 
00146     // Create joint state interface for all joints
00147     js_interface_.registerHandle(hardware_interface::JointStateHandle(
00148         joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
00149 
00150     // Decide what kind of command interface this actuator/joint has
00151     hardware_interface::JointHandle joint_handle;
00152     if(hardware_interface == "EffortJointInterface")
00153     {
00154       // Create effort joint interface
00155       joint_control_methods_[j] = EFFORT;
00156       joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
00157                                                      &joint_effort_command_[j]);
00158       ej_interface_.registerHandle(joint_handle);
00159     }
00160     else if(hardware_interface == "PositionJointInterface")
00161     {
00162       // Create position joint interface
00163       joint_control_methods_[j] = POSITION;
00164       joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
00165                                                      &joint_position_command_[j]);
00166       pj_interface_.registerHandle(joint_handle);
00167     }
00168     else if(hardware_interface == "VelocityJointInterface")
00169     {
00170       // Create velocity joint interface
00171       joint_control_methods_[j] = VELOCITY;
00172       joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
00173                                                      &joint_velocity_command_[j]);
00174       vj_interface_.registerHandle(joint_handle);
00175     }
00176     else
00177     {
00178       ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
00179         << hardware_interface );
00180       return false;
00181     }
00182 
00183     // Get the gazebo joint that corresponds to the robot joint.
00184     //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: "
00185     //  << joint_names_[j]);
00186     gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
00187     if (!joint)
00188     {
00189       ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
00190         << "\" which is not in the gazebo model.");
00191       return false;
00192     }
00193     sim_joints_.push_back(joint);
00194 
00195     registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j],
00196                         joint_limit_nh, urdf_model,
00197                         &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
00198                         &joint_effort_limits_[j]);
00199     if (joint_control_methods_[j] != EFFORT)
00200     {
00201       // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
00202       // joint->SetParam("vel") to control the joint.
00203       const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" +
00204                                joint_names_[j]);
00205       if (pid_controllers_[j].init(nh, true))
00206       {
00207         switch (joint_control_methods_[j])
00208         {
00209           case POSITION:
00210             joint_control_methods_[j] = POSITION_PID;
00211             break;
00212           case VELOCITY:
00213             joint_control_methods_[j] = VELOCITY_PID;
00214             break;
00215         }
00216       }
00217       else
00218       {
00219         // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are
00220         // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is
00221         // going to be called.
00222 #if GAZEBO_MAJOR_VERSION > 2
00223         joint->SetParam("fmax", 0, joint_effort_limits_[j]);
00224 #else
00225         joint->SetMaxForce(0, joint_effort_limits_[j]);
00226 #endif
00227       }
00228     }
00229   }
00230 
00231   // Register interfaces
00232   registerInterface(&js_interface_);
00233   registerInterface(&ej_interface_);
00234   registerInterface(&pj_interface_);
00235   registerInterface(&vj_interface_);
00236 
00237   // Initialize the emergency stop code.
00238   e_stop_active_ = false;
00239   last_e_stop_active_ = false;
00240 
00241   return true;
00242 }
00243 
00244 void DefaultRobotHWSim::readSim(ros::Time time, ros::Duration period)
00245 {
00246   for(unsigned int j=0; j < n_dof_; j++)
00247   {
00248     // Gazebo has an interesting API...
00249     if (joint_types_[j] == urdf::Joint::PRISMATIC)
00250     {
00251       joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian();
00252     }
00253     else
00254     {
00255       joint_position_[j] += angles::shortest_angular_distance(joint_position_[j],
00256                             sim_joints_[j]->GetAngle(0).Radian());
00257     }
00258     joint_velocity_[j] = sim_joints_[j]->GetVelocity(0);
00259     joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0));
00260   }
00261 }
00262 
00263 void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period)
00264 {
00265   // If the E-stop is active, joints controlled by position commands will maintain their positions.
00266   if (e_stop_active_)
00267   {
00268     if (!last_e_stop_active_)
00269     {
00270       last_joint_position_command_ = joint_position_;
00271       last_e_stop_active_ = true;
00272     }
00273     joint_position_command_ = last_joint_position_command_;
00274   }
00275   else
00276   {
00277     last_e_stop_active_ = false;
00278   }
00279 
00280   ej_sat_interface_.enforceLimits(period);
00281   ej_limits_interface_.enforceLimits(period);
00282   pj_sat_interface_.enforceLimits(period);
00283   pj_limits_interface_.enforceLimits(period);
00284   vj_sat_interface_.enforceLimits(period);
00285   vj_limits_interface_.enforceLimits(period);
00286 
00287   for(unsigned int j=0; j < n_dof_; j++)
00288   {
00289     switch (joint_control_methods_[j])
00290     {
00291       case EFFORT:
00292         {
00293           const double effort = e_stop_active_ ? 0 : joint_effort_command_[j];
00294           sim_joints_[j]->SetForce(0, effort);
00295         }
00296         break;
00297 
00298       case POSITION:
00299 #if GAZEBO_MAJOR_VERSION >= 4
00300         sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
00301 #else
00302         sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
00303 #endif
00304         break;
00305 
00306       case POSITION_PID:
00307         {
00308           double error;
00309           switch (joint_types_[j])
00310           {
00311             case urdf::Joint::REVOLUTE:
00312               angles::shortest_angular_distance_with_limits(joint_position_[j],
00313                                                             joint_position_command_[j],
00314                                                             joint_lower_limits_[j],
00315                                                             joint_upper_limits_[j],
00316                                                             error);
00317               break;
00318             case urdf::Joint::CONTINUOUS:
00319               error = angles::shortest_angular_distance(joint_position_[j],
00320                                                         joint_position_command_[j]);
00321               break;
00322             default:
00323               error = joint_position_command_[j] - joint_position_[j];
00324           }
00325 
00326           const double effort_limit = joint_effort_limits_[j];
00327           const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
00328                                       -effort_limit, effort_limit);
00329           sim_joints_[j]->SetForce(0, effort);
00330         }
00331         break;
00332 
00333       case VELOCITY:
00334 #if GAZEBO_MAJOR_VERSION > 2
00335         sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
00336 #else
00337         sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
00338 #endif
00339         break;
00340 
00341       case VELOCITY_PID:
00342         double error;
00343         if (e_stop_active_)
00344           error = -joint_velocity_[j];
00345         else
00346           error = joint_velocity_command_[j] - joint_velocity_[j];
00347         const double effort_limit = joint_effort_limits_[j];
00348         const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
00349                                     -effort_limit, effort_limit);
00350         sim_joints_[j]->SetForce(0, effort);
00351         break;
00352     }
00353   }
00354 }
00355 
00356 void DefaultRobotHWSim::eStopActive(const bool active)
00357 {
00358   e_stop_active_ = active;
00359 }
00360 
00361 // Register the limits of the joint specified by joint_name and joint_handle. The limits are
00362 // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also.
00363 // Return the joint's type, lower position limit, upper position limit, and effort limit.
00364 void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name,
00365                          const hardware_interface::JointHandle& joint_handle,
00366                          const ControlMethod ctrl_method,
00367                          const ros::NodeHandle& joint_limit_nh,
00368                          const urdf::Model *const urdf_model,
00369                          int *const joint_type, double *const lower_limit,
00370                          double *const upper_limit, double *const effort_limit)
00371 {
00372   *joint_type = urdf::Joint::UNKNOWN;
00373   *lower_limit = -std::numeric_limits<double>::max();
00374   *upper_limit = std::numeric_limits<double>::max();
00375   *effort_limit = std::numeric_limits<double>::max();
00376 
00377   joint_limits_interface::JointLimits limits;
00378   bool has_limits = false;
00379   joint_limits_interface::SoftJointLimits soft_limits;
00380   bool has_soft_limits = false;
00381 
00382   if (urdf_model != NULL)
00383   {
00384     const boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(joint_name);
00385     if (urdf_joint != NULL)
00386     {
00387       *joint_type = urdf_joint->type;
00388       // Get limits from the URDF file.
00389       if (joint_limits_interface::getJointLimits(urdf_joint, limits))
00390         has_limits = true;
00391       if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
00392         has_soft_limits = true;
00393     }
00394   }
00395   // Get limits from the parameter server.
00396   if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits))
00397     has_limits = true;
00398 
00399   if (!has_limits)
00400     return;
00401 
00402   if (*joint_type == urdf::Joint::UNKNOWN)
00403   {
00404     // Infer the joint type.
00405 
00406     if (limits.has_position_limits)
00407     {
00408       *joint_type = urdf::Joint::REVOLUTE;
00409     }
00410     else
00411     {
00412       if (limits.angle_wraparound)
00413         *joint_type = urdf::Joint::CONTINUOUS;
00414       else
00415         *joint_type = urdf::Joint::PRISMATIC;
00416     }
00417   }
00418 
00419   if (limits.has_position_limits)
00420   {
00421     *lower_limit = limits.min_position;
00422     *upper_limit = limits.max_position;
00423   }
00424   if (limits.has_effort_limits)
00425     *effort_limit = limits.max_effort;
00426 
00427   if (has_soft_limits)
00428   {
00429     switch (ctrl_method)
00430     {
00431       case EFFORT:
00432         {
00433           const joint_limits_interface::EffortJointSoftLimitsHandle
00434             limits_handle(joint_handle, limits, soft_limits);
00435           ej_limits_interface_.registerHandle(limits_handle);
00436         }
00437         break;
00438       case POSITION:
00439         {
00440           const joint_limits_interface::PositionJointSoftLimitsHandle
00441             limits_handle(joint_handle, limits, soft_limits);
00442           pj_limits_interface_.registerHandle(limits_handle);
00443         }
00444         break;
00445       case VELOCITY:
00446         {
00447           const joint_limits_interface::VelocityJointSoftLimitsHandle
00448             limits_handle(joint_handle, limits, soft_limits);
00449           vj_limits_interface_.registerHandle(limits_handle);
00450         }
00451         break;
00452     }
00453   }
00454   else
00455   {
00456     switch (ctrl_method)
00457     {
00458       case EFFORT:
00459         {
00460           const joint_limits_interface::EffortJointSaturationHandle
00461             sat_handle(joint_handle, limits);
00462           ej_sat_interface_.registerHandle(sat_handle);
00463         }
00464         break;
00465       case POSITION:
00466         {
00467           const joint_limits_interface::PositionJointSaturationHandle
00468             sat_handle(joint_handle, limits);
00469           pj_sat_interface_.registerHandle(sat_handle);
00470         }
00471         break;
00472       case VELOCITY:
00473         {
00474           const joint_limits_interface::VelocityJointSaturationHandle
00475             sat_handle(joint_handle, limits);
00476           vj_sat_interface_.registerHandle(sat_handle);
00477         }
00478         break;
00479     }
00480   }
00481 }
00482 
00483 }
00484 
00485 PLUGINLIB_EXPORT_CLASS(gazebo_ros_control::DefaultRobotHWSim, gazebo_ros_control::RobotHWSim)


gazebo_ros_control
Author(s): Jonathan Bohren, Dave Coleman
autogenerated on Thu Jun 6 2019 18:41:22