Compute the rollouts state costs in parallel using OpenMP. #59
Conversation
|
Is it still possible to cancel out of an ongoing planning computation? |
|
I am not sure. |
|
This looks like a great optimization contribution, can it be wrapped? @raghavendersahdev is trying to migrate more stomp integration into the main moveit and we want to port the stomp_moveit plugin over |
| computeTwist(last_tool_pose,tool_goal_pose_,dof_nullity_,tool_twist_error); | ||
|
|
||
| double pos_error = tool_twist_error.segment(0,3).norm(); | ||
| double orientation_error = tool_twist_error.segment(3,3).norm(); |
There was a problem hiding this comment.
The original members that these local variables (pos_error and orientation_error) replace were initialized from yaml through ros parameters. These local variable values are never set and so the result is probably incorrect
| for(auto r = 0u ; r < config_.num_rollouts; r++) | ||
| { | ||
| if(!proceed_) | ||
| if(proceed) |
There was a problem hiding this comment.
The proceed variable is never changed and so there is no point in checking it. The original code changed the proceed_ atomic boolean member from outside the function so that the optimization could be stopped upon user request. This change won't allow to cancel out of an ongoing plan.
| @@ -109,7 +109,7 @@ class StompCostFunction | |||
| int iteration_number, | |||
| int rollout_number, | |||
| Eigen::VectorXd& costs, | |||
| bool& validity) = 0 ; | |||
| bool& validity) const = 0 ; | |||
There was a problem hiding this comment.
Too bad OpenMP requires using const functions, this doesn't allow the use of class members and in this case a couple of public interface methods need to be modified.
| {0 , 0 , 0 , 1 , 0 , 0 , 0 }, // position | ||
| {0 , 1/12.0 , -2/3.0 , 0 , 2/3.0 , -1/12.0 , 0 }, // velocity | ||
| {0 , -1/12.0 , 16/12.0 , -30/12.0 , 16/12.0 , -1/12.0 , 0 }, // acceleration (five point stencil) | ||
| {1/8.0, -1 , 13/8.0 , 0 , -13/8.0 , 1 , -1/8.0 } // jerk |
There was a problem hiding this comment.
Wow that last row was so off! glad you caught that
There was a problem hiding this comment.
This should still be applied, regardless of OpenMP right?
|
I really welcome these changes, however OpenMP's coding rigidity seems to make it impossible to track a class member from within the parallelized methods, this means that it won't be possible to stop the planner while it's planning. Anyone knows any workarounds for this? |
No description provided.