line_search.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_LINE_SEARCH_HPP_
#define ROBOTOC_LINE_SEARCH_HPP_
 
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/utils/aligned_vector.hpp"
#include "robotoc/core/solution.hpp"
#include "robotoc/core/direction.hpp"
#include "robotoc/core/kkt_residual.hpp"
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/ocp/direct_multiple_shooting.hpp"
#include "robotoc/line_search/line_search_filter.hpp"
#include "robotoc/line_search/line_search_settings.hpp"
 
 
namespace robotoc {
 
class LineSearch {
public:
  LineSearch(const OCP& ocp, 
             const LineSearchSettings& settings=LineSearchSettings());
 
  LineSearch();
 
  ~LineSearch() = default;
 
  LineSearch(const LineSearch&) = default;
 
  LineSearch& operator=(const LineSearch&) = default;
 
  LineSearch(LineSearch&&) noexcept = default;
 
  LineSearch& operator=(LineSearch&&) noexcept = default;
 
  double computeStepSize(
      const DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
      const TimeDiscretization& time_discretization,
      const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Solution& s, 
      const Direction& d, const double max_primal_step_size);
 
  void clearHistory();
 
  void set(const LineSearchSettings& settings);
 
  void resizeData(const TimeDiscretization& time_discretization);
 
private:
  LineSearchFilter filter_;
  LineSearchSettings settings_;
  DirectMultipleShooting dms_trial_;
  Solution s_trial_;
  KKTResidual kkt_residual_;
 
  double lineSearchFilterMethod(
      const DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
      const TimeDiscretization& time_discretization,
      const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Solution& s, 
      const Direction& d, const double max_primal_step_size);
 
  double meritBacktrackingLineSearch(
      const DirectMultipleShooting& dms, aligned_vector<Robot>& robots,
      const TimeDiscretization& time_discretization,
      const Eigen::VectorXd& q, const Eigen::VectorXd& v, const Solution& s, 
      const Direction& d, const double max_primal_step_size);
 
  bool armijoCondition(const double merit, const double merit_trial, 
                       const double merit_directional_derivative, 
                       const double step_size) const;
 
  double penaltyParam(const TimeDiscretization time_discretization, 
                      const Solution& s) const;
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_LINE_SEARCH_HPP_