mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
1e0a9401e7
commit
8392ed5ad3
@ -69,6 +69,10 @@ public:
|
|||||||
void init(const core::RobotModelConstPtr &robot_model);
|
void init(const core::RobotModelConstPtr &robot_model);
|
||||||
void onNewSolution(const SolutionBase &s) override;
|
void onNewSolution(const SolutionBase &s) override;
|
||||||
|
|
||||||
|
bool canCompute() const override;
|
||||||
|
|
||||||
|
void compute() override;
|
||||||
|
|
||||||
void setEndEffector(const std::string& eef) {
|
void setEndEffector(const std::string& eef) {
|
||||||
setProperty("eef", eef);
|
setProperty("eef", eef);
|
||||||
}
|
}
|
||||||
@ -107,6 +111,9 @@ public:
|
|||||||
void setMinSolutionDistance(double distance) {
|
void setMinSolutionDistance(double distance) {
|
||||||
setProperty("min_solution_distance", distance);
|
setProperty("min_solution_distance", distance);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
std::queue<const SolutionBase*> targets_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -203,7 +203,25 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
{
|
{
|
||||||
assert(s.start() && s.end());
|
assert(s.start() && s.end());
|
||||||
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
|
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?
|
// -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...
|
// 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());
|
properties().performInitFrom(INTERFACE, s.start()->properties());
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
|
|
||||||
|
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
|
||||||
|
|
||||||
const bool ignore_collisions = props.get<bool>("ignore_collisions");
|
const bool ignore_collisions = props.get<bool>("ignore_collisions");
|
||||||
const auto& robot_model = sandbox_scene->getRobotModel();
|
const auto& robot_model = sandbox_scene->getRobotModel();
|
||||||
const moveit::core::JointModelGroup* eef_jmg = nullptr;
|
const moveit::core::JointModelGroup* eef_jmg = nullptr;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user