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, 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);
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
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
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
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 }