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

Addition of Failure recovery options for CHOMP by tweaking parameters #987

Merged

Conversation

Projects
None yet
3 participants
@raghavendersahdev
Copy link
Contributor

commented Jul 17, 2018

Dynamically tweaking CHOMP parameters to find a solution:

This pull request is a part of Google summer of code 2018 work by Raghavender Sahdev. This dynamically tweaks some CHOMP parameters in the hope totry and find a solution when one does not exist,

added functionality:

  • tweaks the following parameters for CHOMP: learning_rate, ridge_factor, planning_time_limit, max_iterations
  • adds two additional parameters for the chomp_planning.yaml in the setup assistant
@@ -61,7 +61,7 @@ class ChompOptimizer

virtual ~ChompOptimizer();

void optimize();
int optimize();

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 17, 2018

Contributor

document meaning of return value

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

done, changed it to bool and documented

@@ -105,6 +107,8 @@ class ChompParameters
bool filter_mode_;
// double random_jump_amount_;
std::string trajectory_initialization_method_;
bool enable_failure_recovery_;

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 17, 2018

Contributor

document what these new member variables are for

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

done

{
replan_cnt++;
replan_flag = true;
goto RePlan;

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 17, 2018

Contributor

Goto considered harmful. Please rewrite using a regular for or while loop.

This comment has been minimized.

Copy link
@davetcoleman

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

done, replaced with a do while loop

org_learning_rate = params.learning_rate_;
org_ridge_factor = params.ridge_factor_;
org_planning_time_limit = params.planning_time_limit_;
org_max_iterations = params.max_iterations_;

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 17, 2018

Contributor

why not initialize the org_* variables outside the loop with the params.* values?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

yes you are right, don't know why I did not do that in the first place, thanks 👍

{
if (replan_flag)
{
params.learning_rate_ *= 0.02; // increase learning rate in hope to find obstacles (possibly faster convergence)

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 17, 2018

Contributor

Faster convergence doesn't seem as important as being able to find any feasible solution.

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

changed the comment to increase learning rate in hope to find a successful path

@@ -48,7 +48,7 @@ ChompPlanner::ChompPlanner()
}

bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
const moveit_msgs::MotionPlanRequest& req, chomp::ChompParameters& params,

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 17, 2018

Contributor

wouldn't it be easier to keep params const, copy the values into org_* and use/modify that?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 21, 2018

Author Contributor

changed back to const, currently using a getter method to get a non constant version of the ChompParameters

@@ -50,7 +50,7 @@ class CHOMPInterface : public chomp::ChompPlanner
public:
CHOMPInterface(const ros::NodeHandle& nh = ros::NodeHandle("~"));

const chomp::ChompParameters& getParams() const
chomp::ChompParameters& getParams()

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

this should remain const for best practice. if you want to change the params you can create a setParams(params), or another approach you'll likely prefer making a getParamsNonConst()

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 21, 2018

Author Contributor

changed back to const, currently using a getter method to get a non constant version of the ChompParameters

@@ -250,6 +254,16 @@ inline std::string ChompParameters::getTrajectoryInitializationMethod() const
return trajectory_initialization_method_;
}

inline bool ChompParameters::getEnableFailureRecovery() const

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

in most all cases you do not need inlineas the compiler is very smart these days.

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

removed inline

@@ -51,7 +51,7 @@ class ChompPlanner
virtual ~ChompPlanner(){};

bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req,
const ChompParameters& params, moveit_msgs::MotionPlanDetailedResponse& res) const;
ChompParameters& params, moveit_msgs::MotionPlanDetailedResponse& res) const;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

why are you removing const from params?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

currently resolving the const removal issue, will commit soon.

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

I had removed it earlier as I needed to change the contents of the ChompParameters &params. Didn't realize this might not be the ideal way. currently fixing it

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 21, 2018

Author Contributor

changed, currently using a non constant version of ChompParameters

{
int optimization_result = 0;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

this should be a bool

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

done

@@ -156,6 +156,33 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s

ros::WallTime create_time = ros::WallTime::now();

/// block for replanning (recovery behaviour) if collision free optimized solution not found
int replan_cnt = 0;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

please no abbreviations per google style... replan_count

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 21, 2018

Author Contributor

done

params.planning_time_limit_ += 5; // add 5 additional secs in hope to find a solution
params.max_iterations_ += 50; // increase maximum iterations
}
if (replan_cnt == 0)

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

add comment what is happening here

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

coded it in a better way now, removed if condition

org_max_iterations = params.max_iterations_;
}
};
std::cout << "learning rate: " << org_learning_rate << std::endl

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

use ROS_INFO_STREAM etc

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

removed std::cout

optimizer.optimize();
int optimization_result = optimizer.optimize();

/// replan with updated parameters if no solution is found

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

only two slashes //, three is reserved for doxygen comments in the header file for public functions

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

done

ROS_INFO("Replanning with updated Chomp Parameters (learning_rate, ridge_factor, planning_time_limit, "
"max_iterations), attempt: # %d ",
(replan_cnt + 1));
ROS_INFO("learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ", params.learning_rate_,

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

capitalize learning

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

done

}

// resetting the CHOMP Parameters to the original values after a successful plan
{

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 18, 2018

Member

why brackets here?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 21, 2018

Author Contributor

removed

{
replan_cnt++;
replan_flag = true;
goto RePlan;

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jul 20, 2018

Member

as @mamoll already pointed out, we cannot have goto in this code

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 20, 2018

Author Contributor

done, replaced with do while loop

@@ -50,7 +50,7 @@ class CHOMPInterface : public chomp::ChompPlanner
public:
CHOMPInterface(const ros::NodeHandle& nh = ros::NodeHandle("~"));

const chomp::ChompParameters& getParams() const

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 22, 2018

Contributor

Can you make the method const again?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 23, 2018

Author Contributor

done


double smoothness_cost_velocity_; /// variables associated with the cost in velocity
double smoothness_cost_acceleration_; /// variables associated with the cost in acceleration
double smoothness_cost_jerk_; /// variables associated with the cost in jerk

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 22, 2018

Contributor

Thanks for documenting all the parameters!

{
return trajectory_initialization_method_;
}

This comment has been minimized.

Copy link
@mamoll

mamoll Jul 22, 2018

Contributor

Since the CHOMP parameters are already marked public it is indeed useless to have getter methods for them. Alternatively, the parameters could be declared private. Note that the getter methods return parameters by value (not by reference), so const access is possible. If you make the parameters protected, there should probably also be corresponding setter methods. @davetcoleman, any thoughts?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 22, 2018

Author Contributor

The CHOMP parameters were marked public and also had getter methods from beginning only, which I agree makes having getter methods useless. I can go ahead and delete all the getter methods and directly call using the public variables.....the public nature of the CHOMP parameters sets their value in the CHOMP_interface here... and at some other places also the parameters are directly accessed due to public nature of the variables, I can delete the getter methods, should I? Whereas at some other places the getter methods are used in the CHOMP code, should I convert everywhere to directly accessing the chomp public parameters. and get rid of getter methods?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Jul 23, 2018

Author Contributor

I got rid of all the getter methods in my latest commit, I can always revert back one commit if this is not required by git reset #commit_hash

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch from 6c1c010 to 9abfaf4 Jul 25, 2018

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Jul 27, 2018

its not clear to me which PR i should review: this or ros-planning/moveit_tutorials#195

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor Author

commented Jul 27, 2018

both this PR and ros-planning/moveit_tutorials#195 should be reviewed but the second one is already merged it seems.

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch 2 times, most recently from 0387860 to 40d1ad8 Aug 2, 2018

@@ -61,7 +61,11 @@ class ChompOptimizer

virtual ~ChompOptimizer();

void optimize();
/**
* this function optimizes the chomp cost function and tries to find an optimal path

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

Remove "this function" (more concise) and capitalize Optimizes

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

chomp -> CHOMP

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

void optimize();
/**
* this function optimizes the chomp cost function and tries to find an optimal path
* @return returns true if an optimal collision free path is found else returns false

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

remove "returns" (redundant)

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

bool getUseStochasticDescent() const;
std::string getTrajectoryInitializationMethod() const;
/**
* function which returns the non constant version of the ChompParameters

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

redundant with @return line, remove

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

/**
* function which returns the non constant version of the ChompParameters
* @param params equates the returned chomp parameters to this argument
* @return returns the non constant version of ChompParameters

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

remove "returns" (redundant)

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

* @param params equates the returned chomp parameters to this argument
* @return returns the non constant version of ChompParameters
*/
ChompParameters getNonConstantParams(ChompParameters params);

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

getNonConstParams()

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

non_constant.max_iterations_ += 50; // increase maximum iterations
}

optimizer = new ChompOptimizer(&trajectory, planning_scene, req.group_name, &non_constant, start_state);

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

this is a memory leak - optimizer is never deleted. can you refactor this code to instantiate the ChompOptimizer only once, but use asetParams() function to change the params for each loop?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

thanks, I didn't realize this earlier. I am currently deleting the optimizer before creating a new ChompOptimizer with new parameters, if ChompOptimizer is instantiated only once its, difficult to use the const ChompParameters parameters_ as it is used at a lot of places in the optimize method, using a non const version would not be good here, coz I need to do too many changes in optimize method. So I simply delete the pointer here to prevent a memory leak.

ChompOptimizer* optimizer;

// create a non_const_params variable which stores the non constant version of the const params variable
ChompParameters non_constant = non_constant.getNonConstantParams(params);

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

better variable name - how about params_nonconst

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

sure, params_nonconst it is then, done

ROS_INFO("Planned with Chomp Parameters (learning_rate, ridge_factor, planning_time_limit, "
"max_iterations), attempt: # %d ",
(replan_count + 1));
ROS_INFO("Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ",

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

use NAMED

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

if (non_constant.enable_failure_recovery_)
{
// if (!optimization_result && replan_count != non_constant.getMaxRecoveryAttempts())
//{

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

remove commented code

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

}
else
break;
} while (true);

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Aug 5, 2018

Member

no need for do... while if its always true. simply just use while loop

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 7, 2018

Author Contributor

done

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch from a791a6b to 0fcde6a Aug 7, 2018

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Aug 9, 2018

ready for another pass?

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor Author

commented Aug 9, 2018

yes.

// additional secs in hope to find a solution; increase maximum iterations
params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002,
params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50);
}

This comment has been minimized.

Copy link
@mamoll

mamoll Aug 10, 2018

Contributor

Shouldn't the adjustment of parameters happen after the first call to to ChompOptimizer?

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 10, 2018

Author Contributor

the replan_flag is false initially so when ChompOptimizer is first called it uses the default parameters without any adjustment supplied in the chomp_planning.yaml

@@ -303,6 +303,8 @@ bool MoveItConfigData::outputCHOMPPlanningYAML(const std::string& file_path)
emitter << YAML::Key << "collision_clearence" << YAML::Value << "0.2";
emitter << YAML::Key << "collision_threshold" << YAML::Value << "0.07";
emitter << YAML::Key << "use_stochastic_descent" << YAML::Value << "true";
emitter << YAML::Key << "enable_failure_recovery" << YAML::Value << "false";

This comment has been minimized.

Copy link
@mamoll

mamoll Aug 10, 2018

Contributor

Maybe set to true by default, since CHOMP isn't particularly robust.

This comment has been minimized.

Copy link
@raghavendersahdev

raghavendersahdev Aug 10, 2018

Author Contributor

sure will do this.

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch from 7b1117f to 370fb85 Aug 10, 2018

@mamoll

mamoll approved these changes Aug 13, 2018

@raghavendersahdev raghavendersahdev force-pushed the raghavendersahdev:kinetic-devel branch from cfcf650 to e8676ea Aug 17, 2018

@davetcoleman davetcoleman merged commit f5c25c9 into ros-planning:kinetic-devel Aug 22, 2018

1 check passed

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

davetcoleman added a commit that referenced this pull request Aug 22, 2018

Addition of Failure recovery options for CHOMP by tweaking parameters (
…#987)

* implemented failure recovery options by varying CHOMP parameters in hope to find solution

* clag-format apply

* adding 2 additional parameters to the chomp_planning.yaml in the setup assistant

* addressed some of PR requests

* addressed some of PR requested changes

* converted goto statement part to do-while loop

* converted goto to a do-while loop and applied clang-format

* reverted back to the usage of const ChompPatameters params everywhere by using a non constant version of it

* made getParams const again, addressed PR requests

* got rid of getter methods

* edited the name of CHOMP parameter for trajectory_initialization to make more sense

* addressed PR suggested changes

* addressed PR changes- fixed a bug for possible memory leak

* fixed comment

* added doxygen comment for a function

* made chomp_recovery set to true by default
@davetcoleman

This comment has been minimized.

Copy link
Member

commented Aug 22, 2018

Cherry-picked to Melodic

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Aug 22, 2018

Thanks for this contribution!

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.