Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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);
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
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
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
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 }