mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cost computation: provide valid interface states for Solution
The CostTerm's should get only a single solution that is well-setup with its InterfaceStates. That's impossible though because these states are stored in different places depending on the cost. To avoid this, set stub states for cost computation and change them to the real states later on. This is motivated by the Clearance cost which can act on an InterfaceState only.
This commit is contained in:
parent
bbf7d415f6
commit
53c0964618
@ -197,6 +197,7 @@ class StagePrivate;
|
||||
/// abstract base class for solutions (primitive and sequences)
|
||||
class SolutionBase
|
||||
{
|
||||
friend StagePrivate; // for set[Start|End]StateUnsafe
|
||||
public:
|
||||
virtual ~SolutionBase() = default;
|
||||
|
||||
@ -207,18 +208,22 @@ public:
|
||||
template <Interface::Direction dir>
|
||||
inline const InterfaceState::Solutions& trajectories() const;
|
||||
|
||||
/** set the solution's start_state_
|
||||
*
|
||||
* Must not be used with different states because it registers the solution with the state as well.
|
||||
*/
|
||||
inline void setStartState(const InterfaceState& state) {
|
||||
// only allow setting once (by Stage)
|
||||
assert(start_ == nullptr || start_ == &state);
|
||||
start_ = &state;
|
||||
const_cast<InterfaceState&>(state).addOutgoing(this);
|
||||
setStartStateUnsafe(state);
|
||||
}
|
||||
|
||||
/** set the solution's end_state_
|
||||
*
|
||||
* Must not be used with different states because it registers the solution with the state as well.
|
||||
*/
|
||||
inline void setEndState(const InterfaceState& state) {
|
||||
// only allow setting once (by Stage)
|
||||
assert(end_ == nullptr || end_ == &state);
|
||||
end_ = &state;
|
||||
const_cast<InterfaceState&>(state).addIncoming(this);
|
||||
setEndStateUnsafe(state);
|
||||
}
|
||||
|
||||
inline const Stage* creator() const { return creator_; }
|
||||
@ -247,6 +252,18 @@ protected:
|
||||
SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "")
|
||||
: creator_(creator), cost_(cost), comment_(std::move(comment)) {}
|
||||
|
||||
/** unsafe setter for start_state_
|
||||
*
|
||||
* must only be used if the previously set state removes its link to this solution
|
||||
*/
|
||||
void setStartStateUnsafe(const InterfaceState& state);
|
||||
|
||||
/** unsafe setter for end_state_
|
||||
*
|
||||
* must only be used if the previously set state removes its link to this solution
|
||||
*/
|
||||
void setEndStateUnsafe(const InterfaceState& state);
|
||||
|
||||
private:
|
||||
// back-pointer to creating stage, allows to access sub-solutions
|
||||
Stage* creator_;
|
||||
|
||||
@ -130,7 +130,6 @@ void StagePrivate::validateConnectivity() const {
|
||||
}
|
||||
|
||||
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
||||
solution->setCreator(me());
|
||||
if (introspection_)
|
||||
introspection_->registerSolution(*solution);
|
||||
|
||||
@ -147,34 +146,59 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
|
||||
|
||||
void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) {
|
||||
assert(nextStarts());
|
||||
|
||||
solution->setCreator(me());
|
||||
|
||||
if (!solution->isFailure()) {
|
||||
// chicken-and-egg problem: we don't know whether/where we will store the solution,
|
||||
// but need to store it properly to compute the cost with a clean interface:
|
||||
// set state copies for solution before computing cost:
|
||||
InterfaceState tmp_from{ from }, tmp_to{ to };
|
||||
solution->setStartState(tmp_from);
|
||||
solution->setEndState(tmp_to);
|
||||
|
||||
computeCost(*solution);
|
||||
}
|
||||
|
||||
if (!storeSolution(solution))
|
||||
return; // solution dropped
|
||||
me()->forwardProperties(from, to);
|
||||
|
||||
auto to_it = states_.insert(states_.end(), std::move(to));
|
||||
|
||||
solution->setStartState(from);
|
||||
solution->setEndState(*to_it);
|
||||
// register stored interfaces with solution
|
||||
solution->setStartStateUnsafe(from);
|
||||
solution->setEndStateUnsafe(*to_it);
|
||||
|
||||
if (!solution->isFailure() && computeCost(*solution)) {
|
||||
if (!solution->isFailure())
|
||||
nextStarts()->add(*to_it);
|
||||
}
|
||||
|
||||
newSolution(solution);
|
||||
}
|
||||
|
||||
void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution) {
|
||||
assert(prevEnds());
|
||||
|
||||
solution->setCreator(me());
|
||||
|
||||
if (!solution->isFailure()) {
|
||||
InterfaceState tmp_from{ from }, tmp_to{ to };
|
||||
solution->setStartState(tmp_from);
|
||||
solution->setEndState(tmp_to);
|
||||
|
||||
computeCost(*solution);
|
||||
}
|
||||
|
||||
if (!storeSolution(solution))
|
||||
return; // solution dropped
|
||||
me()->forwardProperties(to, from);
|
||||
|
||||
auto from_it = states_.insert(states_.end(), std::move(from));
|
||||
|
||||
solution->setStartState(*from_it);
|
||||
solution->setEndState(to);
|
||||
solution->setStartStateUnsafe(*from_it);
|
||||
solution->setEndStateUnsafe(to);
|
||||
|
||||
if (!solution->isFailure() && computeCost(*solution))
|
||||
if (!solution->isFailure())
|
||||
prevEnds()->add(*from_it);
|
||||
|
||||
newSolution(solution);
|
||||
@ -182,16 +206,27 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
|
||||
|
||||
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
|
||||
assert(prevEnds() && nextStarts());
|
||||
|
||||
solution->setCreator(me());
|
||||
|
||||
if (!solution->isFailure()) {
|
||||
InterfaceState tmp_from{ state }, tmp_to{ state };
|
||||
solution->setStartState(tmp_from);
|
||||
solution->setEndState(tmp_to);
|
||||
|
||||
computeCost(*solution);
|
||||
}
|
||||
|
||||
if (!storeSolution(solution))
|
||||
return; // solution dropped
|
||||
|
||||
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
|
||||
auto to = states_.insert(states_.end(), std::move(state));
|
||||
|
||||
solution->setStartState(*from);
|
||||
solution->setEndState(*to);
|
||||
solution->setStartStateUnsafe(*from);
|
||||
solution->setEndStateUnsafe(*to);
|
||||
|
||||
if (!solution->isFailure() && computeCost(*solution)) {
|
||||
if (!solution->isFailure()) {
|
||||
prevEnds()->add(*from);
|
||||
nextStarts()->add(*to);
|
||||
}
|
||||
@ -200,14 +235,21 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
|
||||
}
|
||||
|
||||
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
|
||||
solution->setCreator(me());
|
||||
|
||||
if (!solution->isFailure()) {
|
||||
InterfaceState tmp_from{ from }, tmp_to{ to };
|
||||
solution->setStartState(tmp_from);
|
||||
solution->setEndState(tmp_to);
|
||||
|
||||
computeCost(*solution);
|
||||
}
|
||||
|
||||
if (!storeSolution(solution))
|
||||
return; // solution dropped
|
||||
|
||||
solution->setStartState(from);
|
||||
solution->setEndState(to);
|
||||
|
||||
if (!solution->isFailure())
|
||||
computeCost(*solution);
|
||||
solution->setStartStateUnsafe(from);
|
||||
solution->setEndStateUnsafe(to);
|
||||
|
||||
newSolution(solution);
|
||||
}
|
||||
|
||||
@ -141,6 +141,16 @@ void SolutionBase::setCreator(Stage* creator) {
|
||||
creator_ = creator;
|
||||
}
|
||||
|
||||
void SolutionBase::setStartStateUnsafe(const InterfaceState& state) {
|
||||
start_ = &state;
|
||||
const_cast<InterfaceState&>(state).addOutgoing(this);
|
||||
}
|
||||
|
||||
void SolutionBase::setEndStateUnsafe(const InterfaceState& state) {
|
||||
end_ = &state;
|
||||
const_cast<InterfaceState&>(state).addIncoming(this);
|
||||
}
|
||||
|
||||
void SolutionBase::setCost(double cost) {
|
||||
cost_ = cost;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user