From 8392ed5ad31a01bdc8572c31d8263a2af12e81d9 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sun, 23 Sep 2018 21:46:52 +0200 Subject: [PATCH] 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. --- .../task_constructor/stages/compute_ik.h | 7 ++++++ core/src/stages/compute_ik.cpp | 22 ++++++++++++++++++- 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index 8426745b..c0b65954 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -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 targets_; }; } } } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index de2dc019..be5b779f 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -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("ignore_collisions"); const auto& robot_model = sandbox_scene->getRobotModel(); const moveit::core::JointModelGroup* eef_jmg = nullptr;