45 bool operator()(
const ConstraintSamplerPtr& a,
const ConstraintSamplerPtr& b)
const 47 const std::vector<std::string>& alinks = a->getJointModelGroup()->getUpdatedLinkModelNames();
48 const std::vector<std::string>& blinks = b->getJointModelGroup()->getUpdatedLinkModelNames();
49 std::set<std::string> a_updates(alinks.begin(), alinks.end());
50 std::set<std::string> b_updates(blinks.begin(), blinks.end());
52 bool a_contains_b = std::includes(a_updates.begin(), a_updates.end(), b_updates.begin(), b_updates.end());
54 bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(), a_updates.begin(), a_updates.end());
57 if (a_contains_b && !b_contains_a)
59 if (b_contains_a && !a_contains_b)
63 bool a_depends_on_b =
false;
64 bool b_depends_on_a =
false;
65 const std::vector<std::string>& fda = a->getFrameDependency();
66 const std::vector<std::string>& fdb = b->getFrameDependency();
67 for (std::size_t i = 0; i < fda.size() && !a_depends_on_b; ++i)
68 for (std::size_t j = 0; j < blinks.size(); ++j)
69 if (blinks[j] == fda[i])
71 a_depends_on_b =
true;
74 for (std::size_t i = 0; i < fdb.size() && !b_depends_on_a; ++i)
75 for (std::size_t j = 0; j < alinks.size(); ++j)
76 if (alinks[j] == fdb[i])
78 b_depends_on_a =
true;
81 if (b_depends_on_a && a_depends_on_b)
84 "Circular frame dependency! " 85 "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')",
86 a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
89 if (b_depends_on_a && !a_depends_on_b)
91 if (a_depends_on_b && !b_depends_on_a)
97 if (ja && jb ==
nullptr)
99 if (jb && ja ==
nullptr)
103 return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
108 const std::string& group_name,
109 const std::vector<ConstraintSamplerPtr>& samplers)
115 for (std::size_t i = 0; i <
samplers_.size(); ++i)
117 const std::vector<std::string>& fd =
samplers_[i]->getFrameDependency();
118 for (std::size_t j = 0; j < fd.size(); ++j)
121 ROS_DEBUG_NAMED(
"constraint_samplers",
"Union sampler for group '%s' includes sampler for group '%s'",
127 unsigned int max_attempts)
129 state = reference_state;
138 for (std::size_t i = 1; i <
samplers_.size(); ++i)
152 for (std::size_t i = 0; i <
samplers_.size(); ++i)
#define ROS_WARN_NAMED(name,...)
const std::string & getName() const
Get the name of the joint group.
UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const std::vector< ConstraintSamplerPtr > &samplers)
Constructor, which will re-order its internal list of samplers on construction.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
Produces a sample from all configured samplers.
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
ConstraintSampler is an abstract base class that allows the sampling of a kinematic state for a parti...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
#define ROS_DEBUG_NAMED(name,...)
bool operator()(const ConstraintSamplerPtr &a, const ConstraintSamplerPtr &b) const
std::vector< ConstraintSamplerPtr > samplers_
Holder for sorted internal list of samplers.
const robot_model::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
std::vector< std::string > frame_depends_
Holds the set of frames that must exist in the reference state to allow samples to be drawn...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Representation of a robot's state. This includes position, velocity, acceleration and effort...
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...