70 const std::string& group_name,
71 const moveit_msgs::msg::Constraints& constr)
75 return ConstraintSamplerPtr();
78 RCLCPP_DEBUG(getLogger(),
79 "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n",
80 jmg->
getName().c_str(), ss.str().c_str());
82 ConstraintSamplerPtr joint_sampler;
84 if (!constr.joint_constraints.empty())
86 RCLCPP_DEBUG(getLogger(),
87 "There are joint constraints specified. "
88 "Attempting to construct a JointConstraintSampler for group '%s'",
91 std::map<std::string, bool> joint_coverage;
93 joint_coverage[joint] =
false;
96 std::vector<kinematic_constraints::JointConstraint> jc;
97 for (
const moveit_msgs::msg::JointConstraint& joint_constraint : constr.joint_constraints)
111 bool full_coverage =
true;
112 for (
const std::pair<const std::string, bool>& it : joint_coverage)
116 full_coverage =
false;
124 JointConstraintSamplerPtr sampler = std::make_shared<JointConstraintSampler>(scene, jmg->
getName());
125 if (sampler->configure(jc))
127 RCLCPP_DEBUG(getLogger(),
"Allocated a sampler satisfying joint constraints for group '%s'",
137 JointConstraintSamplerPtr sampler = std::make_shared<JointConstraintSampler>(scene, jmg->
getName());
138 if (sampler->configure(jc))
140 RCLCPP_DEBUG(getLogger(),
141 "Temporary sampler satisfying joint constraints for group '%s' allocated. "
142 "Looking for different types of constraints before returning though.",
144 joint_sampler = sampler;
149 std::vector<ConstraintSamplerPtr> samplers;
151 samplers.push_back(joint_sampler);
161 RCLCPP_DEBUG(getLogger(),
162 "There is an IK allocator for '%s'. "
163 "Checking for corresponding position and/or orientation constraints",
167 std::map<std::string, IKConstraintSamplerPtr> used_l;
173 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
175 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
177 if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
179 kinematic_constraints::PositionConstraintPtr pc(
181 kinematic_constraints::OrientationConstraintPtr oc(
183 if (pc->configure(constr.position_constraints[p], scene->getTransforms()) &&
184 oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
186 IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->
getName());
191 if (used_l.find(constr.position_constraints[p].link_name) != used_l.end())
194 if (used_l[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
200 used_l[constr.position_constraints[p].link_name] = iks;
201 RCLCPP_DEBUG(getLogger(),
202 "Allocated an IK-based sampler for group '%s' "
203 "satisfying position and orientation constraints on link '%s'",
204 jmg->
getName().c_str(), constr.position_constraints[p].link_name.c_str());
213 std::map<std::string, IKConstraintSamplerPtr> used_l_full_pose = used_l;
215 for (
const moveit_msgs::msg::PositionConstraint& position_constraint : constr.position_constraints)
219 if (used_l_full_pose.find(position_constraint.link_name) != used_l_full_pose.end())
222 kinematic_constraints::PositionConstraintPtr pc(
224 if (pc->configure(position_constraint, scene->getTransforms()))
226 IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->
getName());
230 if (used_l.find(position_constraint.link_name) != used_l.end())
232 if (used_l[position_constraint.link_name]->getSamplingVolume() < iks->getSamplingVolume())
237 used_l[position_constraint.link_name] = iks;
238 RCLCPP_DEBUG(getLogger(),
239 "Allocated an IK-based sampler for group '%s' "
240 "satisfying position constraints on link '%s'",
241 jmg->
getName().c_str(), position_constraint.link_name.c_str());
247 for (
const moveit_msgs::msg::OrientationConstraint& orientation_constraint : constr.orientation_constraints)
251 if (used_l_full_pose.find(orientation_constraint.link_name) != used_l_full_pose.end())
254 kinematic_constraints::OrientationConstraintPtr oc(
256 if (oc->configure(orientation_constraint, scene->getTransforms()))
258 IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->
getName());
262 if (used_l.find(orientation_constraint.link_name) != used_l.end())
264 if (used_l[orientation_constraint.link_name]->getSamplingVolume() < iks->getSamplingVolume())
269 used_l[orientation_constraint.link_name] = iks;
270 RCLCPP_DEBUG(getLogger(),
271 "Allocated an IK-based sampler for group '%s' "
272 "satisfying orientation constraints on link '%s'",
273 jmg->
getName().c_str(), orientation_constraint.link_name.c_str());
279 if (used_l.size() == 1)
281 if (samplers.empty())
283 return used_l.begin()->second;
287 samplers.push_back(used_l.begin()->second);
288 return std::make_shared<UnionConstraintSampler>(scene, jmg->
getName(), samplers);
291 else if (used_l.size() > 1)
293 RCLCPP_DEBUG(getLogger(),
294 "Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume",
297 IKConstraintSamplerPtr iks = used_l.begin()->second;
298 double msv = iks->getSamplingVolume();
299 for (std::map<std::string, IKConstraintSamplerPtr>::const_iterator it = ++used_l.begin(); it != used_l.end(); ++it)
301 double v = it->second->getSamplingVolume();
308 if (samplers.empty())
314 samplers.push_back(iks);
315 return std::make_shared<UnionConstraintSampler>(scene, jmg->
getName(), samplers);
322 if (!ik_subgroup_alloc.empty())
324 RCLCPP_DEBUG(getLogger(),
325 "There are IK allocators for subgroups of group '%s'. "
326 "Checking for corresponding position and/or orientation constraints",
329 bool some_sampler_valid =
false;
331 std::set<std::size_t> used_p, used_o;
332 for (moveit::core::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin();
333 it != ik_subgroup_alloc.end(); ++it)
336 moveit_msgs::msg::Constraints sub_constr;
337 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
339 if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
341 if (used_p.find(p) == used_p.end())
343 sub_constr.position_constraints.push_back(constr.position_constraints[p]);
349 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
351 if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
353 if (used_o.find(o) == used_o.end())
355 sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
362 if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
364 RCLCPP_DEBUG(getLogger(),
"Attempting to construct a sampler for the '%s' subgroup of '%s'",
365 it->first->getName().c_str(), jmg->
getName().c_str());
369 RCLCPP_DEBUG(getLogger(),
370 "Constructed a sampler for the joints corresponding to group '%s', "
371 "but part of group '%s'",
372 it->first->getName().c_str(), jmg->
getName().c_str());
373 some_sampler_valid =
true;
374 samplers.push_back(cs);
378 if (some_sampler_valid)
380 RCLCPP_DEBUG(getLogger(),
"Constructing sampler for group '%s' as a union of %zu samplers",
381 jmg->
getName().c_str(), samplers.size());
382 return std::make_shared<UnionConstraintSampler>(scene, jmg->
getName(), samplers);
389 RCLCPP_DEBUG(getLogger(),
"Allocated a sampler satisfying joint constraints for group '%s'", jmg->
getName().c_str());
390 return joint_sampler;
393 RCLCPP_DEBUG(getLogger(),
"No constraints sampler allocated for group '%s'", jmg->
getName().c_str());
395 return ConstraintSamplerPtr();