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, const std::string& group_name,
00044                                            const std::string& base_frame, const std::string& tip_frame,
00045                                            double search_discretization)
00046 {
00047   robot_description_ = robot_description;
00048   group_name_ = group_name;
00049   base_frame_ = removeSlash(base_frame);
00050   tip_frame_ = removeSlash(tip_frame);  // for backwards compatibility
00051   tip_frames_.push_back(removeSlash(tip_frame));
00052   search_discretization_ = search_discretization;
00053   setSearchDiscretization(search_discretization);
00054 }
00055 
00056 void kinematics::KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name,
00057                                            const std::string& base_frame, const std::vector<std::string>& tip_frames,
00058                                            double search_discretization)
00059 {
00060   robot_description_ = robot_description;
00061   group_name_ = group_name;
00062   base_frame_ = removeSlash(base_frame);
00063   search_discretization_ = search_discretization;
00064   setSearchDiscretization(search_discretization);
00065 
00066   // Copy tip frames to local vector after stripping slashes
00067   tip_frames_.clear();
00068   for (std::size_t i = 0; i < tip_frames.size(); ++i)
00069     tip_frames_.push_back(removeSlash(tip_frames[i]));
00070 
00071   // Copy tip frames to our legacy variable if only one tip frame is passed in the input vector. Remove eventually.
00072   if (tip_frames.size() == 1)
00073     tip_frame_ = removeSlash(tip_frames[0]);
00074 }
00075 
00076 bool kinematics::KinematicsBase::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
00077 {
00078   for (std::size_t i = 0; i < redundant_joint_indices.size(); ++i)
00079   {
00080     if (redundant_joint_indices[i] >= getJointNames().size())
00081     {
00082       return false;
00083     }
00084   }
00085   redundant_joint_indices_ = redundant_joint_indices;
00086   setSearchDiscretization(DEFAULT_SEARCH_DISCRETIZATION);
00087 
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) :
00103                                                                           false;
00104 }
00105 
00106 std::string kinematics::KinematicsBase::removeSlash(const std::string& str) const
00107 {
00108   return (!str.empty() && str[0] == '/') ? removeSlash(str.substr(1)) : str;
00109 }
00110 
00111 bool kinematics::KinematicsBase::supportsGroup(const moveit::core::JointModelGroup* jmg,
00112                                                std::string* error_text_out) const
00113 {
00114   // Default implementation for legacy solvers:
00115   if (!jmg->isChain())
00116   {
00117     if (error_text_out)
00118     {
00119       *error_text_out = "This plugin only supports joint groups which are chains";
00120     }
00121     return false;
00122   }
00123 
00124   return true;
00125 }
00126 
00127 bool kinematics::KinematicsBase::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
00128                                                const std::vector<double>& ik_seed_state,
00129                                                std::vector<std::vector<double> >& solutions,
00130                                                kinematics::KinematicsResult& result,
00131                                                const kinematics::KinematicsQueryOptions& options) const
00132 {
00133   std::vector<double> solution;
00134   result.solution_percentage = 0.0;
00135 
00136   bool supported = false;
00137   if (std::find(supported_methods_.begin(), supported_methods_.end(), options.discretization_method) ==
00138       supported_methods_.end())
00139   {
00140     result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED;
00141     return false;
00142   }
00143 
00144   if (ik_poses.size() != 1)
00145   {
00146     logError("moveit.kinematics_base: This kinematic solver does not support getPositionIK for multiple poses");
00147     result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED;
00148     return false;
00149   }
00150 
00151   if (ik_poses.size() == 0)
00152   {
00153     logError("moveit.kinematics_base: Input ik_poses array is empty");
00154     result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES;
00155     return false;
00156   }
00157 
00158   moveit_msgs::MoveItErrorCodes error_code;
00159   if (getPositionIK(ik_poses[0], ik_seed_state, solution, error_code, options))
00160   {
00161     solutions.resize(1);
00162     solutions[0] = solution;
00163     result.kinematic_error = kinematics::KinematicErrors::OK;
00164     result.solution_percentage = 1.0f;
00165   }
00166   else
00167   {
00168     result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION;
00169     return false;
00170   }
00171 
00172   return true;
00173 }


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