Commit Graph

337 Commits

Author SHA1 Message Date
Robert Haschke
f229743024 Debug evolution of interfaces 2021-03-22 14:42:23 +01:00
Robert Haschke
9466517f72 operator<< for Interface + Priority
Co-authored by v4hn
2021-03-22 14:42:23 +01:00
Robert Haschke
604a0da1e4 Actually implement pruning and re-enabling of states 2021-03-22 14:42:23 +01:00
Robert Haschke
19484cec64 Restore pruning
If a stage fails to find a solution, this often implies that further planning
on the open end(s) of connected InterfaceStates is not needed anymore.
Thus the InterfaceStates along all connected solution paths will be marked as disabled.
They are not removed from the pending state lists though, because they might get
reactivated by solutions found in future.

To this end, we introduced the method ContainerBase::onNewFailure().
2021-03-22 14:42:23 +01:00
Robert Haschke
6c18e2b482 test comparison of ConnectingPrivate's StatePairs 2021-03-22 14:42:23 +01:00
Robert Haschke
2a09daa42f InterfaceState::Priority::enabled()
To allow pruning, we need to enable and disable InterfaceStates to be considered for further
planning. In the past, we just indicated the disabled status with infinite costs.
However, because we might need to re-enable states (with previous state),
we need to separate these two concepts.
2021-03-22 14:42:23 +01:00
Robert Haschke
10f6b7a58e MoveTo/MoveRel: reduce code duplication
... using templated versions for computeForward + computeBackward
as they essentially perform the same operations.
2021-03-19 18:22:04 +01:00
Robert Haschke
dfffd77eee MoveTo/MoveRel: Report errors via solution comments instead of ROS_WARN 2021-03-19 18:22:04 +01:00
Robert Haschke
0b63c590c5 Replace MoveIt! -> MoveIt 2021-03-15 14:29:19 +01:00
v4hn
3d8faea079 add test to validate correct solution state in computeCost
This is somewhat cumbersome because of the additional internal/external
layer introduced through the container.
But it's still better than leaving this unverified.
2021-03-05 14:38:29 +01:00
v4hn
f4fb8c6f71 fix computeCost interface (again)
Fixes the missing creator pointer introduced in a6a23d3775
2021-03-05 14:38:23 +01:00
v4hn
f7a94956c3 output stream formatting for InterfaceState::Priority 2021-03-05 14:38:06 +01:00
Robert Haschke
a6fa45214f Remove return value from Container::insert
adapted Container::add

Logical consequence of removing the ROS_ERROR output in `setParent`
2021-02-24 12:22:38 +01:00
v4hn
4386d0cc60 remove roscpp as an export dependency for the core package
We do use ROS in the background. But there is no need for a public export dependency on it.

This patch also resolves the following catkin_lint issue:

moveit_task_constructor_core: CMakeLists.txt(17): error: package 'roscpp' must be in CATKIN_DEPENDS in catkin_package()
2021-02-24 12:22:38 +01:00
Robert Haschke
9f18219993 minor cleanups
* unify usage of pimpl()
* fix StatePair constructors
* improve/add comments
* test_container: reset MOCK_ID for each test to facilitate identification of stages
2020-12-06 23:55:58 +01:00
Robert Haschke
0925031a81 Propagating: disable infeasible state pruning
Because stages might get re-enabled later, we must not throw them away.
Remove dropFailedStarts(), dropFailedEnds().
Extend Pruning unittest.
2020-12-06 23:55:10 +01:00
Robert Haschke
a6a23d3775 Remove set[Start|End]StateUnsafe
These methods were introduced to temporarily set meaningful states for cost calculation
w/o connecting the solution to these temporary states (to solve a chicken-egg problem).
This commit provides TmpInterfaceStateProvider as an alternative approach to this problem.

This essentially reverts 53c0964618.
2020-12-06 04:11:35 +01:00
Robert Haschke
c8e75ab205 Simplify SolutionCollector 2020-12-06 04:11:35 +01:00
Robert Haschke
576a1aa48d Remove default constructor for Priority
It's better to explicitly state the initial value.
2020-12-06 04:11:35 +01:00
Robert Haschke
56ad37d80e Provide move constructor for InterfaceState
The code was tuned to move InterfaceStates around, but there wasn't the matching constructor!
2020-12-06 04:11:35 +01:00
Robert Haschke
f05b08fa8a SerialContainer: Simplify solution traversal
Compute depth + accumulated cost of a partial solution sequence using InterfaceState::Priority.
2020-12-06 04:09:59 +01:00
Robert Haschke
08dc19f7b3 Fix cost calculation
The default CostTerm should rely on precomputed costs. Only, if a CostTerm is explicitly defined
for a particular stage, it should modify costs.
2020-12-06 04:06:08 +01:00
Robert Haschke
8b1c66c4b7 Allow naming a Task
which just names the top-level container
2020-10-24 04:43:30 +02:00
Robert Haschke
b071a059f9 Rename TaskPrivate::id -> TaskPrivate::ns
TaskPrivate's id_ actually served as a namespace parameter.
2020-10-24 04:43:30 +02:00
Jafar Abdi
be270cb574
Task: Add ability to set timeout (#213)
Task::setTimeout will allow setting an overall timeout.
2020-10-24 03:22:05 +02:00
Robert Haschke
4089d5eefa Container::remove(): return removed Stage 2020-09-22 10:56:11 +02:00
Robert Haschke
687fcb6a6d Update comments to solution's computeCost() methods 2020-09-21 22:52:19 +02:00
v4hn
62586c2688 rework CostTerm's from support flags to vtable
requested in review.

Without support for custom aggregators, which we dropped again
after finding more flaws with it, I agree that this is the nicer solution.

On the downside, it converts the interfaces from copyable objects
to another round of shared_ptrs.

I added shortcuts for lambda costs to keep support for
`stage->setCostTerm([](auto&& s){ return 42; })`
without the additional
`stage->setCostTerm(LambdaCostTerm{ [](auto&& s){ return 42; } } )`
2020-09-21 22:52:19 +02:00
v4hn
45e6cdb47d Revert "add CostAggregator for SerialContainer"
This reverts commit dc7ce9bdfac97eb468f5a850adcb27cc118b5fd7.

It turns out multiple places in SerialContainer's cost inference
expect 0.0 as the neutral element (which is why std::min and multiply did not work).
While these additional issues can be fixed, it would make the interface much less elegant.

We should consider adding it back if an actual use-case is there to discuss.
2020-09-21 22:52:19 +02:00
v4hn
cce7380f9d Clearance: provide better interface for different modes
Interface::Direction should always designate a valid direction.
2020-09-21 22:52:19 +02:00
v4hn
f63d5b6892 Make transform in cost::Clearance configurable
In theory this can be done with a PassThrough with a modifying transform,
but this specific mapping is intrinsic to the definition of Clearance as a cost.
2020-09-21 22:52:19 +02:00
v4hn
cdc7cb04fc refactor Forward -> PassThrough
based on review feedback
2020-09-21 22:52:19 +02:00
v4hn
05932ad5f1 enable implicit construction of CostTerm from lambdas
without the need for explicit casts to the correct signature.
2020-09-21 22:52:19 +02:00
v4hn
034a155315 provide CostTerm class for all CostTerms
This pattern allows cost::Constant to override the hierarchical cost computation
for the SerialContainer and avoid traversing the graph.

I implemented the CostTerm::supports() pattern over a full double visitor pattern
with overloads for each SolutionBase specialization, because the SerialContainer
needs to know whether cost aggregation of the subsolutions should take place or whether
the SolutionSequence should be forwarded to the CostTerm.
This would not be possible with a `virtual double operator()(const SolutionSequence&)`
callback in CostTerm.

Alternatively, implementing the hierarchical aggregation in the default implementation
of this operator would be possible as well, but breaks intuition:
- the corresponding methods to handle `SubTrajectory` and `WrappedSolution` *have to*
  default to not touching the solution's cost at all so it is inappropriate to have
  the default implementation for the Sequence do something else
- The SerialContainer also aggregates costs outside the `computeCost()` interface
  (in multiple places in `onNewSolution()` to aggregate costs along partial paths)
  and thus moving the hierarchical aggregation to the CostTerm methods requires
  the aggregator to be shared between the Container and the CostTerm,

The only shortcoming of the implemented approach, by contrast, is that user implementations
that want to handle WrappedSolution or SolutionSequence differently have to ensure
the supports_ flags are set correctly. Notice that most custom CostTerms will only
ever access SubTrajectories and this case is simplified with the provided CostTerm constructors.
2020-09-21 22:50:51 +02:00
v4hn
8ae1060b4d add CostAggregator for SerialContainer
It can be useful to change the default addition to other operators.

The simplest example is applying a Constant cost term to a container.
As the tests show, the visitor-based cost computation ends up adding
cost::Constant *for each subtrajectory*.
2020-09-21 22:50:51 +02:00
v4hn
1689f6cd95 introduce new trivial stage Forward
This can be used together with a custom CostTerm to modify costs of a solution.
2020-09-21 22:31:52 +02:00
v4hn
0e38730fd4 compute cost in liftSolution as well 2020-09-21 22:31:52 +02:00
v4hn
a0975e3afc avoid ambiguity of setCostTerm(nullptr) 2020-09-21 22:31:52 +02:00
v4hn
d68ab383b6 allow setCostTerm for containers
implement visitor pattern for cost computation on solutions

- compute cost terms for solution subtrees instead of only for SubTrajectory

- allows users to set cost terms for containers
2020-09-21 22:31:24 +02:00
v4hn
039b0a2c27 move more overhead to computeCost 2020-09-21 22:31:23 +02:00
v4hn
e53d943b75 remove CostTransform
If required, this can hopefully be implemented through Wrapper stages soon.
2020-09-21 22:31:23 +02:00
v4hn
53c0964618 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.
2020-09-21 22:31:23 +02:00
v4hn
bbf7d415f6 make cost operators const 2020-09-21 22:28:36 +02:00
v4hn
1f8feaaf0a refactor addCost->computeCost 2020-09-21 22:26:25 +02:00
v4hn
d2bd0a0037 generalize Clearance cost
- can now be used to estimate cost for either interface state or the trajectory
- Introduced Interface::Direction NONE as a way of pointing to the trajectory in contrast to START or END
2020-09-21 22:26:25 +02:00
v4hn
7a00ac646e add missing license 2020-09-21 22:24:44 +02:00
v4hn
14c885621f remove Cost suffix for elements in mtc::cost:: 2020-09-21 22:24:44 +02:00
v4hn
9a96a7e4f6 implement LinkMotionCost 2020-09-21 22:24:44 +02:00
v4hn
6dbc742f20 implement correct PathLengthCost
The previous implementation depends on the dynamics limits of the robot,
which might be interesting in some cases, but shouldn't be a default anywhere.
2020-09-21 22:24:44 +02:00
v4hn
a92753dafc add cumulative distance to ClearanceCost 2020-09-21 22:24:44 +02:00