covariant_trajectory_policy.hpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, 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 the Willow Garage 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.
 *********************************************************************/

#ifndef COVARIANT_TRAJECTORY_POLICY_H_
#define COVARIANT_TRAJECTORY_POLICY_H_

#include <Eigen/Core>
#include "planner/TrajectoryOptim/Chomp/chompUtils.hpp"
#include "policy.hpp"

namespace stomp_motion_planner
{
class CovariantTrajectoryPolicy: public Policy
{
public:
    CovariantTrajectoryPolicy();
    virtual ~CovariantTrajectoryPolicy();


    // Functions that are specific to CovariantTrajectoryPolicy:
    bool initialize(/*ros::NodeHandle& node_handle*/);
    bool initialize(/*ros::NodeHandle& node_handle,*/
                                               const int num_time_steps,
                                               const int num_dimensions,
                                               const double movement_duration,
                                               const double cost_ridge_factor,
                                               const std::vector<double>& derivative_costs);
  
    bool setToMinControlCost(Eigen::VectorXd& start, Eigen::VectorXd& goal);
    bool getParametersAll(std::vector<Eigen::VectorXd>& parameters);
    void setFileNameBase(const std::string& file_name_base);
    void setPrintDebug(bool print_debug) { print_debug_ = print_debug; }

    // Functions inherited from Policy:

    bool setNumTimeSteps(const int num_time_steps);

    bool getNumTimeSteps(int& num_time_steps);

    bool getNumDimensions(int& num_dimensions);

    bool getNumParameters(std::vector<int>& num_params);

    bool getBasisFunctions(std::vector<Eigen::MatrixXd>& basis_functions);

    bool getControlCosts(std::vector<Eigen::MatrixXd>& control_costs);

    bool updateParameters(const std::vector<Eigen::MatrixXd>& updates);
  
    void printParameters();

    bool getParameters(std::vector<Eigen::VectorXd>& parameters);

    bool setParameters(const std::vector<Eigen::VectorXd>& parameters);

    bool computeControlCosts(const std::vector<Eigen::MatrixXd>& control_cost_matrices, const std::vector<std::vector<Eigen::VectorXd> >& parameters,
                                     const double weight, std::vector<Eigen::VectorXd>& control_costs);

    bool computeControlCosts(const std::vector<Eigen::MatrixXd>& control_cost_matrices, const std::vector<Eigen::VectorXd>& parameters,
                             const std::vector<Eigen::VectorXd>& noise, const double weight, std::vector<Eigen::VectorXd>& control_costs);

    // Functions inherited from LibraryItem:

    std::string getInfoString();
    std::string getClassName();
    bool readFromDisc(const std::string directory_name, const int item_id, const int trial_id = 0);
    bool writeToDisc(const int trial_id = 0);
    bool readFromDisc(const std::string abs_file_name);
    bool writeToDisc(const std::string abs_file_name);
    std::string getFileName(const int trial_id);

    double movement_dt_;

private:
//    ros::NodeHandle node_handle_;

    std::string file_name_base_;
    bool print_debug_;

    int num_time_steps_;
    int num_vars_free_;
    int num_vars_all_;
    int free_vars_start_index_;
    int free_vars_end_index_;
    int num_dimensions_;
    double movement_duration_;
    double cost_ridge_factor_;
    std::vector<double> derivative_costs_;

    std::vector<int> num_parameters_;
    std::vector<Eigen::MatrixXd> basis_functions_;
    std::vector<Eigen::MatrixXd> control_costs_;
    std::vector<Eigen::MatrixXd> inv_control_costs_;
    std::vector<Eigen::MatrixXd> control_costs_all_;

    std::vector<Eigen::VectorXd> linear_control_costs_;

    std::vector<Eigen::VectorXd> parameters_all_;

    std::vector<Eigen::MatrixXd> differentiation_matrices_;
    void createDifferentiationMatrices();
    bool readParameters();
    bool initializeVariables();
    bool initializeCosts();
    bool initializeBasisFunctions();

    bool computeLinearControlCosts();
    bool computeMinControlCostParameters();
};

// inline functions follow

inline bool CovariantTrajectoryPolicy::getParameters(std::vector<Eigen::VectorXd>& parameters)
{
    if (int(parameters.size()) != num_dimensions_)
    {
        parameters.resize(num_dimensions_, Eigen::VectorXd::Zero(num_time_steps_));
    }
    for (int d=0; d<num_dimensions_; ++d)
    {
        parameters[d] = parameters_all_[d].segment(free_vars_start_index_, num_vars_free_);
    }
    return true;
}
  
inline void CovariantTrajectoryPolicy::printParameters()
{
    Eigen::MatrixXd parameters( num_time_steps_, num_dimensions_);
    
    for (int d=0; d<num_dimensions_; ++d)
    {
      parameters.col(d) = parameters_all_[d].segment(free_vars_start_index_, num_vars_free_);
    }
    
    std::cout << parameters << std::endl;
}

inline bool CovariantTrajectoryPolicy::getParametersAll(std::vector<Eigen::VectorXd>& parameters)
{
    parameters = parameters_all_;
    return true;
}

inline bool CovariantTrajectoryPolicy::setParameters(const std::vector<Eigen::VectorXd>& parameters)
{
    //ROS_ASSERT(int(parameters.size()) == num_dimensions_);
    for (int d=0; d<num_dimensions_; ++d)
    {
        parameters_all_[d].segment(free_vars_start_index_, num_vars_free_) = parameters[d];
    }
    //std::cout << "CovariantTrajectoryPolicy::setParameters => num_vars_free_ : " << num_vars_free_ << std::endl;
    return true;
}

inline bool CovariantTrajectoryPolicy::getBasisFunctions(std::vector<Eigen::MatrixXd>& basis_functions)
{
    basis_functions = basis_functions_;
    return true;
}

inline bool CovariantTrajectoryPolicy::getControlCosts(std::vector<Eigen::MatrixXd>& control_costs)
{
    control_costs = control_costs_;
    return true;
}

inline bool CovariantTrajectoryPolicy::getNumTimeSteps(int& num_time_steps)
{
    num_time_steps = num_time_steps_;
    return true;
}

inline bool CovariantTrajectoryPolicy::getNumDimensions(int& num_dimensions)
{
    num_dimensions = num_dimensions_;
    return true;
}

inline bool CovariantTrajectoryPolicy::getNumParameters(std::vector<int>& num_params)
{
    num_params = num_parameters_;
    return true;
}

inline bool CovariantTrajectoryPolicy::setNumTimeSteps(const int num_time_steps)
{
    //ROS_ASSERT_MSG(num_time_steps_ == num_time_steps, "%d != %d", num_time_steps_, num_time_steps);
    return true;
}

inline std::string CovariantTrajectoryPolicy::getInfoString()
{
    return "";
}

inline std::string CovariantTrajectoryPolicy::getClassName()
{
    return "CovariantTrajectoryPolicy";
}

inline void CovariantTrajectoryPolicy::setFileNameBase(const std::string& file_name_base)
{
    file_name_base_ = file_name_base;
}


}

#endif /* COVARIANT_TRAJECTORY_POLICY_H_ */