joint_model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Ioan A. Sucan
00005 *  Copyright (c) 2008-2013, Willow Garage, Inc.
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 Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 /* Author: Ioan Sucan */
00037 
00038 #include <moveit/exceptions/exceptions.h>
00039 #include <moveit/robot_model/joint_model.h>
00040 #include <moveit/robot_model/link_model.h>
00041 #include <algorithm>
00042 
00043 moveit::core::JointModel::JointModel(const std::string& name)
00044   : name_(name)
00045   , type_(UNKNOWN)
00046   , parent_link_model_(NULL)
00047   , child_link_model_(NULL)
00048   , mimic_(NULL)
00049   , mimic_factor_(1.0)
00050   , mimic_offset_(0.0)
00051   , passive_(false)
00052   , distance_factor_(1.0)
00053   , first_variable_index_(-1)
00054   , joint_index_(-1)
00055 {
00056 }
00057 
00058 moveit::core::JointModel::~JointModel()
00059 {
00060 }
00061 
00062 std::string moveit::core::JointModel::getTypeName() const
00063 {
00064   switch (type_)
00065   {
00066     case UNKNOWN:
00067       return "Unkown";
00068     case REVOLUTE:
00069       return "Revolute";
00070     case PRISMATIC:
00071       return "Prismatic";
00072     case PLANAR:
00073       return "Planar";
00074     case FLOATING:
00075       return "Floating";
00076     case FIXED:
00077       return "Fixed";
00078     default:
00079       return "[Unkown]";
00080   }
00081 }
00082 
00083 int moveit::core::JointModel::getLocalVariableIndex(const std::string& variable) const
00084 {
00085   VariableIndexMap::const_iterator it = variable_index_map_.find(variable);
00086   if (it == variable_index_map_.end())
00087     throw Exception("Could not find variable '" + variable + "' to get bounds for within joint '" + name_ + "'");
00088   return it->second;
00089 }
00090 
00091 bool moveit::core::JointModel::enforceVelocityBounds(double* values, const Bounds& other_bounds) const
00092 {
00093   bool change = false;
00094   for (std::size_t i = 0; i < other_bounds.size(); ++i)
00095     if (other_bounds[i].max_velocity_ < values[i])
00096     {
00097       values[i] = other_bounds[i].max_velocity_;
00098       change = true;
00099     }
00100     else if (other_bounds[i].min_velocity_ > values[i])
00101     {
00102       values[i] = other_bounds[i].min_velocity_;
00103       change = true;
00104     }
00105   return change;
00106 }
00107 
00108 bool moveit::core::JointModel::satisfiesVelocityBounds(const double* values, const Bounds& other_bounds,
00109                                                        double margin) const
00110 {
00111   for (std::size_t i = 0; i < other_bounds.size(); ++i)
00112     if (other_bounds[i].max_velocity_ + margin < values[i])
00113       return false;
00114     else if (other_bounds[i].min_velocity_ - margin > values[i])
00115       return false;
00116   return true;
00117 }
00118 
00119 const moveit::core::VariableBounds& moveit::core::JointModel::getVariableBounds(const std::string& variable) const
00120 {
00121   return variable_bounds_[getLocalVariableIndex(variable)];
00122 }
00123 
00124 void moveit::core::JointModel::setVariableBounds(const std::string& variable, const VariableBounds& bounds)
00125 {
00126   variable_bounds_[getLocalVariableIndex(variable)] = bounds;
00127   computeVariableBoundsMsg();
00128 }
00129 
00130 void moveit::core::JointModel::setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim)
00131 {
00132   for (std::size_t j = 0; j < variable_names_.size(); ++j)
00133     for (std::size_t i = 0; i < jlim.size(); ++i)
00134       if (jlim[i].joint_name == variable_names_[j])
00135       {
00136         variable_bounds_[j].position_bounded_ = jlim[i].has_position_limits;
00137         if (jlim[i].has_position_limits)
00138         {
00139           variable_bounds_[j].min_position_ = jlim[i].min_position;
00140           variable_bounds_[j].max_position_ = jlim[i].max_position;
00141         }
00142         variable_bounds_[j].velocity_bounded_ = jlim[i].has_velocity_limits;
00143         if (jlim[i].has_velocity_limits)
00144         {
00145           variable_bounds_[j].min_velocity_ = -jlim[i].max_velocity;
00146           variable_bounds_[j].max_velocity_ = jlim[i].max_velocity;
00147         }
00148         variable_bounds_[j].acceleration_bounded_ = jlim[i].has_acceleration_limits;
00149         if (jlim[i].has_acceleration_limits)
00150         {
00151           variable_bounds_[j].min_acceleration_ = -jlim[i].max_acceleration;
00152           variable_bounds_[j].max_acceleration_ = jlim[i].max_acceleration;
00153         }
00154         break;
00155       }
00156   computeVariableBoundsMsg();
00157 }
00158 
00159 void moveit::core::JointModel::computeVariableBoundsMsg()
00160 {
00161   variable_bounds_msg_.clear();
00162   for (std::size_t i = 0; i < variable_bounds_.size(); ++i)
00163   {
00164     moveit_msgs::JointLimits lim;
00165     lim.joint_name = variable_names_[i];
00166     lim.has_position_limits = variable_bounds_[i].position_bounded_;
00167     lim.min_position = variable_bounds_[i].min_position_;
00168     lim.max_position = variable_bounds_[i].max_position_;
00169     lim.has_velocity_limits = variable_bounds_[i].velocity_bounded_;
00170     lim.max_velocity = std::min(fabs(variable_bounds_[i].min_velocity_), fabs(variable_bounds_[i].max_velocity_));
00171     lim.has_acceleration_limits = variable_bounds_[i].acceleration_bounded_;
00172     lim.max_acceleration =
00173         std::min(fabs(variable_bounds_[i].min_acceleration_), fabs(variable_bounds_[i].max_acceleration_));
00174     variable_bounds_msg_.push_back(lim);
00175   }
00176 }
00177 
00178 void moveit::core::JointModel::setMimic(const JointModel* mimic, double factor, double offset)
00179 {
00180   mimic_ = mimic;
00181   mimic_factor_ = factor;
00182   mimic_offset_ = offset;
00183 }
00184 
00185 void moveit::core::JointModel::addMimicRequest(const JointModel* joint)
00186 {
00187   mimic_requests_.push_back(joint);
00188 }
00189 
00190 void moveit::core::JointModel::addDescendantJointModel(const JointModel* joint)
00191 {
00192   descendant_joint_models_.push_back(joint);
00193   if (joint->getType() != FIXED)
00194     non_fixed_descendant_joint_models_.push_back(joint);
00195 }
00196 
00197 void moveit::core::JointModel::addDescendantLinkModel(const LinkModel* link)
00198 {
00199   descendant_link_models_.push_back(link);
00200 }
00201 
00202 namespace
00203 {
00204 inline void printBoundHelper(std::ostream& out, double v)
00205 {
00206   if (v <= -std::numeric_limits<double>::infinity())
00207     out << "-inf";
00208   else if (v >= std::numeric_limits<double>::infinity())
00209     out << "inf";
00210   else
00211     out << v;
00212 }
00213 }
00214 
00215 std::ostream& moveit::core::operator<<(std::ostream& out, const VariableBounds& b)
00216 {
00217   out << "P." << (b.position_bounded_ ? "bounded" : "unbounded") << " [";
00218   printBoundHelper(out, b.min_position_);
00219   out << ", ";
00220   printBoundHelper(out, b.max_position_);
00221   out << "]; "
00222       << "V." << (b.velocity_bounded_ ? "bounded" : "unbounded") << " [";
00223   printBoundHelper(out, b.min_velocity_);
00224   out << ", ";
00225   printBoundHelper(out, b.max_velocity_);
00226   out << "]; "
00227       << "A." << (b.acceleration_bounded_ ? "bounded" : "unbounded") << " [";
00228   printBoundHelper(out, b.min_acceleration_);
00229   out << ", ";
00230   printBoundHelper(out, b.max_acceleration_);
00231   out << "];";
00232   return out;
00233 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49