generate IK solutions incrementally

This is not a good approach.
The same can be achieved by generating targets incrementally.
The better approach, to generate IK solutions incrementally,
has to maintain previous solutions for each target.
This commit is contained in:
v4hn 2018-09-23 21:46:52 +02:00 committed by Robert Haschke
parent 1e0a9401e7
commit 8392ed5ad3
2 changed files with 28 additions and 1 deletions

View File

@ -69,6 +69,10 @@ public:
void init(const core::RobotModelConstPtr &robot_model);
void onNewSolution(const SolutionBase &s) override;
bool canCompute() const override;
void compute() override;
void setEndEffector(const std::string& eef) {
setProperty("eef", eef);
}
@ -107,6 +111,9 @@ public:
void setMinSolutionDistance(double distance) {
setProperty("min_solution_distance", distance);
}
protected:
std::queue<const SolutionBase*> targets_;
};
} } }

View File

@ -203,7 +203,25 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
{
assert(s.start() && s.end());
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
// TODO: don't take pointers, this is dangerous
targets_.push(&s);
}
bool ComputeIK::canCompute() const {
return !targets_.empty() || WrapperBase::canCompute();
}
void ComputeIK::compute()
{
if(WrapperBase::canCompute())
WrapperBase::compute();
if(targets_.empty())
return;
const SolutionBase& s = *targets_.front();
targets_.pop();
// -1 TODO: this should not be necessary in my opinion: Why do you think so?
// It is, because the properties on the interface might change from call to call...
@ -211,6 +229,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
properties().performInitFrom(INTERFACE, s.start()->properties());
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
const bool ignore_collisions = props.get<bool>("ignore_collisions");
const auto& robot_model = sandbox_scene->getRobotModel();
const moveit::core::JointModelGroup* eef_jmg = nullptr;