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.
CostTerms only apply to primitive solutions and generalizing them
to Containers would make them quite unintuitive (and adds overhead).
Instead CostTransform can be used in any container to scale, crop, square
the cost of the solutions.
I initially thought about adding scaling factors, but then again,
other transforms are of interest just as well.
My previous patches accidentally disabled *all* (3) reset messages,
instead of keeping exactly one. This patch sends exactly one empty description
(reset) message every time an introspection instance is constructed for a task.
Notice the additional increase of the description queue_size from 1 to 2 to avoid
directly dropping the reset message in favor of the new description likely to be send
shortly afterwards.
Otherwise a new task will always setup the publisher,
even if introspection is disabled afterwards.
It is a good idea to keep introspection on, but there should be a way to initialize the C++ classes without ROS communication.
So far, the start_scene field of a SolutionMsg was only filled by Introspection::fillSolution(),
but not yet by Task::execute().
Addendum(v4hn): The previous approach was actually reasonable too (although the scene should have been marked as `is_diff`) for solutions sent for execution, but keeping the full start_scene around can facilitate debugging from recorded data.
Probably the most invasive format patch, also changing some internal API.
I deliberately disabled ClassCase and MethodCase checks for the moment
to avoid public API changes in this patch set.
Validate interfaces during resolution. No need to separately validate interfaces.
Thus, validateConnectivity() is removed from Task::init().
Functions are kept (but simplified) for unit testing.
* Do not modify explicitly configured direction of Propagator
* Avoid code duplication in pruneInterface() and validateConnectivity()
Only implement pruneInterface() for stages that actually need to adapt their interface.
* Enforce at compile time that either input or output flags are considered.
* Use horizontal flow symbols (as in console output).
* Replace redundant direction(flags).
* Reset mock_id for each test to facilitate assignment
* Simplify creation of nested containers
* Moved some test cases around
* Output all exception messages (even expected ones)
The PROPAGATE concept BOTH declared the stages *will* propagate solutions in
either direction. ANY, on the other hand, only means the propagation
direction is *not resolved yet* (but will be at planning time).
BOTH was originally described to support a more general control flow
than was eventually decided to support. The four exclusive Stage interfaces
CONNECT, PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS, and GENERATOR
do not allow for BOTH as a valid setup anymore, unless you setup a very
convolved task like `Alternatives(GEN, PROP) - Alternatives(PROP, GEN)`
which would be very complex to inspect. The same functionality can still
be achieved more readable as `Alternatives(Seq(GEN, PROP), Seq(PROP, GEN))`.
The confusion between BOTH (propagator *will* send in both directions) and
ANY (propagator will send in *either* direction, decided during init) led
to a lot of confusion with users and was not fully accounted
throughout the pipeline.
Adjust tests.
Notice the difference between ANY (unresolved propagator) and UNKNOWN
(a container before introspecting its children). propagators still
report UNKNOWN as requiredInterface though to simplify control flow.
The simplification enables a much simpler linear inference of the connective
structure of a task, as the first interface direction is always given.
Additionally, unify the resource setup for static interfaces to run
in the constructor, and for dynamic initialization in `pruneInterface`,
getting rid of partial initializations in `init`.
I very much considered just removing the protected inheritance again,
but it would add unnecessary code duplication.
Take note, the overriding `insert` function bypasses the Wrapper and directly forwards to the wrapped container.
This is somewhat dirty and could be an issue for anyone inheriting from `Task`.
`add` falls back to `insert` for both structures,
but `add` throws exceptions and does not provide a return value.
`insert` provides standard STL container access.
So far, returning false from the processor function, just skipped further traversing the current child (depth-wise).
Now, traversal is completely aborted, even not traversing the remaining siblings of the current child.
Having a single boolean return value, we cannot distinguish both cases.
We need the new behaviour for 8061945c15bea22e8f8899c987bc28e3542885aa.
Having multiple solutions, automatic publishing of intermediate solutions is confusing.
One never knows, which one is the final one. If desired, the user should setup a hook for this.
To move a task instance to another one, it's not sufficient to swap all task members,
but we also need to adapt all back pointers, i.e. me_ and parent_ pointers of children,
to point to the (swapped) task instances.
We need to distinguish two cases for how the interface of a nested serial container is determined:
1. from its children
2. from its (outer) context
As long as the interface is not fully resolved, requiredInterface() returns UNKNOWN.
After pruning, the first/last child's interface is remembered and reported instead.
If PropagatingEitherWay's interface is not met in *both* directions (but only one),
in BOTHWAY mode, issue a warning. Otherwise handle both, AUTO and BOTHWAY mode,
in the same fashion when resolving interfaces.
TODO: move validateConnectivity() in StagePrivate.
default action = default action from ContainerBase.
PropagatingEitherWay: issue warning for case above
- to allow solution wrappers (WrappedSolution, SolutionSequence)
to transmit their comment and markers as well
- introduced new SolutionInfo.msg,
which is the info common to solution wrappers and actual SubTrajectories
Reworked cost_queue to correctly sort pointer-like types.
Added unittests for new ValueOrPointeeLess<T> less operator, ordered<T>, and rviz cost ordering.
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.
Release mode builds previously produced broken solutions with too many entries,
debug build triggered the assert
container.cpp:334: assert(solution.empty())
The standard guarantees std::vector(&&a) leaves a.empty() == true,
so the logic there is fine as long as subsolutions is actually
used for move-construction.
"Goal" implies a motion to a target configuration.
MoveRelative explicitly does not do that.
"Direction" is usually not used for rotations,
but perfectly valid to describe them.
I merged 544f574166
together with the first version of this rename
that got force-pushed because it was incomplete.
- remove group + timeout properties: they are passed as arguments to plan()
- move max_velocity_scaling_factor, max_acceleration_scaling_factor to PlannerInterface base class