Commit Graph

384 Commits

Author SHA1 Message Date
Robert Haschke
e894d8bce2 fix caching of PlanningPipeline ptrs
Need to reset cache if corresponding RobotModel was destroyed.
To this end, we cannot simply use the RobotModel's name.
2018-10-20 21:50:05 +02:00
Robert Haschke
f548b7edcc PipelinePlanner: disable publishing of plan request + computed path 2018-10-20 12:21:18 +02:00
Robert Haschke
36fe4c98be reduce copying of shared ptrs 2018-10-20 12:21:18 +02:00
Robert Haschke
d648a4091e guard use of introspection_ pointer 2018-10-20 02:38:11 +02:00
Robert Haschke
af6f7b12bf pick_pa10: fix initialization of RobotState
only a subset of joints was initialized
2018-10-20 02:38:11 +02:00
Robert Haschke
e9b16ba7c0 fix destruction order 2018-10-20 02:38:11 +02:00
v4hn
1a4f6c0e57 keep RobotModelLoader around
Otherwise the robot_model_ does not remain valid
2018-10-17 01:55:28 +02:00
Robert Haschke
a745f55ab7 fix compiler warnings in release mode 2018-10-17 01:55:28 +02:00
Robert Haschke
1787a66d37 fix compiler warnings 2018-10-16 23:14:58 +02:00
Robert Haschke
f1764d0de4 fix tests 2018-10-16 23:14:58 +02:00
Robert Haschke
940543a063 fixup! allow preemption of Task::plan() 2018-10-16 23:14:58 +02:00
Robert Haschke
b5bc97b3eb Task::execute() 2018-09-25 22:17:42 +02:00
Robert Haschke
7bd9bcf972 allow preemption of Task::plan() 2018-09-25 22:16:45 +02:00
Robert Haschke
e5e291d86f FixCollisions stage 2018-09-25 22:16:45 +02:00
Robert Haschke
a7b74673ce PlannerInterface for joint-space interpolation 2018-09-25 22:16:45 +02:00
Robert Haschke
4860462076 ComputeIK: forward child solution's comment 2018-09-25 16:25:22 +02:00
Robert Haschke
c70db65578 fixup! MoveTo: store goal as any type 2018-09-25 16:25:13 +02:00
Robert Haschke
ca9d50e7d8 fix compiler warnings 2018-09-25 13:30:53 +02:00
Robert Haschke
f2573e9bfd fix include order
Local headers should be preferred over those from underlay.
Consequently use target_include_directories() to properly define include order.
2018-09-25 13:30:53 +02:00
Robert Haschke
ff2474f262 pa10 doesn't need move_group 2018-06-03 12:36:12 +02:00
Robert Haschke
932ab5eba3 cast demos as integration unit tests
- moved demos from demo to test folder
- run them as unittest, checking range of solutions
2018-06-03 12:36:01 +02:00
Robert Haschke
3215880b98 Merge branches 'move-to', 'simple-grasp' and 'properties' into master
These branches only work together:
- MoveTo / MoveRelative: common handling of "goal" property
- properties: required changes to allow for multiple inheritance
- generalize SimpleGrasp / GenerateGraspPose
2018-06-03 11:38:50 +02:00
Robert Haschke
2e46082521 remove Container::exposePropertiesOfChild() 2018-06-03 11:36:50 +02:00
Robert Haschke
157caac385 generalize SimpleGrasp / GenerateGraspPose
- move "pregrasp", "grasp" property from SimpleGrasp to GenerateGraspPose
- Container::exposePropertiesOfChild: decouple exposure from inheritance
2018-06-03 11:36:50 +02:00
Robert Haschke
544f574166 MoveRelative: store goal as any type 2018-06-03 09:22:06 +02:00
Robert Haschke
00260d62b5 MoveTo: store goal as any type 2018-06-03 09:22:06 +02:00
Robert Haschke
941dee7fc1 exposeTo: const method 2018-06-03 09:22:06 +02:00
Robert Haschke
abc4c5de50 PropertyMap: allow any type 2018-06-03 09:18:37 +02:00
Robert Haschke
a8431ce661 Container::exposePropertiesOfChild: allow skipping of undefined props 2018-06-03 06:46:12 +02:00
Robert Haschke
0dab499533 Stage: generally allow forwarding of interface properties 2018-06-03 06:46:12 +02:00
Robert Haschke
92296e020f SimpleGrasp(GraspGenerator)
allow any GraspGenerator stage that provides "pregrasp" and "grasp"
postures as well as a target_pose for grasping.
2018-06-03 06:46:12 +02:00
Robert Haschke
beffe27987 improve property debugging 2018-06-03 06:42:11 +02:00
Robert Haschke
cbb2cd69f7 property inheritance: both from PARENT and INTERFACE
- source_id -> source_flags: bits indicating configured paths
- initializers, e.g. fromName(), should throw
- ignore undeclared errors during inheritance
- on undefined error, reset the value to None
- override value only if previously set by lower-priority source
  MANUAL > CURRENT > PARENT > INTERFACE
2018-06-03 06:36:15 +02:00
Robert Haschke
abea2ee82b reuse boost::any's type_index type 2018-06-02 17:46:37 +02:00
Robert Haschke
2e9932cfbc fix: don't report config issues as successful (but empty) solutions 2018-06-02 17:46:28 +02:00
v4hn
381210f43d compatibility with boost 1.54
This is required for ROS indigo on ubuntu 14.04
2018-06-01 15:02:11 +02:00
Michael Görner
5739bf39d9 use correct frame in pick (#52) 2018-06-01 14:55:23 +02:00
v4hn
0091c76208 compute ik: ignore collisions skips early eef check 2018-06-01 11:34:07 +02:00
v4hn
4026661bfe Rewrite and relax approx comparisons
This is more readable.

Adjusting the threshold to 1e-4 is required to allow for tolerances
in potential sampling planner steps in between.
2018-06-01 11:32:29 +02:00
v4hn
bd1edcbde7 add max_solutions param to plan()
For big problems you just don't want *all* solutions
2018-06-01 11:13:44 +02:00
Robert Haschke
ac1ccba883 minor formatting changes (#51) 2018-05-31 18:01:30 +02:00
Robert Haschke
b35aba6e19 validate merged trajectories 2018-05-30 21:23:34 +02:00
Robert Haschke
941e9df737 declare properties "timeout", "marker_ns" for all stages 2018-05-30 21:21:14 +02:00
Robert Haschke
c2dd28abae rework storing of solutions
- solutions_, failures_ as SolutionBaseConstPtrs in StagePrivate
- replace processSolutions() / processFailures() by direct const-access to storage containers
- generic sendForward(), sendBackward(), spawn(), connect() methods in StagePrivate
- reuse StagePrivate's sendForward(), sendBackward(), spawn() in containers

- store created InterfaceStates in StagePrivate::states_
- Interface: ordered<InterfaceState*> (only store pointers)
  allows for common handling of states of valid and failure solutions

- remove additional state+solution storages
- containers: internal->external state mapping as InterfaceState* -> InterfaceState*
2018-05-30 21:02:50 +02:00
v4hn
ae6b86c7d7 remove void line
?
2018-05-30 21:02:19 +02:00
Robert Haschke
5878e8ab6b correctly reset FixedState 2018-05-30 21:02:03 +02:00
Robert Haschke
52fdf30433 simplify compute() API
- remove bool return value
- always create a solution trajectory, also in case of failures
- success/failure determined from solution.isFailure()

minor adjustments during cherry-pick
2018-05-30 21:01:34 +02:00
Robert Haschke
eb50aaed0e SolutionBase: rename 'name' to 'comment' 2018-05-30 20:25:42 +02:00
Robert Haschke
84dec07565 MoveTo/MoveRelative: generic IK target frame
...instead of simple link name
2018-05-30 20:25:07 +02:00
v4hn
495c80350b Merge branches 'joint_pose', 'move-relative-joints', 'basic-merger' and 'compute-ik' 2018-05-30 19:35:58 +02:00
v4hn
015b1466f4 MoveRel: handle unknown links with hard failure 2018-05-30 19:09:57 +02:00
v4hn
56c9128d99 MoveTo: cleaner strict handling of invalid properties 2018-05-30 18:53:39 +02:00
v4hn
b8e6e5e2fa MoveTo: reduce scope of try-catch 2018-05-30 18:44:26 +02:00
v4hn
bbd274da96 MoveTo: simplify getJointStateGoal 2018-05-30 18:42:31 +02:00
Robert Haschke
b7b7a916df basic merger functionality 2018-05-18 17:44:55 +02:00
Robert Haschke
5e8117720e ignore failures for further processing 2018-05-18 17:44:43 +02:00
Robert Haschke
abb68da083 forward properties 2018-05-18 17:44:43 +02:00
Robert Haschke
ce10d96c5c ComputeIK: report collision pairs 2018-05-18 17:44:43 +02:00
Robert Haschke
41e64ed7e1 remove std::map serialization 2018-05-18 17:44:30 +02:00
Robert Haschke
40ca35085a cleanup 2018-05-18 17:44:30 +02:00
llach
9a7aa4dad7 Pick: added setter for relative joint lift motion 2018-05-18 17:44:30 +02:00
llach
d0c8f7f819 MoveRelative: added relative joint space goals to 2018-05-18 17:44:30 +02:00
llach
0779c255df added stream serialization for std::map<std::string, T> 2018-05-18 17:44:30 +02:00
Robert Haschke
9e19f2187b moved implementations of property setters to header 2018-05-18 17:44:17 +02:00
Robert Haschke
17e487be06 cleanup
- indentation: space -> tabs
- only consider joints of JMG
2018-05-18 17:44:17 +02:00
llach
2be3ee611b adapted MoveTo api change. joint_pose -> named_joint_pose 2018-05-18 17:44:17 +02:00
llach
55470062c9 MoveTo: converting named joint poses to robot state msg in init; property names refactoring 2018-05-18 17:44:17 +02:00
llach
97c2312d67 MoveTo: can now take RobotState msg as goal 2018-05-18 17:44:17 +02:00
Robert Haschke
19fa7349d3 Task: only accept containers 2018-05-18 17:42:34 +02:00
Robert Haschke
e3ca832497 Property: silently return empty string when serialization is not supported 2018-05-18 17:42:34 +02:00
Robert Haschke
2106c51c9d Property: provide a fallback serialize() implementation
... in case operator<< is not defined for type T
2018-04-28 22:13:52 +02:00
Robert Haschke
ae3b9126bd get rid off empty JointState errors 2018-04-27 12:07:27 +02:00
Robert Haschke
ecf8029f75 add timing to merged trajectories 2018-04-27 12:06:59 +02:00
Robert Haschke
872e3cc004 unittest for Priority comparisons 2018-04-27 12:04:55 +02:00
Robert Haschke
8fb47e99e8 update ordering on any change of priority 2018-04-27 12:04:55 +02:00
Robert Haschke
67b6dcb49b move implementation into cpp file 2018-04-27 12:04:55 +02:00
v4hn
bbb64f2f8c inf cost states always go last
Also update sorted interface when state becomes inf or get's new cost.
2018-04-27 12:04:55 +02:00
Robert Haschke
728c1b40e2 Merge branches 'travis', 'fixes', 'visualization' and 'todos' 2018-04-15 12:01:19 +02:00
Robert Haschke
b8a4653f31 relax equality condition for joint values for inactive groups 2018-04-15 11:55:45 +02:00
Robert Haschke
5c525f8729 consider v4hn's comment 2018-04-15 11:55:45 +02:00
Robert Haschke
433db67cf9 only attempt to merge if there is something to merge 2018-04-15 11:55:45 +02:00
Robert Haschke
90b0b006eb more todo 2018-04-15 11:26:00 +02:00
Robert Haschke
6d6f185870 feedback 2018-04-15 11:26:00 +02:00
v4hn
ade42456b8 list of random todos 2018-04-15 11:26:00 +02:00
Robert Haschke
51e1dda6ec minor improvements: variable names, comments 2018-04-10 01:52:40 +02:00
Robert Haschke
564f3b0514 fix MoveRelative
Ignore success of planner_->plan() when min_distance is specified (and >= 0).
In this case, compute the achieved distance myself.
2018-04-10 01:52:40 +02:00
Robert Haschke
525ac10bbf std::string -> const std::string& 2018-04-10 01:52:40 +02:00
Robert Haschke
f2c0c7bf90 rename enableCollision -> allowCollision 2018-04-10 01:52:40 +02:00
Robert Haschke
fee7352961 renamed demo: plan_pick_trixi -> plan_pick_pr2 2018-04-10 01:52:40 +02:00
Robert Haschke
a044b719ac removed old stages: Move, Gripper, CartesianPositionMotion 2018-04-10 01:52:40 +02:00
Robert Haschke
b22217deab fix trajectory merging: initialize from well-defined RobotState 2018-04-10 01:52:40 +02:00
Robert Haschke
c553275276 SolutionSequence::fillMessage: ignore sub solutions with same creator as parent 2018-04-10 01:52:40 +02:00
Robert Haschke
556d9e1a5e Merge branches 'robot-model', 'task-move-constructor' and 'const-robot-trajectory' 2018-04-09 21:11:19 +02:00
Robert Haschke
2b79f15244 SubTrajectory: promise to not modify encapsulated RobotTrajectory 2018-04-09 21:10:18 +02:00
Robert Haschke
7463621f56 move assignment operator 2018-04-09 21:10:08 +02:00
Robert Haschke
7ebc4b2c7e Task: move constructor 2018-04-09 21:10:08 +02:00
Robert Haschke
35b35cc17e enable introspection only if ROS was initialized 2018-04-09 21:09:49 +02:00
Robert Haschke
bd60e6f65d Task::setRobotModel() / Task::loadRobotModel() 2018-04-09 21:09:49 +02:00
Robert Haschke
70065c98e7 Merge branches 'master' 'generate-pose' and 'connect' 2018-04-09 21:08:45 +02:00
Robert Haschke
21242ea976 publish failures 2018-04-09 21:08:17 +02:00
Robert Haschke
047a3f7db4 report duplicate joints, accept fixed joints as duplicates 2018-04-09 21:08:17 +02:00
Robert Haschke
55e3aae30a merge trajectories 2018-04-09 21:08:17 +02:00
Robert Haschke
03092e7e45 utility functions to merge multiple RobotTrajectories 2018-04-08 02:36:55 +02:00
Robert Haschke
2c08d9080d Pick, Place as specializations of PickPlaceBase 2018-04-08 02:36:55 +02:00
Robert Haschke
ce5d7c63f0 SimpleGrasp, SimpleUnGrasp as specializations of SimpleGraspBase 2018-04-08 02:36:55 +02:00
Robert Haschke
97d2eb5c6a Pick: remove Connect 2018-04-08 02:36:55 +02:00
Robert Haschke
85e88eabd1 Connecting::compatible() to check compatibility of states 2018-04-08 02:36:55 +02:00
Robert Haschke
eee53a0d80 MoveTo: use moving frame markers 2018-04-05 22:54:51 +02:00
v4hn
1424b51f79 expose typed setters in Cartesian solver 2018-04-05 22:53:24 +02:00
Robert Haschke
77442c0eed Connect: allow multiple groups to be processed in series 2018-04-05 13:51:50 +02:00
Robert Haschke
8811045f89 setCreator() once in StagePrivate::newSolution() 2018-04-05 13:51:50 +02:00
Robert Haschke
9ceecd235f SerialSolution -> SolutionSequence 2018-04-05 13:51:50 +02:00
Robert Haschke
4d77390d77 validate existence of object frame in init() 2018-04-05 13:50:09 +02:00
Robert Haschke
9e1c3059e1 derive GenerateGraspPose from GeneratePose 2018-04-05 13:50:09 +02:00
Robert Haschke
3640928a76 postpone pose transformation 2018-04-05 13:50:09 +02:00
Robert Haschke
603696f294 add missing reset() 2018-04-05 13:50:09 +02:00
v4hn
76c6ba14fa add a simple GeneratePose stage
ComputeIK is a wrapper, so we can't just give it a pose to compute.
2018-04-05 13:50:09 +02:00
llach
a82a48bb70 fix PropertyMap::exposeTo: use other_name as new name for property (#40) 2018-04-05 13:47:23 +02:00
llach
124979e2a8 MoveRelative: adapt msgs to fit class name (#39) 2018-04-05 13:46:13 +02:00
Robert Haschke
205beea503
Merge pull request #28 from ros-planning/pr-path-constraints
implement path constraints
2018-03-26 09:54:02 +02:00
Robert Haschke
2ca68e6645 actually implement path constraints for CartesianPath planner
... validating constraints
2018-03-26 09:50:17 +02:00
v4hn
d7d80c3499 implement path constraints for Move* 2018-03-26 09:50:17 +02:00
v4hn
80ae01dcca expose timeout property as typed setter 2018-03-26 09:50:17 +02:00
v4hn
8af44cefc3 add path constraints to planner API
and use it in the Connect class.

The cartesian planner ignores the constraints for now.
2018-03-26 09:50:17 +02:00
Robert Haschke
f0dc02e2c3 ContainerBasePrivate::position() -> childByIndex() 2018-03-26 00:24:25 +02:00
Robert Haschke
84f5c481be ContainerBasePrivate::position(): generically handle shifting 2018-03-26 00:24:25 +02:00
Robert Haschke
ff0ee407ff remove tool_to_grasp_tf from SimpleGrasp 2018-03-25 06:07:53 +02:00
Robert Haschke
54e4d8eed7 remove tool_to_grasp_tf from GenerateGraspPose 2018-03-25 06:07:53 +02:00
Robert Haschke
12e134ad09 added disclaimer 2018-03-25 06:07:53 +02:00
Robert Haschke
893cb25b5b fix isTargetPoseColliding
- must not use new sandbox_scene (we want to display the place eef)
- jmg not needed anymore
2018-03-25 06:07:53 +02:00
Robert Haschke
c98f10478e frame marker at ik frame 2018-03-25 06:07:53 +02:00
Robert Haschke
5b6a02d105 rename reference frame -> ik frame 2018-03-25 06:07:53 +02:00
Robert Haschke
f497112513 consider reference_frame 2018-03-25 06:04:09 +02:00
Robert Haschke
7531b8ad54 allow arbitrary reference frame for target_pose and ik frame 2018-03-25 06:04:09 +02:00
Robert Haschke
4fcac84d76 remove tests that do not test anything 2018-03-25 06:04:09 +02:00
Robert Haschke
9e5e098339 unittests
- provide simple hard-coded robot model
- test ComputeIK::init()
2018-03-25 06:04:09 +02:00
Robert Haschke
f4adaa949a validate available properties during init() 2018-03-25 06:04:09 +02:00
Robert Haschke
5a55a5949b Merge branches 'bug-fixes', 'gui' and 'pick-stage' 2018-03-24 07:32:20 +01:00
Robert Haschke
a4fed6a18a MoveTo Cartesian: create marker frame at current and goal pose 2018-03-24 06:16:10 +01:00
Robert Haschke
cb38a87a73 CartesianPath: return a partial trajectory in case of failure 2018-03-24 06:16:10 +01:00
Robert Haschke
81d88911a9 cosmetic fixes 2018-03-23 17:18:56 +01:00
Robert Haschke
2331c9aef2 pick: expose IK link frame as eef_frame 2018-03-23 17:18:56 +01:00
Robert Haschke
1c61fb3b85 expose solvers 2018-03-23 17:18:56 +01:00
llach
ea6cc4b6bf moved "attach object" from "pick" to "grasp" stage 2018-03-23 17:18:56 +01:00
Robert Haschke
ceaf896bf8 position scene node w.r.t. fixed frame 2018-03-23 11:22:23 +01:00
Robert Haschke
c2590636b1 fixup! SerialContainer: don't consider failures for solutions 2018-03-23 11:22:23 +01:00
Robert Haschke
862ac37cd6 fix ContainerBasePrivate::copyState(): don't copy (again) on update 2018-03-23 11:22:23 +01:00
Robert Haschke
b3af170663 reset num_failures_ 2018-03-23 11:22:23 +01:00
Robert Haschke
6b912c3531 MoveTo Cartesian Point: bug fix 2018-03-23 11:22:23 +01:00
Robert Haschke
6d859840c3 fix interface detection for nested SerialContainers 2018-03-23 09:18:21 +01:00