mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
FixedState: ignore_collisions=false
Check collisions for FixedState's scene and report failure if needed. Optionally, disable the check via the property ignore_collisions=true.
This commit is contained in:
parent
ca38d11303
commit
ee7cec2aab
@ -45,6 +45,7 @@ namespace stages {
|
||||
|
||||
FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene)
|
||||
: Generator(name), scene_(scene) {
|
||||
properties().declare("ignore_collisions", false);
|
||||
setCostTerm(std::make_unique<cost::Constant>(0.0));
|
||||
}
|
||||
|
||||
@ -62,7 +63,10 @@ bool FixedState::canCompute() const {
|
||||
}
|
||||
|
||||
void FixedState::compute() {
|
||||
spawn(InterfaceState(scene_), 0.0);
|
||||
auto cost = !properties().get<bool>("ignore_collisions") && scene_->isStateColliding() ?
|
||||
std::numeric_limits<double>::infinity() :
|
||||
0.0;
|
||||
spawn(InterfaceState(scene_), cost);
|
||||
ran_ = true;
|
||||
}
|
||||
} // namespace stages
|
||||
|
||||
Loading…
Reference in New Issue
Block a user