Go to the documentation of this file.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/union_constraint_sampler.h>
00038 #include <moveit/constraint_samplers/default_constraint_samplers.h>
00039 #include <algorithm>
00040
00041 namespace constraint_samplers
00042 {
00043 struct OrderSamplers
00044 {
00045 bool operator()(const ConstraintSamplerPtr& a, const ConstraintSamplerPtr& b) const
00046 {
00047 const std::vector<std::string>& alinks = a->getJointModelGroup()->getUpdatedLinkModelNames();
00048 const std::vector<std::string>& blinks = b->getJointModelGroup()->getUpdatedLinkModelNames();
00049 std::set<std::string> a_updates(alinks.begin(), alinks.end());
00050 std::set<std::string> b_updates(blinks.begin(), blinks.end());
00051
00052 bool a_contains_b = std::includes(a_updates.begin(), a_updates.end(), b_updates.begin(), b_updates.end());
00053
00054 bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(), a_updates.begin(), a_updates.end());
00055
00056
00057 if (a_contains_b && !b_contains_a)
00058 return true;
00059 if (b_contains_a && !a_contains_b)
00060 return false;
00061
00062
00063 bool a_depends_on_b = false;
00064 bool b_depends_on_a = false;
00065 const std::vector<std::string>& fda = a->getFrameDependency();
00066 const std::vector<std::string>& fdb = b->getFrameDependency();
00067 for (std::size_t i = 0; i < fda.size() && !a_depends_on_b; ++i)
00068 for (std::size_t j = 0; j < blinks.size(); ++j)
00069 if (blinks[j] == fda[i])
00070 {
00071 a_depends_on_b = true;
00072 break;
00073 }
00074 for (std::size_t i = 0; i < fdb.size() && !b_depends_on_a; ++i)
00075 for (std::size_t j = 0; j < alinks.size(); ++j)
00076 if (alinks[j] == fdb[i])
00077 {
00078 b_depends_on_a = true;
00079 break;
00080 }
00081 if (b_depends_on_a && a_depends_on_b)
00082 {
00083 logWarn("Circular frame dependency! Sampling will likely produce invalid results (sampling for groups '%s' and "
00084 "'%s')",
00085 a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
00086 return true;
00087 }
00088 if (b_depends_on_a && !a_depends_on_b)
00089 return true;
00090 if (a_depends_on_b && !b_depends_on_a)
00091 return false;
00092
00093
00094 JointConstraintSampler* ja = dynamic_cast<JointConstraintSampler*>(a.get());
00095 JointConstraintSampler* jb = dynamic_cast<JointConstraintSampler*>(b.get());
00096 if (ja && jb == NULL)
00097 return true;
00098 if (jb && ja == NULL)
00099 return false;
00100
00101
00102 return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
00103 }
00104 };
00105 }
00106
00107 constraint_samplers::UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene,
00108 const std::string& group_name,
00109 const std::vector<ConstraintSamplerPtr>& samplers)
00110 : ConstraintSampler(scene, group_name), samplers_(samplers)
00111 {
00112
00113 std::stable_sort(samplers_.begin(), samplers_.end(), OrderSamplers());
00114
00115 for (std::size_t i = 0; i < samplers_.size(); ++i)
00116 {
00117 const std::vector<std::string>& fd = samplers_[i]->getFrameDependency();
00118 for (std::size_t j = 0; j < fd.size(); ++j)
00119 frame_depends_.push_back(fd[j]);
00120
00121 logDebug("Union sampler for group '%s' includes sampler for group '%s'", jmg_->getName().c_str(),
00122 samplers_[i]->getJointModelGroup()->getName().c_str());
00123 }
00124 }
00125
00126 bool constraint_samplers::UnionConstraintSampler::sample(robot_state::RobotState& state,
00127 const robot_state::RobotState& reference_state,
00128 unsigned int max_attempts)
00129 {
00130 state = reference_state;
00131 state.setToRandomPositions(jmg_);
00132
00133 if (samplers_.size() >= 1)
00134 {
00135 if (!samplers_[0]->sample(state, reference_state, max_attempts))
00136 return false;
00137 }
00138
00139 for (std::size_t i = 1; i < samplers_.size(); ++i)
00140 {
00141
00142
00143
00144 state.updateLinkTransforms();
00145 if (!samplers_[i]->sample(state, state, max_attempts))
00146 return false;
00147 }
00148 return true;
00149 }
00150
00151 bool constraint_samplers::UnionConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
00152 {
00153 for (std::size_t i = 0; i < samplers_.size(); ++i)
00154 {
00155
00156
00157
00158 state.updateLinkTransforms();
00159 if (!samplers_[i]->project(state, max_attempts))
00160 return false;
00161 }
00162 return true;
00163 }