stomp_moveit_task.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2023, PickNik Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of PickNik Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
#pragma once
 
#include <stomp/task.h>
 
namespace stomp_moveit
{
// @brief A function that computes randomized noisy values for input trajectory values.
using NoiseGeneratorFn =
    std::function<bool(const Eigen::MatrixXd& values, Eigen::MatrixXd& noisy_values, Eigen::MatrixXd& noise)>;
// @brief A function that computes cost values and a validity result for input trajectory values
using CostFn = std::function<bool(const Eigen::MatrixXd& values, Eigen::VectorXd& costs, bool& validity)>;
// @brief A function that applies a filter (smoothing, clamping, ...) of an input trajectory
using FilterFn = std::function<bool(const Eigen::MatrixXd& values, Eigen::MatrixXd& filtered_values)>;
// @brief A function that is called with every iteration to report on the current trajectory candidate and cost
using PostIterationFn = std::function<void(int iteration_number, double cost, const Eigen::MatrixXd& values)>;
// @brief A function that is called when STOMP terminates, providing success flag, cost and solution trajectory
using DoneFn =
    std::function<void(bool success, int total_iterations, double final_cost, const Eigen::MatrixXd& values)>;
 
// @brief A STOMP task that allows injecting generic callbacks for a STOMP planning instance
//
// The ComposableTask stores custom functions for the most important callback types in STOMP and applies them during
// a motion planning run. This class is used for injecting MoveIt concepts and other custom features into STOMP.
class ComposableTask final : public stomp::Task
{
public:
  ComposableTask(NoiseGeneratorFn noise_generator_fn, CostFn cost_fn, FilterFn filter_fn,
                 PostIterationFn post_iteration_fn, DoneFn done_fn)
    : noise_generator_fn_(std::move(noise_generator_fn))
    , cost_fn_(std::move(cost_fn))
    , filter_fn_(std::move(filter_fn))
    , post_iteration_fn_(std::move(post_iteration_fn))
    , done_fn_(std::move(done_fn))
  {
  }
 
  ~ComposableTask() = default;
 
  bool generateNoisyParameters(const Eigen::MatrixXd& parameters, std::size_t /*start_timestep*/,
                               std::size_t /*num_timesteps*/, int /*iteration_number*/, int /*rollout_number*/,
                               Eigen::MatrixXd& parameters_noise, Eigen::MatrixXd& noise) override
  {
    return noise_generator_fn_(parameters, parameters_noise, noise);
  }
 
  bool computeCosts(const Eigen::MatrixXd& parameters, std::size_t /*start_timestep*/, std::size_t /*num_timesteps*/,
                    int /*iteration_number*/, Eigen::VectorXd& costs, bool& validity) override
  {
    return cost_fn_(parameters, costs, validity);
  }
 
  bool computeNoisyCosts(const Eigen::MatrixXd& parameters, std::size_t /*start_timestep*/,
                         std::size_t /*num_timesteps*/, int /*iteration_number*/, int /*rollout_number*/,
                         Eigen::VectorXd& costs, bool& validity) override
  {
    return cost_fn_(parameters, costs, validity);
  }
 
  bool filterParameterUpdates(std::size_t /*start_timestep*/, std::size_t /*num_timesteps*/, int /*iteration_number*/,
                              const Eigen::MatrixXd& parameters, Eigen::MatrixXd& updates) override
  {
    return filter_fn_(parameters, updates);
  }
 
  void postIteration(std::size_t /*start_timestep*/, std::size_t /*num_timesteps*/, int iteration_number, double cost,
                     const Eigen::MatrixXd& parameters) override
  {
    post_iteration_fn_(iteration_number, cost, parameters);
  }
 
  void done(bool success, int total_iterations, double final_cost, const Eigen::MatrixXd& parameters) override
  {
    done_fn_(success, total_iterations, final_cost, parameters);
  }
 
private:
  NoiseGeneratorFn noise_generator_fn_;
  CostFn cost_fn_;
  FilterFn filter_fn_;
  PostIterationFn post_iteration_fn_;
  DoneFn done_fn_;
};
}  // namespace stomp_moveit