Robert Haschke
cc8bcd3fdc
become agnostic to urdfdom's shared_ptr types
2018-02-24 13:42:12 +01:00
Robert Haschke
07e99461fc
conditionally normalize angular
2018-02-24 13:42:12 +01:00
Robert Haschke
cf54024379
fix derivation of propagation direction from connect stage
2018-02-24 13:42:12 +01:00
Robert Haschke
aee2525382
fix ModifyPlanningScene: pass stage properties to callback
2018-02-24 13:42:12 +01:00
Robert Haschke
b767e359e9
GenerateGraspPose: require pregrasp pose
2018-02-24 13:42:12 +01:00
Robert Haschke
6a3456f961
unittest ContainerBasePrivate::position()
2018-02-24 13:42:12 +01:00
Robert Haschke
9cd6efe46f
Stage::init(PlanningScene) -> Stage::init(RobotModel)
2018-02-24 13:37:50 +01:00
v4hn
2a2d406368
remove restriction to watch only generators
2018-02-24 13:37:50 +01:00
Robert Haschke
d035089c10
MonitoringGenerator
2018-02-24 13:37:50 +01:00
Robert Haschke
8624224d6b
FixedState generator
2018-02-24 13:37:41 +01:00
llach
e1281130ac
CurrentState: fetch scene via get_planning_scene service
2018-02-24 13:37:41 +01:00
v4hn
4f2b38f02d
add missing headers
2018-02-21 14:56:51 +01:00
Robert Haschke
cf76352d2d
Merge branches 'fix-containers' and 'fix-priority-updates'
2018-02-20 08:07:48 +01:00
Robert Haschke
ba75f7c835
fixed remaining unittest
2018-02-20 08:06:23 +01:00
Robert Haschke
e368fd9948
avoid accidental overwrite of InterfaceState
2018-02-19 09:05:56 +01:00
Robert Haschke
373d7f204f
update priorities of all interface states along a (partial) solution path
...
if a parallel container is involved somewhere in the middle, it will
again access these states, e.g. planning alternative solutions
2018-02-19 09:05:56 +01:00
Robert Haschke
1eacea6b35
reset InterfaceState::owner_ if state is removed from Interface
2018-02-19 09:05:56 +01:00
Robert Haschke
35f1540369
recursive interface auto-detection
...
trigger auto-detection from top (task) level:
only there we now for sure the accepted interface
2018-02-19 01:05:56 +01:00
Robert Haschke
f45a4e3e2c
improve validation
...
- postpone pruning / interface auto-detection to top task-level
only the task state knows for sure, that it requires its wrapped child to push to both ends
- perform connectivity validation only after pruning
only then, we the interfaces are completely determined
2018-02-19 01:05:56 +01:00
Robert Haschke
a523333ef6
basic auto-detection of interfaces for propagating stages
...
works if propagation direction can be derived from a generator or
connecting stage within the sequence
start-to-end propagation through whole serial container still fails
2018-02-18 21:23:12 +01:00
Robert Haschke
1a0b9b36ee
fix connection creation
...
Establishing the interface connections, we face a chicken-egg-problem:
To establish a connection, a predecessors/successors pull interface is
assigned to the current's stage push interface.
However, propagating stages (in auto-detection mode) can only create
their pull interfaces if the corresponding, opposite-side push interface
is present already (because that's the mechanism to determine the supported
propagation directions).
Hence, we need to resolve this by performing two sweeps:
- initialization, assuming both propagation directions should be supported,
thus generating both pull interfaces, i.e. providing the egg
- stripping down the interfaces to the actual context
This context is provided by two stages pushing from both ends
into a (potentially long) sequence of propagating stages (tbd).
Contributions of this PR:
- PropagatingEitherWay: explicitly distinguish AUTO from BOTHWAYS interface
AUTO: auto-derive interface from provided push interfaces
BOTHWAYS: explicitly require both directions
- SerialContainer: (better, but not yet perfect) validation of connectivity
- ParallelContainer: determine interface from what children offer
2018-02-18 21:23:12 +01:00
Robert Haschke
ec2b06292e
extended unittest for serial connection validation
2018-02-18 14:03:57 +01:00
Robert Haschke
a4feb705d0
cleanup operator<<(ostream, *)
...
- basic implementation for StagePrivate
- implementation for Stage calls this
- implementation for ContainerBase recursively calls this
- implementation for Task added
2018-02-18 13:58:09 +01:00
Robert Haschke
1f4264c8c8
PropagatingEitherWayPrivate: count failures in both directions
2018-02-16 16:01:05 +01:00
Robert Haschke
2ea463fff4
replace std::cout with ros console
2018-02-16 16:01:05 +01:00
Robert Haschke
acffd39091
SerialContainer: fix priority propagation
...
- traverse all (also partial) solution paths
- and update priority at both ends
- remove pending state pairs if cost increased to infinity
2018-02-16 16:01:05 +01:00
Robert Haschke
469c6ccede
MoveRelative: fix marker arrow
2018-02-16 11:37:57 +01:00
Robert Haschke
dd0d004052
cleanup ComputeIK
...
- correctly check collision for target pose before doing IK
- visualize failed collision check / failed IK
2018-02-16 09:55:34 +01:00
Robert Haschke
deb09b7dfd
GenerateGraspPose: marker cleanup
...
- remove arrow marker (approach direction isn't hard-coded anymore)
- consider all rigidly attached parent links for display
- add grasp frame
2018-02-14 17:50:02 +01:00
Robert Haschke
a8cf2dc844
GenerateGraspPose: correctly check for existence of link frames
2018-02-13 19:29:12 +01:00
Robert Haschke
7e66c24822
marker tools: allow vector of LinkModel*
2018-02-13 19:29:12 +01:00
Robert Haschke
b1fac6eed0
MoveRelative: allow zero min_distance
2018-02-13 19:29:12 +01:00
Robert Haschke
0b8bf2b8b7
example: twist motion
2018-02-13 00:01:01 +01:00
Robert Haschke
ca3bce4669
local planning scene
2018-02-12 23:58:49 +01:00
Robert Haschke
da9711a85c
example using new features
2018-02-12 23:56:10 +01:00
Robert Haschke
d053e037eb
cmake cleanup: group source files
2018-02-12 23:55:34 +01:00
Robert Haschke
4be373592a
Merge branches 'cleanup', 'wip-modular-planning', 'wip-containers', 'wip-gui' and 'wip-modify-ps'
2018-02-12 23:54:51 +01:00
Robert Haschke
724f0c3014
modular planning
...
Separate planning approaches (using MoveIt pipeline or computeCartesianPath) from stages.
This allows to reuse planning in various stages without code duplication.
Reworked stages:
Move -> Connect
Gripper + CartesianPositionMotion -> MoveTo
CartesianPositionMotion -> MoveRelative
2018-02-12 23:43:47 +01:00
Robert Haschke
0d338a7f5f
visualize collisions
2018-02-12 23:40:33 +01:00
Robert Haschke
f35a6bc05d
FixCollisionObjects stage
2018-02-12 23:40:33 +01:00
Robert Haschke
f006f0f3d6
reduce code bloat using SFINAE template selection
2018-02-12 23:40:33 +01:00
Robert Haschke
381edf2d22
ModifyPlanningScene stage
...
- attach / detach objects to robot
- enable / disable collision pairs
- works in either direction (FORWARD + BACKWARD)
2018-02-12 23:40:33 +01:00
Robert Haschke
449ccf55c6
merge WrapperBase + Wrapper into WrapperBase
...
Wrapper is not restricted to generator-type stage anymore.
2018-02-12 23:39:46 +01:00
Robert Haschke
032a4e23c1
derive WrapperBase from ParallelContainerBase
2018-02-12 23:39:46 +01:00
Robert Haschke
1ab2d596fb
ContainerBasePrivate::liftSolution()
2018-02-12 23:39:46 +01:00
Robert Haschke
919be24d16
implement ParallelContainerBase
2018-02-12 23:39:46 +01:00
Robert Haschke
a3a8f1a9d2
improved directional (forward/backward) access to interfaces
...
- TraverseDirection -> Interface::Direction
- StagePrivate: pullInterface(), pushInterface()
- trajectories<dir>(SolutionBase) -> SolutionBase::trajectories<dir>()
2018-02-12 23:39:46 +01:00
Robert Haschke
73f509c3ba
simplify usage of ContainerBasePrivate::copyState()
2018-02-12 23:39:46 +01:00
Robert Haschke
eaaaf07214
exploit default argument for Interface constructor
2018-02-12 23:39:46 +01:00
Robert Haschke
cf71d2f0d2
unify use of buffer interfaces in containers
...
All containers need to buffer their children's sendBackward/sendForward states.
2018-02-12 23:39:46 +01:00
Robert Haschke
9bdfcc8c23
expose Task::initScene()
...
... to allow use of local scene
2018-02-12 23:37:19 +01:00
Robert Haschke
e9d0a05f75
default arg in header
2018-02-12 23:37:19 +01:00
Robert Haschke
3a4dc6755b
Merge branches 'fix-remote-task-model', 'wip-cost-ordering' and 'wip-properties'
2018-02-12 23:35:01 +01:00
Robert Haschke
fe2c4f312e
serialize stage properties
2018-02-12 22:45:15 +01:00
Robert Haschke
5ff78f653e
signal callback function
...
... to allow for syncing with rviz::Property
2018-02-12 22:45:15 +01:00
Robert Haschke
c3eead0115
allow only a single inititialization source
2018-02-12 22:45:15 +01:00
Robert Haschke
5c43d700a8
update state priorities in a container-specific fashion
...
Reorder interface list on priority updates.
This requires the InterfaceState to store a pointer to the owning Interface.
2018-02-12 22:44:18 +01:00
Robert Haschke
facd6438c9
Interface: order states by priority
...
Priority is (depth of solutions, accumulated cost along trace).
2018-02-12 22:44:18 +01:00
Robert Haschke
d0351698a6
single map internal_to_external
...
There is no need to distinguish between starts and ends when mapping
states, because start/end states need to be disjoint sets.
2018-02-12 22:44:18 +01:00
Robert Haschke
e6af069139
SerialContainer: traverse solutions w/o stopping stage
...
Always traverse from current solution to the start/end of a complete
path and only call the callback once for the whole trace.
2018-02-12 22:44:18 +01:00
Robert Haschke
6a62f6be76
default argument for NotifyFunction in Interface constructor
2018-02-12 22:44:18 +01:00
Robert Haschke
244a7d147b
sort serial solutions before insertion
2018-02-12 22:36:58 +01:00
Robert Haschke
29577515a0
cost ordering for solutions
2018-02-12 22:36:56 +01:00
Robert Haschke
c84fa4cdd6
cost ordered API
2018-02-12 22:36:12 +01:00
Robert Haschke
1403fc9830
fixed some clang warnings
2018-02-12 22:36:12 +01:00
Robert Haschke
2184450bd6
count all failures for statistics
2018-02-12 22:36:12 +01:00
Robert Haschke
7a13d79bc4
example setting solution markers
2018-02-12 22:36:12 +01:00
Robert Haschke
97bb6ef2c6
allow handling of failures
...
- store "failure" solutions to facilitate debugging
- Introspection assigns solution IDs as soon as they are created in a stage
Thus, solution IDs represent their creation order.
In contrast, the order of publishing (in StageStatistics) should
represent the cost order.
- Storing failures is disabled if Introspection is not available.
2018-02-12 22:35:58 +01:00
Robert Haschke
57cd11e3a9
Wrapper::compute: return true if new solutions are found
2018-02-12 22:33:39 +01:00
Robert Haschke
38f7696acf
ComputeBase::addTrajectory -> ComputeBasePrivate::addTrajectory
...
Forbid indirect access to trajectories_.
Only official compute classes have access.
2018-02-12 22:33:39 +01:00
Robert Haschke
5c1cad14df
Merge branches 'fixes', 'wip-drag-n-drop', 'wip-ikstage' and 'wip-marker-vis'
...
resolved conflicts:
core/include/moveit/task_constructor/container.h
core/src/container.cpp
core/src/stages/generate_grasp_pose.cpp
visualization/motion_planning_tasks/src/task_display.cpp
visualization/motion_planning_tasks/src/task_panel_p.h
2018-02-04 21:36:01 +01:00
Robert Haschke
17683b12c9
more exported stage plugins: CurrentState
2018-02-04 21:30:25 +01:00
Robert Haschke
736136dd9f
collision test for ee group
2018-02-04 09:01:45 +01:00
Robert Haschke
3fd096c8e6
fix IK stage: insert different solutions as different scenes
2018-02-04 09:01:45 +01:00
Robert Haschke
57d907908d
better names for frame and transform variables
2018-02-04 09:01:45 +01:00
Robert Haschke
fe05894709
renamed GenerateGraspPose::setGraspFrame() to setToolToGraspTF()
2018-02-04 09:01:45 +01:00
Robert Haschke
0918ad6897
implement IK as separate stage
...
... wrapping another stage
2018-02-04 09:01:45 +01:00
Robert Haschke
b6959170db
derive WrapperBase from ContainerBase
...
While ParallelContainers can directly promote their child solutions as
their own, a Wrapper needs to modify those solutions. Hence, Wrappers
should be derived directly from Container.
2018-02-04 09:01:45 +01:00
Robert Haschke
e4ad7a0753
removed Stage::validate()
...
... only was checking for implies(a, a) which is always true
2018-02-04 09:01:45 +01:00
Robert Haschke
104e52eb48
promise to not modify solutions
2018-02-04 09:01:45 +01:00
Robert Haschke
f8952bb712
introducing process_id
...
The task id was not unique enough to distinguish different tasks.
When a task publisher is killed and restarted, it usually comes up with
the same task id. However, visualization doesn't notice this change and
get confused / crashes when receiving task statistics and solutions.
2018-02-04 09:01:40 +01:00
Robert Haschke
01d43e3f3b
marker_tools
2018-02-04 09:00:54 +01:00
Robert Haschke
0e8dd85ae6
allow stages to access the created SubTrajectory
...
to add markers, set name, etc.
2018-02-04 09:00:54 +01:00
Robert Haschke
e9363919e8
Merge branch 'wip-properties'
2018-02-04 08:59:35 +01:00
Robert Haschke
41ec46964a
more unit tests
2018-02-04 08:56:11 +01:00
Robert Haschke
3ab899a9a8
cleanup error handling
...
throw std::logic_error on type errors
throw std::runtime_error on undeclared property
don't expose generic PropertyMap::declare()
2018-02-04 08:56:11 +01:00
Robert Haschke
c9e3be08d0
fix initialization order of properties
...
First from INTERFACE, second from PARENT.
INTERFACE initialization only makes sense for Propagating stages.
Connecting stages should ensure that interfaces define identical
properties which is not possible with boost::any.
2018-02-04 08:56:11 +01:00
Robert Haschke
4db7e8eb88
definition of PropertyInitializerSource moved to Stage
2018-02-04 08:56:11 +01:00
Robert Haschke
f6253c46a4
generalize initialization source from enum to int
2018-02-04 08:56:11 +01:00
Robert Haschke
59fe1e7860
separate setValue() and setCurrentValue()
...
setValue() also updates the default value.
reset() reset to the default value.
setCurrentValue() only updates the current value, keeping current default.
Thus setCurrentValue() can be reverted (to default) using reset().
2018-02-04 08:55:58 +01:00
Robert Haschke
d62b85572a
remove property name from InitializerFunction's signature
2018-02-03 21:45:40 +01:00
Robert Haschke
0d6dbee215
initFrom() -> configureInitFrom() + performInitFrom()
...
Use different function names for different semantics.
2018-02-03 21:45:40 +01:00
Robert Haschke
32a0de6bf3
countDefined(): count number of defined properties
2018-02-03 21:45:40 +01:00
Robert Haschke
f2b688d1f0
generalize GenerateGraspPose
...
replacing scalar graspOffset and hard-coded Euler angles with arbitrary graspFrame
2018-02-03 21:45:40 +01:00
Robert Haschke
587dcaebb2
ur5 example: use properties
2017-12-13 14:19:20 +01:00
Robert Haschke
7d8f8dfc8d
PropertyMap
2017-12-13 14:19:20 +01:00
v4hn
d7719e721f
add full license information
...
so dull... but for the matter of completeness
2017-12-12 21:24:32 +01:00
v4hn
9fca79e48b
reduce catkin_lint complains in core
2017-12-08 20:55:30 +01:00
Robert Haschke
0246d60de9
DisplaySolution: struct to unify all data corresponding to a sub trajectory
2017-11-30 18:15:47 +01:00
Robert Haschke
1bc485658e
Merge branch 'wip-visualization' into wip-refactor
2017-11-24 13:40:43 +01:00
Robert Haschke
b48ba91a75
cmake compatbility to 2.8.12
2017-11-24 13:37:50 +01:00
Robert Haschke
9032dabca5
navigate solutions
2017-11-24 11:24:18 +01:00
Robert Haschke
957b10a7a8
don't export SerialContainer as pluginlib class
...
... it's imported as builtin
2017-11-20 00:07:27 +01:00
Robert Haschke
e52dc8db11
Introspection: start solution id at index 1
...
... allowing 0 to have special meaning "invalid"
2017-11-20 00:04:57 +01:00
Robert Haschke
46af6a28da
Introspection: helper fillSolution() adding task_id
2017-11-20 00:04:35 +01:00
Robert Haschke
df1bbc5d32
travis config + fixes
2017-11-13 01:33:33 +01:00
Robert Haschke
e9ba2248f8
Merge remote-tracking branch 'origin/master' into wip-refactor
...
- cmake 3.1 required for CMAKE_CXX_STANDARD
- more old-style signal/slots for Qt4 compatibility
2017-11-13 00:07:23 +01:00
Robert Haschke
d0e3783d4e
removed statistics from task description
...
- make TaskStatistics a latched topic too
- subscribing to topics in order (1. description, 2. statistics, 3. solution)
should ensure that we receive those latched messages in this order
2017-11-12 21:42:48 +01:00
Robert Haschke
67b5a99d8d
receive task solutions
...
- remove parent_id from StageStatistics message
2017-11-12 21:42:48 +01:00
Robert Haschke
df074b48a5
cleanup TODO
2017-11-12 21:42:48 +01:00
Robert Haschke
9c5ddd3f6d
split repo into different ROS packages: msgs, core, visualization
2017-11-12 21:42:48 +01:00