revolute_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/robot_model/revolute_joint_model.h>
00039 #include <boost/math/constants/constants.hpp>
00040 #include <algorithm>
00041 #include <limits>
00042 #include <cmath>
00043 
00044 moveit::core::RevoluteJointModel::RevoluteJointModel(const std::string& name)
00045   : JointModel(name)
00046   , axis_(0.0, 0.0, 0.0)
00047   , continuous_(false)
00048   , x2_(0.0)
00049   , y2_(0.0)
00050   , z2_(0.0)
00051   , xy_(0.0)
00052   , xz_(0.0)
00053   , yz_(0.0)
00054 {
00055   type_ = REVOLUTE;
00056   variable_names_.push_back(name_);
00057   variable_bounds_.resize(1);
00058   variable_bounds_[0].position_bounded_ = true;
00059   variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
00060   variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
00061   variable_index_map_[name_] = 0;
00062   computeVariableBoundsMsg();
00063 }
00064 
00065 unsigned int moveit::core::RevoluteJointModel::getStateSpaceDimension() const
00066 {
00067   return 1;
00068 }
00069 
00070 void moveit::core::RevoluteJointModel::setAxis(const Eigen::Vector3d& axis)
00071 {
00072   axis_ = axis.normalized();
00073   x2_ = axis_.x() * axis_.x();
00074   y2_ = axis_.y() * axis_.y();
00075   z2_ = axis_.z() * axis_.z();
00076   xy_ = axis_.x() * axis_.y();
00077   xz_ = axis_.x() * axis_.z();
00078   yz_ = axis_.y() * axis_.z();
00079 }
00080 
00081 void moveit::core::RevoluteJointModel::setContinuous(bool flag)
00082 {
00083   continuous_ = flag;
00084   if (flag)
00085   {
00086     variable_bounds_[0].position_bounded_ = false;
00087     variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
00088     variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
00089   }
00090   else
00091     variable_bounds_[0].position_bounded_ = true;
00092   computeVariableBoundsMsg();
00093 }
00094 
00095 double moveit::core::RevoluteJointModel::getMaximumExtent(const Bounds& other_bounds) const
00096 {
00097   return variable_bounds_[0].max_position_ - variable_bounds_[0].min_position_;
00098 }
00099 
00100 void moveit::core::RevoluteJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
00101 {
00102   // if zero is a valid value
00103   if (bounds[0].min_position_ <= 0.0 && bounds[0].max_position_ >= 0.0)
00104     values[0] = 0.0;
00105   else
00106     values[0] = (bounds[0].min_position_ + bounds[0].max_position_) / 2.0;
00107 }
00108 
00109 void moveit::core::RevoluteJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
00110                                                                   double* values, const Bounds& bounds) const
00111 {
00112   values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
00113 }
00114 
00115 void moveit::core::RevoluteJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng,
00116                                                                         double* values, const Bounds& bounds,
00117                                                                         const double* near, const double distance) const
00118 {
00119   if (continuous_)
00120   {
00121     values[0] = rng.uniformReal(near[0] - distance, near[0] + distance);
00122     enforcePositionBounds(values, bounds);
00123   }
00124   else
00125     values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
00126                                 std::min(bounds[0].max_position_, near[0] + distance));
00127 }
00128 
00129 void moveit::core::RevoluteJointModel::interpolate(const double* from, const double* to, const double t,
00130                                                    double* state) const
00131 {
00132   if (continuous_)
00133   {
00134     double diff = to[0] - from[0];
00135     if (fabs(diff) <= boost::math::constants::pi<double>())
00136       state[0] = from[0] + diff * t;
00137     else
00138     {
00139       if (diff > 0.0)
00140         diff = 2.0 * boost::math::constants::pi<double>() - diff;
00141       else
00142         diff = -2.0 * boost::math::constants::pi<double>() - diff;
00143       state[0] = from[0] - diff * t;
00144       // input states are within bounds, so the following check is sufficient
00145       if (state[0] > boost::math::constants::pi<double>())
00146         state[0] -= 2.0 * boost::math::constants::pi<double>();
00147       else if (state[0] < -boost::math::constants::pi<double>())
00148         state[0] += 2.0 * boost::math::constants::pi<double>();
00149     }
00150   }
00151   else
00152     state[0] = from[0] + (to[0] - from[0]) * t;
00153 }
00154 
00155 double moveit::core::RevoluteJointModel::distance(const double* values1, const double* values2) const
00156 {
00157   if (continuous_)
00158   {
00159     double d = fmod(fabs(values1[0] - values2[0]), 2.0 * boost::math::constants::pi<double>());
00160     return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
00161   }
00162   else
00163     return fabs(values1[0] - values2[0]);
00164 }
00165 
00166 bool moveit::core::RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds,
00167                                                                double margin) const
00168 {
00169   if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
00170     return false;
00171   return true;
00172 }
00173 
00174 bool moveit::core::RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
00175 {
00176   if (continuous_)
00177   {
00178     double& v = values[0];
00179     if (v <= -boost::math::constants::pi<double>() || v > boost::math::constants::pi<double>())
00180     {
00181       v = fmod(v, 2.0 * boost::math::constants::pi<double>());
00182       if (v <= -boost::math::constants::pi<double>())
00183         v += 2.0 * boost::math::constants::pi<double>();
00184       else if (v > boost::math::constants::pi<double>())
00185         v -= 2.0 * boost::math::constants::pi<double>();
00186       return true;
00187     }
00188   }
00189   else
00190   {
00191     if (values[0] < bounds[0].min_position_)
00192     {
00193       values[0] = bounds[0].min_position_;
00194       return true;
00195     }
00196     else if (values[0] > bounds[0].max_position_)
00197     {
00198       values[0] = bounds[0].max_position_;
00199       return true;
00200     }
00201   }
00202   return false;
00203 }
00204 
00205 void moveit::core::RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Affine3d& transf) const
00206 {
00207   const double c = cos(joint_values[0]);
00208   const double s = sin(joint_values[0]);
00209   const double t = 1.0 - c;
00210   const double txy = t * xy_;
00211   const double txz = t * xz_;
00212   const double tyz = t * yz_;
00213 
00214   const double zs = axis_.z() * s;
00215   const double ys = axis_.y() * s;
00216   const double xs = axis_.x() * s;
00217 
00218   // column major
00219   double* d = transf.data();
00220 
00221   d[0] = t * x2_ + c;
00222   d[1] = txy + zs;
00223   d[2] = txz - ys;
00224   d[3] = 0.0;
00225 
00226   d[4] = txy - zs;
00227   d[5] = t * y2_ + c;
00228   d[6] = tyz + xs;
00229   d[7] = 0.0;
00230 
00231   d[8] = txz + ys;
00232   d[9] = tyz - xs;
00233   d[10] = t * z2_ + c;
00234   d[11] = 0.0;
00235 
00236   d[12] = 0.0;
00237   d[13] = 0.0;
00238   d[14] = 0.0;
00239   d[15] = 1.0;
00240 
00241   //  transf = Eigen::Affine3d(Eigen::AngleAxisd(joint_values[0], axis_));
00242 }
00243 
00244 void moveit::core::RevoluteJointModel::computeVariablePositions(const Eigen::Affine3d& transf,
00245                                                                 double* joint_values) const
00246 {
00247   Eigen::Quaterniond q(transf.rotation());
00248   q.normalize();
00249   size_t maxIdx;
00250   axis_.array().abs().maxCoeff(&maxIdx);
00251   joint_values[0] = 2. * atan2(q.vec()[maxIdx] / axis_[maxIdx], q.w());
00252 }


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