Skip to content
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

CHOMP motion planner cleanup of unused parameters and code + addition of trajectory initialization methods (linear, cubic, quintic-spline) #960

Merged

Conversation

Projects
None yet
5 participants
@raghavendersahdev
Copy link
Contributor

commented Jun 25, 2018

CHOMP Motion planner cleanup of code and new trajectory initialization methods

This pull request is a part of Google summer of code 2018 work by Raghavender Sahdev. This pull request removes any unused parameters and code for the CHOMP planner. It also has the code from Pull request #899.

Checklist

  • removed unused code pertaining to unused CHOMP parameters
  • removed commented code in chomp_optimizer.cpp file.
  • addition of trajectory initialization methods for CHOMP
nh_.param("use_stochastic_descent", params_.use_stochastic_descent_, true);
nh_.param("interpolation_method", params_.trajectory_initialization_method_, std::string("quintic-spline"));
// filter_mode_ = false;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jun 29, 2018

Member

awesome cleanup!

remove this comment also?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jun 29, 2018

Author Contributor

yes will do

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 1, 2018

Author Contributor

done


void fillInLinearInterpolation();

void fillInCubicInterpolation();

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jun 29, 2018

Member

add doxygen comments

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 1, 2018

Author Contributor

done

@@ -53,33 +53,40 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
{
if (!planning_scene)
{
ROS_ERROR_STREAM("No planning scene initialized.");
ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");

This comment has been minimized.

Copy link
@davetcoleman

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 1, 2018

Author Contributor

thanks.

res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
return false;
}

ros::WallTime start_time = ros::WallTime::now();
ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);

// std::cout << trajectory.getTrajectory() << " 1. complete initialized TRAJECTORY in CHOMP_PLANNER..!!!!!" <<
// std::endl;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jun 29, 2018

Member

you cleanup the dead code but then add your own commented out code? :-)

use ROS_DEBUG_STREAM_NAMED if you want to keep stuff like this. otherwise remove it

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jun 29, 2018

Author Contributor

Oh yeah, I should remove my commented code too, will remove it !

//std::cout << " using initialization method: " << params.interpolation_method_ << std::endl;

// std::cout << trajectory.getTrajectory() << "4. complete initialized TRAJECTORY in CHOMP_PLANNER after min
// jerk..!!!!!" << std::endl;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jun 29, 2018

Member

cleanup commented code here and elsewhere

@@ -37,6 +37,8 @@
#include <ros/ros.h>
#include <chomp_motion_planner/chomp_trajectory.h>
#include <iostream>
#include <stdio.h>
using namespace std;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jun 29, 2018

Member

as a convention we do use using namespace std

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 1, 2018

Author Contributor

thanks for the info.

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 4, 2018

Member

to clarify: can you remove it from usage here?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 5, 2018

Author Contributor

sure, removed

@@ -66,8 +68,15 @@ ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_m
const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
num_joints_ = model_group->getActiveJointModels().size();
init();
std::cout << trajectory_ << " complete initialized TRAJECTORY..!!!!!" << std::endl;

This comment has been minimized.

Copy link
@davetcoleman

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 1, 2018

Author Contributor

removed

}

/**
* this contructor simply copies over the source trajectory into the current trajectory object

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jun 29, 2018

Member

function documentation belongs only in the header file, not cpp

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jun 29, 2018

Author Contributor

sure will remove this.

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 1, 2018

Author Contributor

removed.

for (int i = 0; i < num_joints_; i++)
{
double theta = ((*this)(end_index, i) - (*this)(start_index,i)) / (end_index- 1);
for(int j=start_index+1 ; j< end_index ; j++)

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jun 29, 2018

Member

did you run clang-format? there should be spaces around =

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jun 29, 2018

Author Contributor

yes I run clang-format in my latest commit.

This comment has been minimized.

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 12, 2018

Author Contributor

travis (all tests) pass currently.

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch from a928078 to b5d4034 Jul 1, 2018

@mamoll

This comment has been minimized.

Copy link
Contributor

commented Jul 1, 2018

Looks good to me.

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch from 0be2e02 to c020b5d Jul 5, 2018

@@ -388,19 +352,9 @@ void ChompOptimizer::optimize()
// ROS_INFO_STREAM("Collision increments took " << (ros::WallTime::now()-coll_time));
calculateTotalIncrements();

// if(!parameters_->getUseHamiltonianMonteCarlo())

This comment has been minimized.

Copy link
@BryceStevenWilley

BryceStevenWilley Jul 5, 2018

Contributor

So Hamiltonian Monte Carlo is an important part of maintaining CHOMP's completeness/resilience to local minimums. It does have trade offs and I understand that you might not have time to verify and test this feature as much, but IMO this should be kept, or at least marked as a priority for the next time CHOMP is worked on. Maybe the default should be set to false, and the feature can be documented as experimental?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 5, 2018

Author Contributor

Yes I agree to what you say and this also well aligns with the research paper. However none of the HMC based code was getting used and it was all commented. I did try and uncomment all the HMC part for CHOMP to avoid local minima while avoiding obstacles. It was rarely able to break out of the local minima.

Maybe I might have missed something earlier. I could do some more extensive testing later in July . Meanwhile I could leave this part commented and mark this feature documented as experimental if required.

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 5, 2018

Member

I don't know the background theory to this but, based on Bryce's feedback, I'd suggest we keep the commented out stuff around Hamiltonian Monte Carlo but add a TODO note explaining why it is commented out, why it is important, and what might need to be done to restore it / fix it

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 5, 2018

Author Contributor

Ok, sure. I will get the commented out stuff related to Hamiltonian Monte Carlo back with appropriate comments about the reason for its presence.

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch from 65547c7 to f8aa4fd Jul 10, 2018

@mamoll

mamoll approved these changes Jul 13, 2018

Copy link
Contributor

left a comment

👍 Looks good to me.

@davetcoleman davetcoleman merged commit 32dea23 into ros-planning:kinetic-devel Jul 16, 2018

1 check passed

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

This comment has been minimized.

Copy link
Member

commented Jul 16, 2018

Great work @raghavendersahdev !

@bmagyar

This comment has been minimized.

Copy link
Member

commented Jul 16, 2018

Great!!! 👍

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

CHOMP motion planner cleanup of unused parameters and code + addition…
… of trajectory initialization methods (linear, cubic, quintic-spline) (#960)

* added options for trajectory initialization using (linear, cubic) interpolation in addition to quintic-spline, this can be specified in the chomp_planning.yaml file as a string parameter

* removed/cleaned redundant commented code from chomp_optimizer.cpp file

* commented print statements

* removed unused CHOMP parameters code

* applied clang-format and addressed comments

* applied clang-format and addressed comments

* added PR suggested revies/changes

* removed usused 2 lines

* generating chomp_planning.yaml and chomp_planning_pipeline.launch.xml from the moveit setup assistant

* restored CHOMP, hamiltonian monte carlo (HMC) based code and commented it

* applied clang format

dg-shadow added a commit to shadow-robot/moveit that referenced this pull request Jul 30, 2018

CHOMP motion planner cleanup of unused parameters and code + addition…
… of trajectory initialization methods (linear, cubic, quintic-spline) (ros-planning#960)

* added options for trajectory initialization using (linear, cubic) interpolation in addition to quintic-spline, this can be specified in the chomp_planning.yaml file as a string parameter

* removed/cleaned redundant commented code from chomp_optimizer.cpp file

* commented print statements

* removed unused CHOMP parameters code

* applied clang-format and addressed comments

* applied clang-format and addressed comments

* added PR suggested revies/changes

* removed usused 2 lines

* generating chomp_planning.yaml and chomp_planning_pipeline.launch.xml from the moveit setup assistant

* restored CHOMP, hamiltonian monte carlo (HMC) based code and commented it

* applied clang format

MohmadAyman added a commit to MohmadAyman/moveit that referenced this pull request Aug 25, 2018

CHOMP motion planner cleanup of unused parameters and code + addition…
… of trajectory initialization methods (linear, cubic, quintic-spline) (ros-planning#960)

* added options for trajectory initialization using (linear, cubic) interpolation in addition to quintic-spline, this can be specified in the chomp_planning.yaml file as a string parameter

* removed/cleaned redundant commented code from chomp_optimizer.cpp file

* commented print statements

* removed unused CHOMP parameters code

* applied clang-format and addressed comments

* applied clang-format and addressed comments

* added PR suggested revies/changes

* removed usused 2 lines

* generating chomp_planning.yaml and chomp_planning_pipeline.launch.xml from the moveit setup assistant

* restored CHOMP, hamiltonian monte carlo (HMC) based code and commented it

* applied clang format

@rhaschke rhaschke referenced this pull request Dec 9, 2018

Closed

Chomp fix goalstate check #899

0 of 3 tasks complete
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.