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 robot_model::PrismaticJointModel::PrismaticJointModel(const std::string& name) : JointModel(name), axis_(0.0, 0.0, 0.0)
00041 {
00042   type_ = PRISMATIC;
00043   variable_bounds_.push_back(std::make_pair(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max()));
00044   variable_names_.push_back(name_);
00045 }
00046 
00047 unsigned int robot_model::PrismaticJointModel::getStateSpaceDimension() const
00048 {
00049   return 1;
00050 }
00051 
00052 double robot_model::PrismaticJointModel::getMaximumExtent(const Bounds &other_bounds) const
00053 {
00054   return other_bounds[0].second - other_bounds[0].first;
00055 }
00056 
00057 void robot_model::PrismaticJointModel::getVariableDefaultValues(std::vector<double> &values, const Bounds &bounds) const
00058 {
00059   // if zero is a valid value
00060   if (bounds[0].first <= 0.0 && bounds[0].second >= 0.0)
00061     values.push_back(0.0);
00062   else
00063     values.push_back((bounds[0].first + bounds[0].second)/2.0);
00064 }
00065 
00066 bool robot_model::PrismaticJointModel::satisfiesBounds(const std::vector<double> &values, const Bounds &bounds, double margin) const
00067 {
00068   assert(bounds.size() > 0);
00069   if (values[0] < bounds[0].first - margin || values[0] > bounds[0].second + margin)
00070     return false;
00071   return true;
00072 }
00073 
00074 void robot_model::PrismaticJointModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
00075 {
00076   values.push_back(rng.uniformReal(bounds[0].first, bounds[0].second));
00077 }
00078 
00079 void robot_model::PrismaticJointModel::getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
00080                                                                          const std::vector<double> &near, const double distance) const
00081 {
00082   values.push_back(rng.uniformReal(std::max(bounds[0].first, near[values.size()] - distance),
00083                                    std::min(bounds[0].second, near[values.size()] + distance)));
00084 }
00085 
00086 void robot_model::PrismaticJointModel::enforceBounds(std::vector<double> &values, const Bounds &bounds) const
00087 {
00088   const std::pair<double, double> &b = bounds[0];
00089   if (values[0] < b.first)
00090     values[0] = b.first;
00091   else
00092     if (values[0] > b.second)
00093       values[0] = b.second;
00094 }
00095 
00096 double robot_model::PrismaticJointModel::distance(const std::vector<double> &values1, const std::vector<double> &values2) const
00097 {
00098   assert(values1.size() == 1);
00099   assert(values2.size() == 1);
00100   return fabs(values1[0] - values2[0]);
00101 }
00102 
00103 void robot_model::PrismaticJointModel::interpolate(const std::vector<double> &from, const std::vector<double> &to, const double t, std::vector<double> &state) const
00104 {
00105   state[0] = from[0] + (to[0] - from[0]) * t;
00106 }
00107 
00108 void robot_model::PrismaticJointModel::computeTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00109 {
00110   transf.setIdentity();
00111   updateTransform(joint_values, transf);
00112 }
00113 
00114 void robot_model::PrismaticJointModel::updateTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00115 {
00116   transf.translation() = Eigen::Vector3d(axis_ * joint_values[0]);
00117 }
00118 
00119 void robot_model::PrismaticJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
00120 {
00121   joint_values.resize(1);
00122   joint_values[0] = transf.translation().dot(axis_);
00123 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:47