New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 14 commits into from Oct 13, 2018

Conversation

Projects
None yet
7 participants
@raghavendersahdev
Copy link
Contributor

raghavendersahdev commented Aug 1, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Aug 1, 2018

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

@@ -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

This comment has been minimized.

@mamoll

mamoll Aug 2, 2018

Contributor

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.

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 2, 2018

Contributor

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

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 5, 2018

Contributor

done ! replaced function name with fillInFromTrajectory()

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 comment has been minimized.

@mamoll

mamoll Aug 2, 2018

Contributor

This would be 0 if num_response_points>num_chomp_trajectory_points

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 2, 2018

Contributor

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

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 5, 2018

Contributor

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

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

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

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

This comment has been minimized.

@mamoll

mamoll Aug 2, 2018

Contributor

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

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 2, 2018

Contributor

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.

* 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;

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

no _ underscore in function name

Also remove "Version" from function name

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 5, 2018

Contributor

sure will do

@@ -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

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

Capitalize "this" at start of sentence here and below

This comment has been minimized.

@raghavendersahdev
Show resolved Hide resolved 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(

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

no _ underscore in function name

Also remove "Version" from function name

This comment has been minimized.

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

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

no _ underscore in function name

Also remove "Version" from function name

This comment has been minimized.

@raghavendersahdev

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

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

more professional (no !)

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 7, 2018

Contributor

oh yeah sure !! removed.

// 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;

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

better variable name than "2"

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 7, 2018

Contributor

changed to trajectory_msg_from_response


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

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

better var name than 2

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 7, 2018

Contributor

changed to res_detailed_moveit_msgs


bool planning_success;

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

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

you don't need temp and planning_success, combine them

@@ -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">

This comment has been minimized.

@davetcoleman

davetcoleman Aug 5, 2018

Member

add line break instead of word wrap usage

This comment has been minimized.

@raghavendersahdev

raghavendersahdev Aug 7, 2018

Contributor

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).

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:CHOMP_planning_request_adapter branch from 098c130 to 6df219b Aug 7, 2018

@davetcoleman

This comment has been minimized.

Copy link
Member

davetcoleman commented Aug 9, 2018

is this ready for review again?

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Aug 9, 2018

yes

@mamoll

This comment has been minimized.

Copy link
Contributor

mamoll commented Aug 10, 2018

👍 looks good to me.

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:CHOMP_planning_request_adapter branch from 3e59acb to 8419c39 Aug 10, 2018

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:CHOMP_planning_request_adapter branch from 8419c39 to 85262e8 Aug 17, 2018

@davetcoleman

This comment has been minimized.

Copy link
Member

davetcoleman commented Aug 22, 2018

travis failed, i just restarted it

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Aug 22, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Aug 22, 2018

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 davetcoleman referenced this pull request Sep 27, 2018

Merged

Planning Request Adapters tutorials #220

0 of 6 tasks complete
@davetcoleman

This comment has been minimized.

Copy link
Member

davetcoleman commented Sep 27, 2018

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

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:CHOMP_planning_request_adapter branch from 309e845 to 1c24027 Oct 1, 2018

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Oct 1, 2018

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

@raghavendersahdev raghavendersahdev referenced this pull request Oct 9, 2018

Open

STOMP Smoothing Adapter #78

0 of 2 tasks complete

@davetcoleman davetcoleman merged commit b84b1bf into ros-planning:kinetic-devel Oct 13, 2018

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

@rhaschke rhaschke referenced this pull request Oct 15, 2018

Merged

hot fix #1012 #1086

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented Oct 15, 2018

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

@simonschmeisser

This comment has been minimized.

Copy link
Contributor

simonschmeisser commented Oct 15, 2018

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

This comment has been minimized.

Copy link
Contributor

rhaschke commented Oct 15, 2018

strange, why did this compile here?

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

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

Merge pull request #1086 from ubi-agni/hotfix-#1012
Fixing issues introduced by #1012.

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Oct 17, 2018

Addition of CHOMP planning adapter for optimizing result of other pla…
…nners (ros-planning#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 ros-planning#1086
@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Oct 17, 2018

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

This comment has been minimized.

Copy link
Contributor

rhaschke commented Oct 17, 2018

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

@simonschmeisser

This comment has been minimized.

Copy link
Contributor

simonschmeisser commented Oct 17, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Oct 17, 2018

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

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Oct 17, 2018

Or is releasing as a debian only the way

@simonschmeisser

This comment has been minimized.

Copy link
Contributor

simonschmeisser commented Oct 17, 2018

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

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented Oct 17, 2018

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

@simonschmeisser

This comment has been minimized.

Copy link
Contributor

simonschmeisser commented Oct 24, 2018

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

find_package(Boost REQUIRED)

include_directories(
../../../moveit_planners/chomp/chomp_motion_planner/include/

This comment has been minimized.

@130s

130s Oct 24, 2018

Member

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.

This comment has been minimized.

@130s

130s Oct 24, 2018

Member

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment