Commit Graph

89 Commits

Author SHA1 Message Date
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