prismatic_joint_model.cpp
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 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/robot_model/prismatic_joint_model.h>
00038 #include <limits>
00039 
00040 moveit::core::PrismaticJointModel::PrismaticJointModel(const std::string& name) : JointModel(name), axis_(0.0, 0.0, 0.0)
00041 {
00042   type_ = PRISMATIC;
00043   variable_names_.push_back(name_);
00044   variable_bounds_.resize(1);
00045   variable_bounds_[0].position_bounded_ = true;
00046   variable_bounds_[0].min_position_ = -std::numeric_limits<double>::max();
00047   variable_bounds_[0].max_position_ = std::numeric_limits<double>::max();
00048   variable_index_map_[name_] = 0;
00049   computeVariableBoundsMsg();
00050 }
00051 
00052 unsigned int moveit::core::PrismaticJointModel::getStateSpaceDimension() const
00053 {
00054   return 1;
00055 }
00056 
00057 double moveit::core::PrismaticJointModel::getMaximumExtent(const Bounds& other_bounds) const
00058 {
00059   return variable_bounds_[0].max_position_ - other_bounds[0].min_position_;
00060 }
00061 
00062 void moveit::core::PrismaticJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
00063 {
00064   // if zero is a valid value
00065   if (bounds[0].min_position_ <= 0.0 && bounds[0].max_position_ >= 0.0)
00066     values[0] = 0.0;
00067   else
00068     values[0] = (bounds[0].min_position_ + bounds[0].max_position_) / 2.0;
00069 }
00070 
00071 bool moveit::core::PrismaticJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds,
00072                                                                 double margin) const
00073 {
00074   if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
00075     return false;
00076   return true;
00077 }
00078 
00079 void moveit::core::PrismaticJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
00080                                                                    double* values, const Bounds& bounds) const
00081 {
00082   values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
00083 }
00084 
00085 void moveit::core::PrismaticJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng,
00086                                                                          double* values, const Bounds& bounds,
00087                                                                          const double* near,
00088                                                                          const double distance) const
00089 {
00090   values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] - distance),
00091                               std::min(bounds[0].max_position_, near[0] + distance));
00092 }
00093 
00094 bool moveit::core::PrismaticJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
00095 {
00096   if (values[0] < bounds[0].min_position_)
00097   {
00098     values[0] = bounds[0].min_position_;
00099     return true;
00100   }
00101   else if (values[0] > bounds[0].max_position_)
00102   {
00103     values[0] = bounds[0].max_position_;
00104     return true;
00105   }
00106   return false;
00107 }
00108 
00109 double moveit::core::PrismaticJointModel::distance(const double* values1, const double* values2) const
00110 {
00111   return fabs(values1[0] - values2[0]);
00112 }
00113 
00114 void moveit::core::PrismaticJointModel::interpolate(const double* from, const double* to, const double t,
00115                                                     double* state) const
00116 {
00117   state[0] = from[0] + (to[0] - from[0]) * t;
00118 }
00119 
00120 void moveit::core::PrismaticJointModel::computeTransform(const double* joint_values, Eigen::Affine3d& transf) const
00121 {
00122   double* d = transf.data();
00123   d[0] = 1.0;
00124   d[1] = 0.0;
00125   d[2] = 0.0;
00126   d[3] = 0.0;
00127 
00128   d[4] = 0.0;
00129   d[5] = 1.0;
00130   d[6] = 0.0;
00131   d[7] = 0.0;
00132 
00133   d[8] = 0.0;
00134   d[9] = 0.0;
00135   d[10] = 1.0;
00136   d[11] = 0.0;
00137 
00138   d[12] = axis_.x() * joint_values[0];
00139   d[13] = axis_.y() * joint_values[0];
00140   d[14] = axis_.z() * joint_values[0];
00141   d[15] = 1.0;
00142 
00143   //  transf.setIdentity();
00144   //  transf.translation() = Eigen::Vector3d(axis_ * joint_values[0]);
00145 }
00146 
00147 void moveit::core::PrismaticJointModel::computeVariablePositions(const Eigen::Affine3d& transf,
00148                                                                  double* joint_values) const
00149 {
00150   joint_values[0] = transf.translation().dot(axis_);
00151 }


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