policy_improvement.hpp

/*********************************************************************
 * 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, 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.
 
 \file    policy_improvement.h
 
 \author  Ludovic Righetti, Peter Pastor, Mrinal Kalakrishnan
 \date    May 26, 2010
 
 **********************************************************************/

#ifndef POLICYIMPROVEMENT_H_
#define POLICYIMPROVEMENT_H_

// ros includes
//#include <ros/ros.h>


#include <Eigen/Core>

// local includes
#include "policy.hpp"
#include "task.hpp"

#include "planner/TrajectoryOptim/Chomp/chompMultivariateGaussian.hpp"

#include <boost/shared_ptr.hpp>

extern std::vector<double> global_noiseTrajectory1;
extern std::vector<double> global_noiseTrajectory2;

namespace stomp_motion_planner
{
  
  struct Rollout
  {
    std::vector<Eigen::VectorXd> parameters_;                       
    std::vector<Eigen::VectorXd> noise_;                            
    std::vector<Eigen::VectorXd> noise_projected_;                  
    std::vector<Eigen::VectorXd> parameters_noise_projected_;       
    Eigen::VectorXd state_costs_;                                   
    std::vector<Eigen::VectorXd> control_costs_;                    
    std::vector<Eigen::VectorXd> total_costs_;                      
    std::vector<Eigen::VectorXd> cumulative_costs_;                 
    std::vector<Eigen::VectorXd> probabilities_;                    
    bool out_of_bounds_; 
    double getCost();   
    void printCost();
    void printProbabilities();
  };
  
  class PolicyImprovement
  {
  public:
    PolicyImprovement();
    
    ~PolicyImprovement();
    
    bool initialize(const int num_rollouts, const int num_time_steps, const int num_reused_rollouts,
                    const int num_extra_rollouts, 
                    boost::shared_ptr<stomp_motion_planner::Policy> policy,
                    boost::shared_ptr<stomp_motion_planner::Task>   task,
                    bool use_cumulative_costs=true);
    
    bool setNumRollouts(const int num_rollouts, const int num_reused_rollouts, const int num_extra_rollouts);
    
    bool getRollouts(std::vector<std::vector<Eigen::VectorXd> >& rollouts, const std::vector<double>& noise_stddev,
                     bool get_reused, std::vector<std::vector<Eigen::VectorXd> >& reused_rollouts);
    
    bool setRolloutCosts(const Eigen::MatrixXd& costs, const double control_cost_weight, std::vector<double>& rollout_costs_total);
    
    bool improvePolicy(std::vector<Eigen::MatrixXd>& parameter_updates);
    
    bool addExtraRollouts(std::vector<std::vector<Eigen::VectorXd> >& rollouts, std::vector<Eigen::VectorXd>& rollout_costs);
    
    void testNoiseGenerators();
    
    void setRolloutOutOfBounds(int id, bool out_of_bounds);
    
    bool resetReusedRollouts();
    
    std::vector<Eigen::MatrixXd> projection_matrix_;                        
  private:
    
    bool initialized_;
    
    int num_dimensions_;
    std::vector<int> num_parameters_;
    int num_rollouts_;
    int num_time_steps_;
    int num_rollouts_reused_;
    int num_rollouts_extra_;
    
    bool rollouts_reused_;                                                  
    bool rollouts_reused_next_;                                             
    bool extra_rollouts_added_;                                             
    int num_rollouts_gen_;                                                  
    bool use_multiplication_by_m_;
    bool use_cumulative_costs_;                                             
    boost::shared_ptr<stomp_motion_planner::Policy> policy_;
    boost::shared_ptr<stomp_motion_planner::Task>   task_;
    
    std::vector<Eigen::MatrixXd> control_costs_;                            
    std::vector<Eigen::MatrixXd> inv_control_costs_;                        
    double control_cost_weight_;
    
    std::vector<Eigen::MatrixXd> basis_functions_;                          
    std::vector<Eigen::VectorXd> parameters_;                               
    std::vector<Rollout> rollouts_;
    std::vector<Rollout> reused_rollouts_;
    std::vector<Rollout> extra_rollouts_;
    
    std::vector<MultivariateGaussian> noise_generators_;                    
    std::vector<Eigen::MatrixXd> parameter_updates_;                        
    // temporary variables pre-allocated for efficiency:
    std::vector<Eigen::VectorXd> tmp_noise_;                
    std::vector<Eigen::VectorXd> tmp_parameters_;           
    Eigen::VectorXd tmp_max_cost_;                          
    Eigen::VectorXd tmp_min_cost_;                          
    Eigen::VectorXd tmp_max_minus_min_cost_;                
    Eigen::VectorXd tmp_sum_rollout_probabilities_;         
    std::vector<std::pair<double, int> > rollout_cost_sorter_;  
    bool preAllocateMultivariateGaussianSampler();
    bool preAllocateTempVariables();
    bool preComputeProjectionMatrices();
    
    void resampleUpdates();
    bool computeProjectedNoise();
    bool computeRolloutControlCosts();
    bool computeRolloutCumulativeCosts();
    bool computeRolloutProbabilities();
    bool computeParameterUpdates();
    
    bool computeNoise(Rollout& rollout);
    bool computeProjectedNoise(Rollout& rollout);
    bool computeRolloutControlCosts(Rollout& rollout);
    bool copyParametersFromPolicy();
    
    void addStraightLines( std::vector<int> points, Rollout& rollouts);
    bool generateRollouts(const std::vector<double>& noise_variance);
  };
  
}

#endif /* POLICYIMPROVEMENT_H_ */