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(),
00053 b_updates.begin(), b_updates.end());
00054
00055 bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(),
00056 a_updates.begin(), a_updates.end());
00057
00058
00059 if (a_contains_b && !b_contains_a)
00060 return true;
00061 if (b_contains_a && !a_contains_b)
00062 return false;
00063
00064
00065 bool a_depends_on_b = false;
00066 bool b_depends_on_a = false;
00067 const std::vector<std::string> &fda = a->getFrameDependency();
00068 const std::vector<std::string> &fdb = b->getFrameDependency();
00069 for (std::size_t i = 0 ; i < fda.size() && !a_depends_on_b ; ++i)
00070 for (std::size_t j = 0 ; j < blinks.size() ; ++j)
00071 if (blinks[j] == fda[i])
00072 {
00073 a_depends_on_b = true;
00074 break;
00075 }
00076 for (std::size_t i = 0 ; i < fdb.size() && !b_depends_on_a ; ++i)
00077 for (std::size_t j = 0 ; j < alinks.size() ; ++j)
00078 if (alinks[j] == fdb[i])
00079 {
00080 b_depends_on_a = true;
00081 break;
00082 }
00083 if (b_depends_on_a && a_depends_on_b)
00084 {
00085 logWarn("Circular frame dependency! Sampling will likely produce invalid results (sampling for groups '%s' and '%s')",
00086 a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
00087 return true;
00088 }
00089 if (b_depends_on_a && !a_depends_on_b)
00090 return true;
00091 if(a_depends_on_b && !b_depends_on_a)
00092 return false;
00093
00094
00095 JointConstraintSampler *ja = dynamic_cast<JointConstraintSampler*>(a.get());
00096 JointConstraintSampler *jb = dynamic_cast<JointConstraintSampler*>(b.get());
00097 if (ja && jb == NULL)
00098 return true;
00099 if (jb && ja == NULL)
00100 return false;
00101
00102
00103 return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
00104 }
00105 };
00106 }
00107
00108 constraint_samplers::UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, 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(), samplers_[i]->getJointModelGroup()->getName().c_str());
00122 }
00123 }
00124
00125 bool constraint_samplers::UnionConstraintSampler::sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
00126 {
00127 state = reference_state;
00128 state.setToRandomPositions(jmg_);
00129
00130 if (samplers_.size() >= 1)
00131 {
00132 if (!samplers_[0]->sample(state, reference_state, max_attempts))
00133 return false;
00134 }
00135
00136 for (std::size_t i = 1 ; i < samplers_.size() ; ++i)
00137 if (!samplers_[i]->sample(state, state, max_attempts))
00138 return false;
00139
00140 return true;
00141 }
00142
00143 bool constraint_samplers::UnionConstraintSampler::project(robot_state::RobotState &state, unsigned int max_attempts)
00144 {
00145 for (std::size_t i = 0 ; i < samplers_.size() ; ++i)
00146 if (!samplers_[i]->project(state, max_attempts))
00147 return false;
00148 return true;
00149 }
00150