floating_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/floating_joint_model.h>
00039 #include <boost/math/constants/constants.hpp>
00040 #include <console_bridge/console.h>
00041 #include <limits>
00042 #include <cmath>
00043 
00044 moveit::core::FloatingJointModel::FloatingJointModel(const std::string& name)
00045   : JointModel(name)
00046   , angular_distance_weight_(1.0)
00047 {
00048   type_ = FLOATING;
00049   local_variable_names_.push_back("trans_x");
00050   local_variable_names_.push_back("trans_y");
00051   local_variable_names_.push_back("trans_z");
00052   local_variable_names_.push_back("rot_x");
00053   local_variable_names_.push_back("rot_y");
00054   local_variable_names_.push_back("rot_z");
00055   local_variable_names_.push_back("rot_w");
00056   for (int i = 0 ; i < 7 ; ++i)
00057   {
00058     variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
00059     variable_index_map_[variable_names_.back()] = i;
00060   }
00061   
00062   variable_bounds_.resize(7);
00063   
00064   variable_bounds_[0].position_bounded_ = true;
00065   variable_bounds_[1].position_bounded_ = true;
00066   variable_bounds_[2].position_bounded_ = true;
00067   variable_bounds_[3].position_bounded_ = true;
00068   variable_bounds_[4].position_bounded_ = true;
00069   variable_bounds_[5].position_bounded_ = true;
00070   variable_bounds_[6].position_bounded_ = true;
00071 
00072   variable_bounds_[0].min_position_ = -std::numeric_limits<double>::infinity();
00073   variable_bounds_[0].max_position_ = std::numeric_limits<double>::infinity();
00074   variable_bounds_[1].min_position_ = -std::numeric_limits<double>::infinity();
00075   variable_bounds_[1].max_position_ = std::numeric_limits<double>::infinity();
00076   variable_bounds_[2].min_position_ = -std::numeric_limits<double>::infinity();
00077   variable_bounds_[2].max_position_ = std::numeric_limits<double>::infinity();
00078   variable_bounds_[3].min_position_ = -1.0;
00079   variable_bounds_[3].max_position_ = 1.0;
00080   variable_bounds_[4].min_position_ = -1.0;
00081   variable_bounds_[4].max_position_ = 1.0;
00082   variable_bounds_[5].min_position_ = -1.0;
00083   variable_bounds_[5].max_position_ = 1.0;
00084   variable_bounds_[6].min_position_ = -1.0;
00085   variable_bounds_[6].max_position_ = 1.0;
00086 
00087   computeVariableBoundsMsg();
00088 }
00089 
00090 double moveit::core::FloatingJointModel::getMaximumExtent(const Bounds &other_bounds) const
00091 {
00092   double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
00093   double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
00094   double dz = other_bounds[2].max_position_ - other_bounds[2].min_position_;
00095   return sqrt(dx*dx + dy*dy + dz*dz) + boost::math::constants::pi<double>() * 0.5 * angular_distance_weight_;
00096 }
00097 
00098 double moveit::core::FloatingJointModel::distance(const double *values1, const double *values2) const
00099 {
00100   return distanceTranslation(values1, values2) + angular_distance_weight_ * distanceRotation(values1, values2);
00101 }
00102 
00103 double moveit::core::FloatingJointModel::distanceTranslation(const double *values1, const double *values2) const
00104 {
00105   double dx = values1[0] - values2[0];
00106   double dy = values1[1] - values2[1];
00107   double dz = values1[2] - values2[2];
00108   return sqrt(dx*dx + dy*dy + dz*dz);
00109 }
00110 
00111 double moveit::core::FloatingJointModel::distanceRotation(const double *values1, const double *values2) const
00112 {
00113   double dq = fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
00114   if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
00115     return 0.0;
00116   else
00117     return acos(dq);
00118 }
00119 
00120 void moveit::core::FloatingJointModel::interpolate(const double *from, const double *to, const double t, double *state) const
00121 {
00122   // interpolate position
00123   state[0] = from[0] + (to[0] - from[0]) * t;
00124   state[1] = from[1] + (to[1] - from[1]) * t;
00125   state[2] = from[2] + (to[2] - from[2]) * t;
00126   
00127   double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
00128   double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
00129   if (theta > std::numeric_limits<double>::epsilon())
00130   {
00131     double d = 1.0 / sin(theta);
00132     double s0 = sin((1.0 - t) * theta);
00133     double s1 = sin(t * theta);
00134     if (dq < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00135       s1 = -s1;
00136     state[3] = (from[3] * s0 + to[3] * s1) * d;
00137     state[4] = (from[4] * s0 + to[4] * s1) * d;
00138     state[5] = (from[5] * s0 + to[5] * s1) * d;
00139     state[6] = (from[6] * s0 + to[6] * s1) * d;
00140   }
00141   else
00142   {
00143     state[3] = from[3];
00144     state[4] = from[4];
00145     state[5] = from[5];
00146     state[6] = from[6];
00147   }
00148 }
00149 
00150 bool moveit::core::FloatingJointModel::satisfiesPositionBounds(const double *values, const Bounds &bounds, double margin) const
00151 {
00152   if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
00153     return false;
00154   if (values[1] < bounds[1].min_position_ - margin || values[1] > bounds[1].max_position_ + margin)
00155     return false;
00156   if (values[2] < bounds[2].min_position_ - margin || values[2] > bounds[2].max_position_ + margin)
00157     return false;
00158   double normSqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
00159   if (fabs(normSqr - 1.0) > std::numeric_limits<float>::epsilon() * 10.0)
00160     return false;
00161   return true;
00162 }
00163 
00164 bool moveit::core::FloatingJointModel::normalizeRotation(double *values) const
00165 {
00166   // normalize the quaternion if we need to
00167   double normSqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
00168   if (fabs(normSqr - 1.0) > std::numeric_limits<double>::epsilon() * 100.0)
00169   {
00170     double norm = sqrt(normSqr);
00171     if (norm < std::numeric_limits<double>::epsilon() * 100.0)
00172     {
00173       logWarn("Quaternion is zero in RobotState representation. Setting to identity");
00174       values[3] = 0.0;
00175       values[4] = 0.0;
00176       values[5] = 0.0;
00177       values[6] = 1.0;
00178     }
00179     else
00180     {
00181       values[3] /= norm;
00182       values[4] /= norm;
00183       values[5] /= norm;
00184       values[6] /= norm;
00185     }
00186     return true;
00187   }
00188   else
00189     return false;
00190 }
00191 
00192 unsigned int moveit::core::FloatingJointModel::getStateSpaceDimension() const
00193 {
00194   return 6;
00195 }
00196 
00197 bool moveit::core::FloatingJointModel::enforcePositionBounds(double *values, const Bounds &bounds) const
00198 {
00199   bool result = normalizeRotation(values);
00200   for (unsigned int i = 0 ; i < 3 ; ++i)
00201   {
00202     if (values[i] < bounds[i].min_position_)
00203     {
00204       values[i] = bounds[i].min_position_;
00205       result = true;
00206     }
00207     else
00208       if (values[i] > bounds[i].max_position_)
00209       {
00210         values[i] = bounds[i].max_position_;
00211         result = true;
00212       }
00213   }
00214   return result;
00215 }
00216 
00217 void moveit::core::FloatingJointModel::computeTransform(const double *joint_values, Eigen::Affine3d &transf) const
00218 {
00219   transf = Eigen::Affine3d(Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) *
00220                            Eigen::Quaterniond(joint_values[6],joint_values[3], joint_values[4], joint_values[5]).toRotationMatrix());
00221 }
00222 
00223 void moveit::core::FloatingJointModel::computeVariablePositions(const Eigen::Affine3d& transf, double *joint_values) const
00224 {
00225   joint_values[0] = transf.translation().x();
00226   joint_values[1] = transf.translation().y();
00227   joint_values[2] = transf.translation().z();
00228   Eigen::Quaterniond q(transf.rotation());
00229   joint_values[3] = q.x();
00230   joint_values[4] = q.y();
00231   joint_values[5] = q.z();
00232   joint_values[6] = q.w();
00233 }
00234 
00235 void moveit::core::FloatingJointModel::getVariableDefaultPositions(double *values, const Bounds &bounds) const
00236 {
00237   for (unsigned int i = 0 ; i < 3 ; ++i)
00238   {
00239     // if zero is a valid value
00240     if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
00241       values[i] = 0.0;
00242     else
00243       values[i] = (bounds[i].min_position_ + bounds[i].max_position_)/2.0;
00244   }
00245   
00246   values[3] = 0.0;
00247   values[4] = 0.0;
00248   values[5] = 0.0;
00249   values[6] = 1.0;
00250 }
00251 
00252 void moveit::core::FloatingJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &bounds) const
00253 {
00254   if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() || bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
00255     values[0] = 0.0;
00256   else
00257     values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
00258   if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() || bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
00259     values[1] = 0.0;
00260   else
00261     values[1] = rng.uniformReal(bounds[1].min_position_, bounds[1].max_position_);
00262   if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() || bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
00263     values[2] = 0.0;
00264   else
00265     values[2] = rng.uniformReal(bounds[2].min_position_, bounds[2].max_position_);
00266   
00267   double q[4]; rng.quaternion(q);
00268   values[3] = q[0];
00269   values[4] = q[1];
00270   values[5] = q[2];
00271   values[6] = q[3];
00272 }
00273 
00274 void moveit::core::FloatingJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &bounds,
00275                                                                         const double *near, const double distance) const
00276 {
00277   if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() || bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
00278     values[0] = 0.0;
00279   else
00280     values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
00281                                 std::min(bounds[0].max_position_, near[0] + distance));
00282   if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() || bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
00283     values[1] = 0.0;
00284   else
00285     values[1] = rng.uniformReal(std::max(bounds[1].min_position_, near[1] - distance),
00286                                 std::min(bounds[1].max_position_, near[1] + distance));
00287   if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() || bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
00288     values[2] = 0.0;
00289   else
00290     values[2] = rng.uniformReal(std::max(bounds[2].min_position_, near[2] - distance),
00291                                 std::min(bounds[2].max_position_, near[2] + distance));
00292   
00293   double da = angular_distance_weight_ * distance;
00294   if (da >= .25 * boost::math::constants::pi<double>())
00295   {
00296     double q[4]; rng.quaternion(q);
00297     values[3] = q[0];
00298     values[4] = q[1];
00299     values[5] = q[2];
00300     values[6] = q[3];
00301   }
00302   else
00303   {
00304     //taken from OMPL
00305     // sample angle & axis
00306     double ax = rng.gaussian01();
00307     double ay = rng.gaussian01();
00308     double az = rng.gaussian01();
00309     double angle = 2.0 * pow(rng.uniform01(), 1.0/3.0) * da;
00310     // convert to quaternion
00311     double q[4];
00312     double norm = sqrt(ax * ax + ay * ay + az * az);
00313     if (norm < 1e-6)
00314     {
00315       q[0] = q[1] = q[2] = 0.0;
00316       q[3] = 1.0;
00317     }
00318     else
00319     {
00320       double s = sin(angle / 2.0);
00321       q[0] = s * ax / norm;
00322       q[1] = s * ay / norm;
00323       q[2] = s * az / norm;
00324       q[3] = cos(angle / 2.0);
00325     }
00326     // multiply quaternions: near * q
00327     values[3] = near[6]*q[0] + near[3]*q[3] + near[4]*q[2] - near[5]*q[1];
00328     values[4] = near[6]*q[1] + near[4]*q[3] + near[5]*q[0] - near[3]*q[2];
00329     values[5] = near[6]*q[2] + near[5]*q[3] + near[3]*q[1] - near[4]*q[0];
00330     values[6] = near[6]*q[3] - near[3]*q[0] - near[4]*q[1] - near[5]*q[2];
00331   }
00332 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:52