added StompSmoothingAdapter #10
Conversation
There was a problem hiding this comment.
@afrixs thank you a lot for this! I've been planning to add this for a while now but never got to do it. I left some comments for implementation details and possible TODOs. I'm willing to merge this shortly after testing.
| @@ -48,15 +48,15 @@ NoiseGeneratorFn get_normal_distribution_generator(size_t num_timesteps, std::ve | |||
| r = std::make_shared<math::MultivariateGaussian>(Eigen::VectorXd::Zero(num_timesteps), covariance); | |||
| } | |||
|
|
|||
| auto raw_noise = std::make_shared<Eigen::VectorXd>(num_timesteps); | |||
| @@ -28,5 +28,8 @@ class StompPlanningContext : public planning_interface::PlanningContext | |||
| private: | |||
| const stomp_moveit::Params params_; | |||
| std::shared_ptr<stomp::Stomp> stomp_; | |||
|
|
|||
| bool extractSeedTrajectory(const planning_interface::MotionPlanRequest &req, | |||
There was a problem hiding this comment.
looks like this could be a free function instead. I'm also thinking about changing this to return std::optional in the future, but this version works for me as well
| @@ -13,16 +13,24 @@ | |||
| #include <stomp_moveit_parameters.hpp> | |||
|
|
|||
| #include <moveit/constraint_samplers/constraint_sampler_manager.h> | |||
| #include <moveit/robot_state/conversions.h> | |||
|
|
|||
| namespace stomp_moveit | |||
| { | |||
| bool solveWithStomp(const std::shared_ptr<stomp::Stomp>& stomp, const moveit::core::RobotState& start_state, | |||
There was a problem hiding this comment.
For readability, I would suggest creating a separate function "adaptWithStomp()" that takes the input trajectory instead of start and goal states.
| const auto config = getStompConfig(params_, group->getActiveJointModels().size() /* num_dimensions */); | ||
| auto config = getStompConfig(params_, group->getActiveJointModels().size() /* num_dimensions */); | ||
| robot_trajectory::RobotTrajectoryPtr input_trajectory; | ||
| if (extractSeedTrajectory(request_, input_trajectory) && !input_trajectory->empty()) |
There was a problem hiding this comment.
We don't have to implement this now, but it would be great if you could leave a note that failed seed trajectories should be handled in a special way, and not result in a fresh motion plan.
| { | ||
| } | ||
|
|
||
| // todo[noetic] add override again |
There was a problem hiding this comment.
| // todo[noetic] add override again |
|
|
||
| // STOMP reads the seed trajectory from trajectory constraints so we need to convert the waypoints first | ||
| const size_t seed_waypoint_count = res.trajectory->getWayPointCount(); | ||
| const std::vector<std::string> variable_names = |
There was a problem hiding this comment.
just noticed that we have been iterating over all variables. We should probably switch this to active variables only, that's also what the extract function and STOMP use. Groups with mimic joints will probably just produce an error. A note/TODO on that would be fine for now, though.
|
Formatting is not happy. We are using pre-commit, here are some instructions on how to fix that locally https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks |
|
Sorry for not being active yesterday, my daughter was being born, but thank you for finishing these PRs! |
noise_generatorsto work with flexible trajectory size