Further factorize and simplify FallbacksPrivate classes employing ideas from @v4hn.
The key difference between the variants his how they advance to the next job.
Thus, the only virtual method required is nextJob().
- Enable moving/swapping of other container impls (e.g. Fallbacks)
- Clarify (via move semantics) that content of source impl will be lost
- Get rid of friend declarations
Adapt test results FallbacksFixturePropagate.computeFirstSuccessfulStagePerSolutionOnly
due to 2e63c154aa:
The order of computations has changed, because we lock the processed state
as soon as it is forwarded to the first fallback child.
In this case, after processing GEN1 und FWD1 once, we have the two states with costs 2, 4 in the queue.
The first one, i.e. with cost 2 is forwarded to the child FWD2, which fails.
In the next cycle, although we have new states in the queue (1, 2, 3, 4), we stick with state "2"
and forward it two FWD3, which adds costs 210, resulting in 212.
With previous code, the Fallback container switched to state "1", forwarded to FWD2.
- Drop variable current_external_state_
- Instead encode the info that the external state wasn't yet forwarded to any child via stage = children().cend()
- If all children have exhausted their solutions for this state, it is removed from the pending list
Not only propagate updates along solution paths, but also bridge
the gap of a `Connecting` stage.
- If a state becomes enabled, re-enable opposite `ARMED` states as well.
- If a state becomes pruned, also prune opposite states if they don't have alternatives.
- Make sure that we don't run into a recursive update loop by disabling notify() callbacks.
This also requires to drop the assertion in SerialContainer::onNewSolution()
that new solutions will have enabled start+end states (a CONNECT stage's solution might not).
- Switch directions: FORWARD <-> BACKWARD to make the function reusable for status propagation.
- We need to ignore the source state when looking for opposite states of the target state.
Thus add both, source and target state arguments.
- Centrally distinguish between have owner() or not in InterfaceState::updatePriority()
- Have a separate updateStatus() method to just update the pruning status
- Split Interface::updatePriority() into a method taking the InterfaceState*
and one taking an Interface::iterator (for efficiency)
- Early return in container.cpp's updateStatePrios()
Setting up a demo for
Fallbacks({CartesianPath,PTP,RRTConnect})
I found the logic did not work as expected yet.
- process last job spec as well
- ignore failures when looking for a solution
- add more debug output
Note that while this ensures other stages outside the Fallbacks container
can compute as well, it does not solve the problem internally.
A new incoming state will only ever be considered once
the current stage cannot compute any more.
We have no way of telling a child to compute for *a specific state* for now.
So once we copied a state to its interface we have to let it compute until
all possibilities are exhausted to detect whether or not it could generate a solution for it.
If we wouldn't do so, there were no way of knowing when to fall back
to the next child as long as the stage can still compute on *any* copied solution.
Keep the previous logic around for Generator stages.
Note that this only makes sense for *pure* Generators and not for MonitoringGenerator,
because for the latter we would expect monitored solutions to be passed individually
(similar to pruning).
never unload the plugin loader before the plugins (IK plugins here).
We don't have unrelated loaders in gtest executables, so the static should be fine.
yes, most pruning happen along children of a serial container,
but children for many tests comprise a lot of other containers as well.
- migrated pruning tests from Connect to ConnectMockup (as the concrete implementation
is not relevant for them)
- added missing header to stage_mockups.h
This could have been done already back when `runCompute` was introduced.
Wrapping the calls in try/catch comes from the previous implementation directly
calling `compute()`.
The current implementation will not fall back for each state
independently, but is meant to stay with the first child producing
a solution. For propagators, this is problematic though
as the picked child depends on the (arbitrary) first received state.
Instead, fallbacks should pass each state to each child separately
until one produces a solution for it (or all are exhausted).
... in favor of checking version numbers.
Checking for one header was used for multiple independent things.
In theory we could do exact feature testing instead of using the next release number,
but in practice nobody cares about the individual commits between older releases.
If MoveIt and MTC use incompatible versions of pybind11, the tests
will fail because MoveIt objects like RobotModel or PlanningScene
cannot be passed to MTC objects and vice versa.
Using template names T is not a good idea, because this name is used
verbatim for some error reporting, resulting e.g. in:
Tried to call pure virtual function "T::canCompute"
Implement visualization as red-green arrow
* overload makeArrow to allow creation with points
* create new function for visualization
* if no plan is found, construct arrow from green cylinder and red arrow
* adjust arrow construction for backward propagators
* Install module libs into CATKIN_GLOBAL_PYTHON_DESTINATION (assuming unique names).
This avoids the need to link them into the source space, because they are found also from devel space.
* Use pybind11's def_submodule() to create the `core` and `stages` submodules,
everything linked into the same lib
Get hashing for inverted lookups, but incur
structural overhead.
Whether this is worth it depends on the number of mapped interface states
and the number of pruning/reactivation requests.
Start and End are already used for an entirely different concept,
so if anyone ever wants to read this code, we should use new terms instead.
Because the source state is the disabled state that *failed* to extend,
triggering the whole subtree to be disabled, I went for the new terms
DISABLED and DISABLED_FAILED.
The key to pruning in the Connecting stage was the following:
- Don't remove states during pruning, but only disable them.
They might become re-enabled due to further input.
- Distinguish START and END sides of a disabled solution tree to break their symmetry.
The START side from where we started disabling, can be re-enabled by a new partner state in
Connecting, the END side must not. This was important as, otherwise, the states would simply
get re-enabled immediately. The END side only gets re-enabled if the START side actually
connects the whole solution branch.
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().
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.
This is somewhat cumbersome because of the additional internal/external
layer introduced through the container.
But it's still better than leaving this unverified.
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()
* unify usage of pimpl()
* fix StatePair constructors
* improve/add comments
* test_container: reset MOCK_ID for each test to facilitate identification of stages
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.
Adding a further overload of the `setGoal()` function that is exposed to
the python api. This should provide an interface for passing in dicts as
joint name and -angle configurations.
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; } } )`
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.
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.
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.
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*.
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
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.
- 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
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.