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