kinematics_base.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 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: Sachin Chitta, Dave Coleman */
00036 
00037 #include <moveit/kinematics_base/kinematics_base.h>
00038 #include <moveit/robot_model/joint_model_group.h>
00039 
00040 const double kinematics::KinematicsBase::DEFAULT_SEARCH_DISCRETIZATION = 0.1;
00041 const double kinematics::KinematicsBase::DEFAULT_TIMEOUT = 1.0;
00042 
00043 void kinematics::KinematicsBase::setValues(const std::string& robot_description,
00044                        const std::string& group_name,
00045                        const std::string& base_frame,
00046                        const std::string& tip_frame,
00047                        double search_discretization)
00048 {
00049   robot_description_ = robot_description;
00050   group_name_ = group_name;
00051   base_frame_ = removeSlash(base_frame);
00052   tip_frame_ = removeSlash(tip_frame); // for backwards compatibility
00053   tip_frames_.push_back(removeSlash(tip_frame));
00054   search_discretization_ = search_discretization;
00055 }
00056 
00057 void kinematics::KinematicsBase::setValues(const std::string& robot_description,
00058                        const std::string& group_name,
00059                        const std::string& base_frame,
00060                        const std::vector<std::string>& tip_frames,
00061                        double search_discretization)
00062 {
00063   robot_description_ = robot_description;
00064   group_name_ = group_name;
00065   base_frame_ = removeSlash(base_frame);
00066   search_discretization_ = search_discretization;
00067 
00068   // Copy tip frames to local vector after stripping slashes
00069   tip_frames_.clear();
00070   for (std::size_t i = 0; i < tip_frames.size(); ++i)
00071     tip_frames_.push_back(removeSlash(tip_frames[i]));
00072 
00073   // Copy tip frames to our legacy variable if only one tip frame is passed in the input vector. Remove eventually.
00074   if (tip_frames.size() == 1)
00075     tip_frame_ = removeSlash(tip_frames[0]);
00076 }
00077 
00078 bool kinematics::KinematicsBase::setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices)
00079 {
00080   for(std::size_t i = 0; i < redundant_joint_indices.size(); ++i)
00081   {
00082     if(redundant_joint_indices[i] >= getJointNames().size())
00083     {
00084       return false;
00085     }
00086   }
00087   redundant_joint_indices_ = redundant_joint_indices;
00088   return true;
00089 }
00090 
00091 bool kinematics::KinematicsBase::setRedundantJoints(const std::vector<std::string> &redundant_joint_names)
00092 {
00093   const std::vector<std::string> &jnames = getJointNames();
00094   std::vector<unsigned int> redundant_joint_indices;
00095   for (std::size_t i = 0 ; i < redundant_joint_names.size() ; ++i)
00096     for (std::size_t j = 0 ; j < jnames.size() ; ++j)
00097       if (jnames[j] == redundant_joint_names[i])
00098       {
00099     redundant_joint_indices.push_back(j);
00100     break;
00101       }
00102   return redundant_joint_indices.size() == redundant_joint_names.size() ? setRedundantJoints(redundant_joint_indices) : false;
00103 }
00104 
00105 std::string kinematics::KinematicsBase::removeSlash(const std::string &str) const
00106 {
00107   return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str;
00108 }
00109 
00110 const bool kinematics::KinematicsBase::supportsGroup(const moveit::core::JointModelGroup *jmg,
00111                                                      std::string* error_text_out) const
00112 {
00113   // Default implementation for legacy solvers:
00114   if (!jmg->isChain())
00115   {
00116     if(error_text_out)
00117     {
00118       *error_text_out = "This plugin only supports joint groups which are chains";
00119     }
00120     return false;
00121   }
00122 
00123   return true;
00124 }


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