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/constraint_samplers/constraint_sampler_manager.h>
00038 #include <moveit/constraint_samplers/default_constraint_samplers.h>
00039 #include <moveit/constraint_samplers/union_constraint_sampler.h>
00040 #include <sstream>
00041
00042 constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectSampler(
00043 const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
00044 const moveit_msgs::Constraints& constr) const
00045 {
00046 for (std::size_t i = 0; i < sampler_alloc_.size(); ++i)
00047 if (sampler_alloc_[i]->canService(scene, group_name, constr))
00048 return sampler_alloc_[i]->alloc(scene, group_name, constr);
00049
00050
00051 return selectDefaultSampler(scene, group_name, constr);
00052 }
00053
00054 constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(
00055 const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
00056 const moveit_msgs::Constraints& constr)
00057 {
00058 const robot_model::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name);
00059 if (!jmg)
00060 return constraint_samplers::ConstraintSamplerPtr();
00061 std::stringstream ss;
00062 ss << constr;
00063 logDebug("Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n",
00064 jmg->getName().c_str(), ss.str().c_str());
00065
00066 ConstraintSamplerPtr joint_sampler;
00067
00068 if (!constr.joint_constraints.empty())
00069 {
00070 logDebug("There are joint constraints specified. Attempting to construct a JointConstraintSampler for group '%s'",
00071 jmg->getName().c_str());
00072
00073 std::map<std::string, bool> joint_coverage;
00074 for (std::size_t i = 0; i < jmg->getVariableNames().size(); ++i)
00075 joint_coverage[jmg->getVariableNames()[i]] = false;
00076
00077
00078 std::vector<kinematic_constraints::JointConstraint> jc;
00079 for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
00080 {
00081 kinematic_constraints::JointConstraint j(scene->getRobotModel());
00082 if (j.configure(constr.joint_constraints[i]))
00083 {
00084 if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
00085 {
00086 joint_coverage[j.getJointVariableName()] = true;
00087 jc.push_back(j);
00088 }
00089 }
00090 }
00091
00092
00093 bool full_coverage = true;
00094 for (std::map<std::string, bool>::iterator it = joint_coverage.begin(); it != joint_coverage.end(); ++it)
00095 if (!it->second)
00096 {
00097 full_coverage = false;
00098 break;
00099 }
00100
00101
00102 if (full_coverage)
00103 {
00104 JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
00105 if (sampler->configure(jc))
00106 {
00107 logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
00108 return sampler;
00109 }
00110 }
00111 else
00112
00113
00114 if (!jc.empty())
00115 {
00116 JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
00117 if (sampler->configure(jc))
00118 {
00119 logDebug("Temporary sampler satisfying joint constraints for group '%s' allocated. Looking for different types "
00120 "of constraints before returning though.",
00121 jmg->getName().c_str());
00122 joint_sampler = sampler;
00123 }
00124 }
00125 }
00126
00127 std::vector<ConstraintSamplerPtr> samplers;
00128 if (joint_sampler)
00129 samplers.push_back(joint_sampler);
00130
00131
00132 const robot_model::JointModelGroup::KinematicsSolver& ik_alloc = jmg->getGroupKinematics().first;
00133 const robot_model::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second;
00134
00135
00136
00137 if (ik_alloc)
00138 {
00139 logDebug("There is an IK allocator for '%s'. Checking for corresponding position and/or orientation constraints",
00140 jmg->getName().c_str());
00141
00142
00143 std::map<std::string, IKConstraintSamplerPtr> usedL;
00144
00145
00146
00147
00148
00149 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00150 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00151 if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
00152 {
00153 kinematic_constraints::PositionConstraintPtr pc(
00154 new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
00155 kinematic_constraints::OrientationConstraintPtr oc(
00156 new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
00157 if (pc->configure(constr.position_constraints[p], scene->getTransforms()) &&
00158 oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
00159 {
00160 IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
00161 if (iks->configure(IKSamplingPose(pc, oc)))
00162 {
00163 bool use = true;
00164
00165 if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
00166
00167 if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
00168 use = false;
00169 if (use)
00170 {
00171
00172 usedL[constr.position_constraints[p].link_name] = iks;
00173 logDebug("Allocated an IK-based sampler for group '%s' satisfying position and orientation constraints "
00174 "on link '%s'",
00175 jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
00176 }
00177 }
00178 }
00179 }
00180
00181
00182 std::map<std::string, IKConstraintSamplerPtr> usedL_fullPose = usedL;
00183
00184 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00185 {
00186
00187
00188 if (usedL_fullPose.find(constr.position_constraints[p].link_name) != usedL_fullPose.end())
00189 continue;
00190
00191 kinematic_constraints::PositionConstraintPtr pc(
00192 new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
00193 if (pc->configure(constr.position_constraints[p], scene->getTransforms()))
00194 {
00195 IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
00196 if (iks->configure(IKSamplingPose(pc)))
00197 {
00198 bool use = true;
00199 if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
00200 if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
00201 use = false;
00202 if (use)
00203 {
00204 usedL[constr.position_constraints[p].link_name] = iks;
00205 logDebug("Allocated an IK-based sampler for group '%s' satisfying position constraints on link '%s'",
00206 jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
00207 }
00208 }
00209 }
00210 }
00211
00212 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00213 {
00214
00215
00216 if (usedL_fullPose.find(constr.orientation_constraints[o].link_name) != usedL_fullPose.end())
00217 continue;
00218
00219 kinematic_constraints::OrientationConstraintPtr oc(
00220 new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
00221 if (oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
00222 {
00223 IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
00224 if (iks->configure(IKSamplingPose(oc)))
00225 {
00226 bool use = true;
00227 if (usedL.find(constr.orientation_constraints[o].link_name) != usedL.end())
00228 if (usedL[constr.orientation_constraints[o].link_name]->getSamplingVolume() < iks->getSamplingVolume())
00229 use = false;
00230 if (use)
00231 {
00232 usedL[constr.orientation_constraints[o].link_name] = iks;
00233 logDebug("Allocated an IK-based sampler for group '%s' satisfying orientation constraints on link '%s'",
00234 jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
00235 }
00236 }
00237 }
00238 }
00239
00240 if (usedL.size() == 1)
00241 {
00242 if (samplers.empty())
00243 return usedL.begin()->second;
00244 else
00245 {
00246 samplers.push_back(usedL.begin()->second);
00247 return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
00248 }
00249 }
00250 else if (usedL.size() > 1)
00251 {
00252 logDebug("Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume",
00253 jmg->getName().c_str());
00254
00255 IKConstraintSamplerPtr iks = usedL.begin()->second;
00256 double msv = iks->getSamplingVolume();
00257 for (std::map<std::string, IKConstraintSamplerPtr>::const_iterator it = ++usedL.begin(); it != usedL.end(); ++it)
00258 {
00259 double v = it->second->getSamplingVolume();
00260 if (v < msv)
00261 {
00262 iks = it->second;
00263 msv = v;
00264 }
00265 }
00266 if (samplers.empty())
00267 {
00268 return iks;
00269 }
00270 else
00271 {
00272 samplers.push_back(iks);
00273 return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
00274 }
00275 }
00276 }
00277
00278
00279
00280 if (!ik_subgroup_alloc.empty())
00281 {
00282 logDebug("There are IK allocators for subgroups of group '%s'. Checking for corresponding position and/or "
00283 "orientation constraints",
00284 jmg->getName().c_str());
00285
00286 bool some_sampler_valid = false;
00287
00288 std::set<std::size_t> usedP, usedO;
00289 for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin();
00290 it != ik_subgroup_alloc.end(); ++it)
00291 {
00292
00293 moveit_msgs::Constraints sub_constr;
00294 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00295 if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
00296 if (usedP.find(p) == usedP.end())
00297 {
00298 sub_constr.position_constraints.push_back(constr.position_constraints[p]);
00299 usedP.insert(p);
00300 }
00301
00302 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00303 if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
00304 if (usedO.find(o) == usedO.end())
00305 {
00306 sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
00307 usedO.insert(o);
00308 }
00309
00310
00311 if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
00312 {
00313 logDebug("Attempting to construct a sampler for the '%s' subgroup of '%s'", it->first->getName().c_str(),
00314 jmg->getName().c_str());
00315 ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr);
00316 if (cs)
00317 {
00318 logDebug("Constructed a sampler for the joints corresponding to group '%s', but part of group '%s'",
00319 it->first->getName().c_str(), jmg->getName().c_str());
00320 some_sampler_valid = true;
00321 samplers.push_back(cs);
00322 }
00323 }
00324 }
00325 if (some_sampler_valid)
00326 {
00327 logDebug("Constructing sampler for group '%s' as a union of %u samplers", jmg->getName().c_str(),
00328 (unsigned int)samplers.size());
00329 return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
00330 }
00331 }
00332
00333
00334 if (joint_sampler)
00335 {
00336 logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
00337 return joint_sampler;
00338 }
00339
00340 logDebug("No constraints sampler allocated for group '%s'", jmg->getName().c_str());
00341
00342 return ConstraintSamplerPtr();
00343 }