Addition of CHOMP planning adapter for optimizing result of other planners #1012

raghavendersahdev · 2018-08-01T15:20:54Z

Addition of CHOMP planning adapter to act as an additional optimizer for other planners like OMPL

This PR is a work done by Raghavender as a part of 2018 Google summer of code. As a part of this PR, CHOMP planning adapter works and can be used as an additional optimization technique in combination with other motion planners like OMPL.

With this PR, CHOMP can act as a post-processor for other planners. one simply needs to add a line in their ompl_planning_pipeline.yaml file if they want to use this. Just add this line in your planning_adapter= > default_planner_request_adapters/CHOMPOptimizerAdapter to add CHOMP a an additional post-processing optimizer.
Corresponding Tutorials PR available here!

Checklist

addition of CHOMP planning adapter in the planning_request_adapter folder.
added code in chomp planner folder. Addition of functionality in CHOMP to receive input paths obtained from OMPL and act as an optimizer.
updated planning_scene.h and planning_scene.cpp; Added code in the planning_scene folder to make certain variables mutable and addition of constant version of certain functions.

raghavendersahdev · 2018-08-01T15:22:12Z

Hi @davetcoleman @mamoll this PR is ready for review.

mamoll · 2018-08-02T01:15:27Z

moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h

@@ -129,6 +132,11 @@ class ChompTrajectory
   */
  void fillInCubicInterpolation();

+  /**
+   * \brief receives the path obtained from OMPL and puts it into the appropriate trajectory format required for CHOMP


Is MotionPlanDetailedResponse really OMPL-specific or can the trajectory be produced by other planners? If it's not OMPL-specific, then rename this to initializeWithTrajectory or something like that.

actually trajectory from STOMP can also be made to work with MotionPlanDetailedResponse as in here . I can possibly change it to initializeWithTrajectory

done ! replaced function name with fillInFromTrajectory()

mamoll · 2018-08-02T01:18:37Z

moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp

+  int num_joints_trajectory = trajectory_msgs.joint_trajectory.points[0].positions.size();
+
+  // variables for populating the CHOMP trajectory with correct number of trajectory points
+  int repeated_factor = num_chomp_trajectory_points / num_response_points;


This would be 0 if num_response_points>num_chomp_trajectory_points

yes you are correct, I will write another way of setting of the trajectory whenever num_response>num_chomp_trajectory_points. For chomp there are generally 100 points and OMPL produces less than 100 points almost always, but I get it, I will implement another way of loading of trajectory for num_response_points > chomp_trajectory_points

done! implemented a simple decimation down sampling of the input trajectory to address this.

per @mamoll, you should add error checking and return false if the input trajectory_msgs has no points

mamoll · 2018-08-02T01:29:07Z

moveit_ros/planning/planning_request_adapter_plugins/src/chomp_optimizer_adapter.cpp

+      res.trajectory_ = res_detailed.trajectory_[0];
+      res.planning_time_ = res_detailed.processing_time_[0];
+    }
+


If the initial trajectory is valid, but CHOMP makes it invalid, does the planning adaptor return the initial trajectory?

Yes, it should as per this block, if planning_success from CHOMP is false, then the res MotionPlanDetailedResponse object is never changed from CHOMP and should stay the same as obtained from OMPL as this is the only place where the response trajectory is changed in the chomp_optimizer_adapter.cpp apart from being set initially here in the OMPL planner.

davetcoleman · 2018-08-05T17:57:05Z

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

+   * checker for a constant planning scene whenever required, e.g., for some planners like CHOMP
+   */
+  const void
+  addCollisionDetector_ConstVersion(const collision_detection::CollisionDetectorAllocatorPtr& allocator) const;


no _ underscore in function name

Also remove "Version" from function name

sure will do

davetcoleman · 2018-08-05T17:57:28Z

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -246,6 +246,14 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
   * */
  void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator);

+  /** \brief Add a new collision detector type.
+   *
+   * this is the constant version of the function addCollisionDetector(), it is added here for changing the collision


Capitalize "this" at start of sentence here and below

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

davetcoleman · 2018-08-05T17:57:55Z

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

+  * this is the constant version of the function setActiveCollisionDetector(), it is added here for setting the
+  * collision checker for a constant planning scene whenever required, e.g., for some planners like CHOMP
+  */
+  const void setActiveCollisionDetector_ConstVersion(


no _ underscore in function name

Also remove "Version" from function name

davetcoleman · 2018-08-05T17:58:03Z

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

+   * name of a collision detector that has been previously added with
+   * addCollisionDetector_ConstVersion().
+   */
+  const bool setActiveCollisionDetector_ConstVersion(const std::string& collision_detector_name) const;


no _ underscore in function name

Also remove "Version" from function name

davetcoleman · 2018-08-05T18:05:36Z

moveit_ros/planning/planning_request_adapter_plugins/src/chomp_optimizer_adapter.cpp

+
+  virtual std::string getDescription() const
+  {
+    return "CHOMP Optimizer !!!!";


more professional (no !)

oh yeah sure !! removed.

davetcoleman · 2018-08-05T18:05:54Z

moveit_ros/planning/planning_request_adapter_plugins/src/chomp_optimizer_adapter.cpp

+    // populate the trajectory to pass to CHOMPPlanner::solve() method. Obtain trajectory from OMPL's
+    // planning_interface::MotionPlanResponse object and put / populate it in the
+    // moveit_msgs::MotionPlanDetailedResponse object
+    moveit_msgs::RobotTrajectory trajectory_msgs2;


better variable name than "2"

changed to trajectory_msg_from_response

davetcoleman · 2018-08-05T18:06:07Z

moveit_ros/planning/planning_request_adapter_plugins/src/chomp_optimizer_adapter.cpp

+
+    chomp::ChompPlanner chompPlanner;
+    planning_interface::MotionPlanDetailedResponse res_detailed;
+    moveit_msgs::MotionPlanDetailedResponse res2;


better var name than 2

changed to res_detailed_moveit_msgs

davetcoleman · 2018-08-05T18:06:37Z

moveit_ros/planning/planning_request_adapter_plugins/src/chomp_optimizer_adapter.cpp

+
+    bool planning_success;
+
+    bool temp = chompPlanner.solve(planning_scene, req, params_, res2);


you don't need temp and planning_success, combine them

davetcoleman · 2018-08-05T18:07:54Z

moveit_ros/planning/planning_request_adapters_plugin_description.xml

@@ -34,5 +34,10 @@
    <description>
    </description>
  </class>
+
+  <class name="default_planner_request_adapters/CHOMPOptimizerAdapter" type="default_planner_request_adapters::CHOMPOptimizerAdapter" base_class_type="planning_request_adapter::PlanningRequestAdapter">


add line break instead of word wrap usage

all others classes/plugins in the planning_request_adapters_plugin_description.xml file also had a word wrap usage and not a line break, so I kept this as word wrap for now. I can change everything to line break, but I think it should be word wrap for all (I maybe wrong).

davetcoleman · 2018-08-09T20:59:46Z

is this ready for review again?

raghavendersahdev · 2018-08-09T21:02:48Z

yes

mamoll · 2018-08-10T14:23:10Z

👍 looks good to me.

davetcoleman · 2018-08-22T00:50:40Z

travis failed, i just restarted it

raghavendersahdev · 2018-08-22T02:14:52Z

it still failed after you restarted, I just committed a a sample dot, to restart travis to double check, last time something similar happened to another of my PRs in moveit_tutorials, I just made a sample commit and it passed..lets see this time

raghavendersahdev · 2018-08-22T02:53:27Z

Ok so now travis passes which is good but weird, maybe my guess is when its only restarted it might be picking up some form of CMakeCache file but when a commit is made it restarts from scratch, I don't exactly know travis functionality so I may be wrong here.

davetcoleman · 2018-09-27T23:25:34Z

Travis failed:

Errors << moveit_ros_planning:cmake /root/ws_moveit/logs/moveit_ros_planning/build.cmake.000.log
CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:76 (find_package):
Could not find a package configuration file provided by
"chomp_motion_planner" with any of the following names:

I'm guessing you are missing a dependency on chomp_motion_planner in moveit_ros_planning

…ges in planning_scene.h / .cpp files to make this work

…truction but works fine right now

…tion_method

…ajectory has more points than CHOMP trajectory points

…omp_initialization from trajectory simpler, now has if, for, for

raghavendersahdev · 2018-10-01T14:59:27Z

resolved merge conflicts by re-basing the PR and Travis passes currently

rhaschke · 2018-10-15T13:11:39Z

As a note for others: This merged PR currently breaks kinetic-devel. There is a fix available (#1086), waiting for approval and merge.

simonschmeisser · 2018-10-15T13:22:59Z

strange, why did this compile here? Shouldn't it have failed due to the missing package.xml entry here as well as it does in kinetic-devel (and on our internal build server)

rhaschke · 2018-10-15T13:36:30Z

strange, why did this compile here?

Probably, it was simply build-order luck that it compiled here.

Fixing issues introduced by #1012.

…nners (moveit#1012) * added functionality for CHOMP Optimization Adapter and made some changes in planning_scene.h / .cpp files to make this work * added functionality for optimizing path obtained from OMPL, under construction but works fine right now * added functionality to initialize trajectories in CHOMP when input trajectory has more points than CHOMP trajectory points * hotfix moveit#1086

raghavendersahdev · 2018-10-17T11:49:43Z

The way travis function sometimes confuses me, I wonder if there is a tutorial on its functionality somewhere online . . Sorry for temporarily breaking kinetic-devel , my bad ; In my defense, I was blindly dependent on travis results to ensure clean builds . .

rhaschke · 2018-10-17T11:53:40Z

This wasn't your fault. It's our task as maintainers to actually ensure that nothing breaks.

simonschmeisser · 2018-10-17T11:55:26Z

I don't think this is related to travis ...
You did not include the dependency on chomp_planner in package.xml, therefore catkin tools did not know that it has to build packages in a specific order. (ie chomp_planner before the failing dependent package). So sometimes it did build it before, sometimes after. You could have observed the same behaviour "offline" if you rebuild the whole workspace from scratch (catkin clean all)

I remember seeing warnings/errors about cases like this some time ago, was that check pre catkin tools?

raghavendersahdev · 2018-10-17T12:00:45Z

thanks @rhaschke ; @simonschmeisser On these lines I have a small quick question, do you know a way to ensure that certain packages are built in a specific order, in this case how would you ensure chomp_planner package is built before some of its other packages on which it might depend (here =Planning_request_adapters package)...

I am asking this as I observed this in my code while I was creating the STOMP planning adapter in my PR #1081 , which involved stomp_moveit package to be built before the planning_adapters is complied/built... I was getting a similar issue, I kind of pseudo hacked it on my local machine to install stomp_moveit and then reinstall moveit so that Planning adapters reads that package... but is there a concrete legit way to handle these order of builds in catkin...

raghavendersahdev · 2018-10-17T12:01:38Z

Or is releasing as a debian only the way

simonschmeisser · 2018-10-17T12:03:45Z

no, having a dependency (stomp_moveit) in in package.xml (of planning_adapters) should ensure that stomp_moveit gets build before planning_adapters

raghavendersahdev · 2018-10-17T12:06:58Z

oh Yes you are right, thanks 👍 I did not do that, will do that

simonschmeisser · 2018-10-24T08:46:36Z

Note that the right thing to do would be to move the adapter to it's own package in one of the chomp sub-directories to a) resolve this dependency inversion b) make it possible to CATKIN_IGNORE chomp completely and c) make it possible to release MoveIt! without it

130s · 2018-10-24T08:49:51Z

moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt

+find_package(Boost REQUIRED)
+
+include_directories(
+        ../../../moveit_planners/chomp/chomp_motion_planner/include/


Sorry if I missed any discussion, but is relative path a good way to find resource?

I'm afraid using relative path of another package would limit the portability of a package.

I guess with the suggestion in #1012 (comment) we'll be able to replace this hardcoded relative path with auto-substituted path.

…nners (moveit#1012) * added funcitonality for CHOMP Optimization Adapter and made some changes in planning_scene.h / .cpp files to make this work * added comments and cleaned up some code * added funcitonality for optimizing path obtained from OMPL, underconstruction but works fine right now * did some commenting and cleaning up of code * cleaned up some code and added comments * corrected my previous public/private small bug in planning_scene.h * changed CHOMP Parameter name to make more sense trajectory_initialization_method * added functionality to initialize trajectories in CHOMP when input trajectory has more points than CHOMP trajectory points * changed author for chomp_optimizer_adapter.cpp * addressed Dave's PR requested changes * addressed PR requested changes added a new helper function to make chomp_initialization from trajectory simpler, now has if, for, for * changed OMPL to fillTrajectory as the parameter value * just a dot * added chomp_motion_planner dependency in moveit_ros_planning

mamoll reviewed Aug 2, 2018

View reviewed changes

raghavendersahdev mentioned this pull request Aug 2, 2018

added tutorial section to use OMPL as a pre-processor for CHOMP moveit/moveit_tutorials#204

Closed

2 tasks

raghavendersahdev referenced this pull request Aug 5, 2018

under construction commit for feedback from Mark and Dave

7d5e328

davetcoleman requested changes Aug 5, 2018

View reviewed changes

raghavendersahdev force-pushed the CHOMP_planning_request_adapter branch from 098c130 to 6df219b Compare August 7, 2018 05:44

raghavendersahdev force-pushed the CHOMP_planning_request_adapter branch from 3e59acb to 8419c39 Compare August 10, 2018 20:32

bmagyar approved these changes Aug 16, 2018

View reviewed changes

raghavendersahdev force-pushed the CHOMP_planning_request_adapter branch from 8419c39 to 85262e8 Compare August 17, 2018 12:43

davetcoleman approved these changes Aug 22, 2018

View reviewed changes

davetcoleman mentioned this pull request Sep 27, 2018

Planning Request Adapters tutorials moveit/moveit_tutorials#220

Merged

6 tasks

raghavendersahdev added 12 commits October 1, 2018 03:45

added funcitonality for CHOMP Optimization Adapter and made some chan…

4663b81

…ges in planning_scene.h / .cpp files to make this work

added comments and cleaned up some code

e812349

added funcitonality for optimizing path obtained from OMPL, undercons…

ec3a2c1

…truction but works fine right now

did some commenting and cleaning up of code

4f293cf

cleaned up some code and added comments

d4cf911

corrected my previous public/private small bug in planning_scene.h

b481ffd

changed CHOMP Parameter name to make more sense trajectory_initializa…

e65a228

…tion_method

added functionality to initialize trajectories in CHOMP when input tr…

e3e4d92

…ajectory has more points than CHOMP trajectory points

changed author for chomp_optimizer_adapter.cpp

ef993b1

addressed Dave's PR requested changes

ec9c7a1

addressed PR requested changes added a new helper function to make ch…

796b7e3

…omp_initialization from trajectory simpler, now has if, for, for

changed OMPL to fillTrajectory as the parameter value

abeb5ba

just a dot

1c24027

raghavendersahdev force-pushed the CHOMP_planning_request_adapter branch from 309e845 to 1c24027 Compare October 1, 2018 08:05

added chomp_motion_planner dependency in moveit_ros_planning

bacac4b

raghavendersahdev mentioned this pull request Oct 9, 2018

STOMP Smoothing Adapter ros-industrial/industrial_moveit#78

Closed

2 tasks

davetcoleman approved these changes Oct 13, 2018

View reviewed changes

davetcoleman merged commit b84b1bf into moveit:kinetic-devel Oct 13, 2018

rhaschke mentioned this pull request Oct 15, 2018

hot fix #1012 #1086

Merged

rhaschke added a commit that referenced this pull request Oct 17, 2018

Merge pull request #1086 from ubi-agni/hotfix-#1012

f85f672

Fixing issues introduced by #1012.

simonschmeisser mentioned this pull request Oct 19, 2018

Chomp trajectory seed #1061

Closed

130s reviewed Oct 24, 2018

View reviewed changes

This was referenced Oct 26, 2018

Moveit regression on Melodic #1154

Closed

[CMake] add build depend on cmake_modules #1161

Closed

IanTheEngineer mentioned this pull request Oct 29, 2018

remove left-over tf artifacts #1170

Merged


		bool planning_success;

		bool temp = chompPlanner.solve(planning_scene, req, params_, res2);

Addition of CHOMP planning adapter for optimizing result of other planners #1012

Addition of CHOMP planning adapter for optimizing result of other planners #1012

Conversation

raghavendersahdev commented Aug 1, 2018 • edited Loading

Addition of CHOMP planning adapter to act as an additional optimizer for other planners like OMPL

Checklist

raghavendersahdev commented Aug 1, 2018

Choose a reason for hiding this comment

raghavendersahdev Aug 2, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

raghavendersahdev Aug 7, 2018 • edited Loading

Choose a reason for hiding this comment

davetcoleman commented Aug 9, 2018

raghavendersahdev commented Aug 9, 2018

mamoll commented Aug 10, 2018

davetcoleman commented Aug 22, 2018

raghavendersahdev commented Aug 22, 2018

raghavendersahdev commented Aug 22, 2018 • edited Loading

davetcoleman commented Sep 27, 2018

raghavendersahdev commented Oct 1, 2018

rhaschke commented Oct 15, 2018

simonschmeisser commented Oct 15, 2018

rhaschke commented Oct 15, 2018 • edited by 130s Loading

raghavendersahdev commented Oct 17, 2018

rhaschke commented Oct 17, 2018

simonschmeisser commented Oct 17, 2018

raghavendersahdev commented Oct 17, 2018 • edited Loading

raghavendersahdev commented Oct 17, 2018

simonschmeisser commented Oct 17, 2018

raghavendersahdev commented Oct 17, 2018

simonschmeisser commented Oct 24, 2018

130s Oct 24, 2018 • edited Loading

Choose a reason for hiding this comment

130s Oct 24, 2018 • edited Loading

Choose a reason for hiding this comment

raghavendersahdev commented Aug 1, 2018 •

edited

Loading

raghavendersahdev Aug 2, 2018 •

edited

Loading

raghavendersahdev Aug 7, 2018 •

edited

Loading

raghavendersahdev commented Aug 22, 2018 •

edited

Loading

rhaschke commented Oct 15, 2018 •

edited by 130s

Loading

raghavendersahdev commented Oct 17, 2018 •

edited

Loading

130s Oct 24, 2018 •

edited

Loading

130s Oct 24, 2018 •

edited

Loading