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


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