42 constraint_samplers::ConstraintSamplerPtr
44 const std::string& group_name,
45 const moveit_msgs::Constraints& constr)
const 55 constraint_samplers::ConstraintSamplerPtr
57 const std::string& group_name,
58 const moveit_msgs::Constraints& constr)
62 return constraint_samplers::ConstraintSamplerPtr();
66 "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n",
67 jmg->
getName().c_str(), ss.str().c_str());
69 ConstraintSamplerPtr joint_sampler;
71 if (!constr.joint_constraints.empty())
73 ROS_DEBUG_NAMED(
"constraint_samplers",
"There are joint constraints specified. " 74 "Attempting to construct a JointConstraintSampler for group '%s'",
77 std::map<std::string, bool> joint_coverage;
82 std::vector<kinematic_constraints::JointConstraint> jc;
83 for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
86 if (j.configure(constr.joint_constraints[i]))
88 if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
90 joint_coverage[j.getJointVariableName()] =
true;
97 bool full_coverage =
true;
98 for (std::map<std::string, bool>::iterator it = joint_coverage.begin(); it != joint_coverage.end(); ++it)
101 full_coverage =
false;
109 if (sampler->configure(jc))
111 ROS_DEBUG_NAMED(
"constraint_samplers",
"Allocated a sampler satisfying joint constraints for group '%s'",
122 if (sampler->configure(jc))
125 "Temporary sampler satisfying joint constraints for group '%s' allocated. " 126 "Looking for different types of constraints before returning though.",
128 joint_sampler = sampler;
133 std::vector<ConstraintSamplerPtr> samplers;
135 samplers.push_back(joint_sampler);
138 const robot_model::JointModelGroup::KinematicsSolver& ik_alloc = jmg->
getGroupKinematics().first;
145 ROS_DEBUG_NAMED(
"constraint_samplers",
"There is an IK allocator for '%s'. " 146 "Checking for corresponding position and/or orientation constraints",
150 std::map<std::string, IKConstraintSamplerPtr> usedL;
156 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
157 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
158 if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
160 kinematic_constraints::PositionConstraintPtr pc(
162 kinematic_constraints::OrientationConstraintPtr oc(
164 if (pc->configure(constr.position_constraints[p], scene->getTransforms()) &&
165 oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
172 if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
174 if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
179 usedL[constr.position_constraints[p].link_name] = iks;
180 ROS_DEBUG_NAMED(
"constraint_samplers",
"Allocated an IK-based sampler for group '%s' " 181 "satisfying position and orientation constraints on link '%s'",
182 jmg->
getName().c_str(), constr.position_constraints[p].link_name.c_str());
189 std::map<std::string, IKConstraintSamplerPtr> usedL_fullPose = usedL;
191 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
195 if (usedL_fullPose.find(constr.position_constraints[p].link_name) != usedL_fullPose.end())
198 kinematic_constraints::PositionConstraintPtr pc(
200 if (pc->configure(constr.position_constraints[p], scene->getTransforms()))
206 if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
207 if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
211 usedL[constr.position_constraints[p].link_name] = iks;
212 ROS_DEBUG_NAMED(
"constraint_samplers",
"Allocated an IK-based sampler for group '%s' " 213 "satisfying position constraints on link '%s'",
214 jmg->
getName().c_str(), constr.position_constraints[p].link_name.c_str());
220 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
224 if (usedL_fullPose.find(constr.orientation_constraints[o].link_name) != usedL_fullPose.end())
227 kinematic_constraints::OrientationConstraintPtr oc(
229 if (oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
235 if (usedL.find(constr.orientation_constraints[o].link_name) != usedL.end())
236 if (usedL[constr.orientation_constraints[o].link_name]->getSamplingVolume() < iks->getSamplingVolume())
240 usedL[constr.orientation_constraints[o].link_name] = iks;
241 ROS_DEBUG_NAMED(
"constraint_samplers",
"Allocated an IK-based sampler for group '%s' " 242 "satisfying orientation constraints on link '%s'",
243 jmg->
getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
249 if (usedL.size() == 1)
251 if (samplers.empty())
252 return usedL.begin()->second;
255 samplers.push_back(usedL.begin()->second);
259 else if (usedL.size() > 1)
262 "Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume",
265 IKConstraintSamplerPtr iks = usedL.begin()->second;
266 double msv = iks->getSamplingVolume();
267 for (std::map<std::string, IKConstraintSamplerPtr>::const_iterator it = ++usedL.begin(); it != usedL.end(); ++it)
269 double v = it->second->getSamplingVolume();
276 if (samplers.empty())
282 samplers.push_back(iks);
290 if (!ik_subgroup_alloc.empty())
292 ROS_DEBUG_NAMED(
"constraint_samplers",
"There are IK allocators for subgroups of group '%s'. " 293 "Checking for corresponding position and/or orientation constraints",
296 bool some_sampler_valid =
false;
298 std::set<std::size_t> usedP, usedO;
299 for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin();
300 it != ik_subgroup_alloc.end(); ++it)
303 moveit_msgs::Constraints sub_constr;
304 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
305 if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
306 if (usedP.find(p) == usedP.end())
308 sub_constr.position_constraints.push_back(constr.position_constraints[p]);
312 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
313 if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
314 if (usedO.find(o) == usedO.end())
316 sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
321 if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
323 ROS_DEBUG_NAMED(
"constraint_samplers",
"Attempting to construct a sampler for the '%s' subgroup of '%s'",
324 it->first->getName().c_str(), jmg->
getName().c_str());
328 ROS_DEBUG_NAMED(
"constraint_samplers",
"Constructed a sampler for the joints corresponding to group '%s', " 329 "but part of group '%s'",
330 it->first->getName().c_str(), jmg->
getName().c_str());
331 some_sampler_valid =
true;
332 samplers.push_back(cs);
336 if (some_sampler_valid)
338 ROS_DEBUG_NAMED(
"constraint_samplers",
"Constructing sampler for group '%s' as a union of %zu samplers",
339 jmg->
getName().c_str(), samplers.size());
347 ROS_DEBUG_NAMED(
"constraint_samplers",
"Allocated a sampler satisfying joint constraints for group '%s'",
349 return joint_sampler;
352 ROS_DEBUG_NAMED(
"constraint_samplers",
"No constraints sampler allocated for group '%s'", jmg->
getName().c_str());
354 return ConstraintSamplerPtr();
A structure for potentially holding a position constraint and an orientation constraint for use durin...
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
Class for constraints on the orientation of a link.
std::vector< ConstraintSamplerAllocatorPtr > sampler_alloc_
Holds the constraint sampler allocators, which will be tested in order.
const std::string & getName() const
Get the name of the joint group.
Class for handling single DOF joint constraints.
This class exists as a union of constraint samplers. It contains a vector of constraint samplers...
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
#define ROS_DEBUG_NAMED(name,...)
const std::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
A class that allows the sampling of IK constraints.
std::map< const JointModelGroup *, KinematicsSolver > KinematicsSolverMap
Map from group instances to allocator functions & bijections.
Class for constraints on the XYZ position of a link.
ConstraintSamplerPtr selectSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const moveit_msgs::Constraints &constr) const
Selects among the potential sampler allocators.
static ConstraintSamplerPtr selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const moveit_msgs::Constraints &constr)
Default logic to select a ConstraintSampler given a constraints message.