joint.hpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #ifndef ROS_ETHERCAT_MODEL_JOINT_H
00036 #define ROS_ETHERCAT_MODEL_JOINT_H
00037 
00038 #include <map>
00039 #include <string>
00040 #include <vector>
00041 #include <cfloat>
00042 #include <tinyxml.h>
00043 #include <urdf_model/joint.h>
00044 
00045 namespace ros_ethercat_model
00046 {
00047 
00048 enum
00049 {
00050   JOINT_NONE,
00051   JOINT_ROTARY,
00052   JOINT_CONTINUOUS,
00053   JOINT_PRISMATIC,
00054   JOINT_FIXED,
00055   JOINT_PLANAR,
00056   JOINT_TYPES_MAX
00057 };
00058 
00059 class JointState;
00060 
00061 class JointStatistics
00062 {
00063 public:
00064   JointStatistics() :
00065     min_position_(0), max_position_(0),
00066     max_abs_velocity_(0.0), max_abs_effort_(0.0),
00067     violated_limits_(false), initialized_(false)
00068   {
00069   }
00070 
00071   void update(JointState *jnt);
00072   void reset()
00073   {
00074     double tmp = min_position_;
00075     min_position_ = max_position_;
00076     max_position_ = tmp;
00077     max_abs_velocity_ = 0.0;
00078     max_abs_effort_ = 0.0;
00079     violated_limits_ = false;
00080   }
00081 
00082   double min_position_, max_position_;
00083   double max_abs_velocity_;
00084   double max_abs_effort_;
00085   bool violated_limits_;
00086 
00087 private:
00088   bool initialized_;
00089   double old_position_;
00090 };
00091 
00092 class JointState
00093 {
00094 public:
00096   void enforceLimits()
00097   {
00098     double effort_high, effort_low;
00099 
00100     getLimits(effort_low, effort_high);
00101 
00102     // limit the commanded effort based on position, velocity and effort limits
00103     commanded_effort_ = std::min(std::max(commanded_effort_, effort_low), effort_high);
00104   }
00105 
00107   void getLimits(double &effort_low, double &effort_high)
00108   {
00109     // only enforce joints that specify joint limits and safety code
00110     if (!joint_->safety || !joint_->limits)
00111     {
00112       effort_low = -std::numeric_limits<double>::max();
00113       effort_high = std::numeric_limits<double>::max();
00114       return;
00115     }
00116 
00117     double vel_high = joint_->limits->velocity;
00118     double vel_low = -joint_->limits->velocity;
00119     effort_high = joint_->limits->effort;
00120     effort_low = -joint_->limits->effort;
00121 
00122     // enforce position bounds on rotary and prismatic joints that are calibrated
00123     if (calibrated_ && (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC))
00124     {
00125       // Computes the velocity bounds based on the absolute limit and the
00126       // proximity to the joint limit.
00127       vel_high = std::max(-joint_->limits->velocity,
00128                           std::min(joint_->limits->velocity,
00129                                    -joint_->safety->k_position * (position_ - joint_->safety->soft_upper_limit)));
00130       vel_low = std::min(joint_->limits->velocity,
00131                          std::max(-joint_->limits->velocity,
00132                                   -joint_->safety->k_position * (position_ - joint_->safety->soft_lower_limit)));
00133     }
00134 
00135     // Computes the effort bounds based on the velocity and effort bounds.
00136     effort_high = std::max(-joint_->limits->effort,
00137                            std::min(joint_->limits->effort,
00138                                     -joint_->safety->k_velocity * (velocity_ - vel_high)));
00139     effort_low = std::min(joint_->limits->effort,
00140                           std::max(-joint_->limits->effort,
00141                                    -joint_->safety->k_velocity * (velocity_ - vel_low)));
00142   }
00143 
00145   boost::shared_ptr<const urdf::Joint> joint_;
00146 
00148   double position_;
00149 
00151   double velocity_;
00152 
00154   double measured_effort_;
00155 
00156   // joint statistics
00157   JointStatistics joint_statistics_;
00158 
00160   double commanded_effort_;
00161 
00163   bool calibrated_;
00164 
00166   double reference_position_;
00167 
00169   JointState() :
00170     position_(0.0),
00171     velocity_(0.0),
00172     measured_effort_(0.0),
00173     commanded_effort_(0.0),
00174     calibrated_(false),
00175     reference_position_(0.0)
00176   {
00177   }
00178 };
00179 inline void JointStatistics::update(JointState *jnt)
00180 {
00181   if (initialized_)
00182   {
00183     if (jnt->joint_->safety && jnt->joint_->limits
00184         && (fabs(jnt->commanded_effort_) > fabs(jnt->measured_effort_)))
00185       violated_limits_ = true;
00186     min_position_ = fmin(jnt->position_, min_position_);
00187     max_position_ = fmax(jnt->position_, max_position_);
00188     max_abs_velocity_ = fmax(fabs(jnt->velocity_), max_abs_velocity_);
00189     max_abs_effort_ = fmax(fabs(jnt->measured_effort_), max_abs_effort_);
00190   }
00191   else
00192   {
00193     min_position_ = jnt->position_;
00194     max_position_ = jnt->position_;
00195     initialized_ = true;
00196   }
00197   old_position_ = jnt->position_;
00198 }
00199 
00200 }
00201 
00202 #endif /* JOINT_H */


ros_ethercat_model
Author(s): Manos Nikolaidis
autogenerated on Thu Aug 27 2015 14:47:12