switching_time_optimization.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_SWITCHING_TIME_OPTIMIZATION_HPP_
#define ROBOTOC_SWITCHING_TIME_OPTIMIZATION_HPP_
 
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/core/performance_index.hpp"
#include "robotoc/core/kkt_residual.hpp"
#include "robotoc/core/kkt_matrix.hpp"
#include "robotoc/planner/contact_sequence.hpp"
#include "robotoc/ocp/time_discretization.hpp"
#include "robotoc/sto/sto_cost_function.hpp"
#include "robotoc/sto/sto_constraints.hpp"
 
 
namespace robotoc {
 
class SwitchingTimeOptimization {
public:
  SwitchingTimeOptimization(const OCP& ocp);
 
  SwitchingTimeOptimization();
 
  ~SwitchingTimeOptimization() = default;
 
  SwitchingTimeOptimization(const SwitchingTimeOptimization&) = default;
 
  SwitchingTimeOptimization& operator=(const SwitchingTimeOptimization&) = default;
 
  SwitchingTimeOptimization(SwitchingTimeOptimization&&) noexcept = default;
 
  SwitchingTimeOptimization& operator=(SwitchingTimeOptimization&&) noexcept = default;
 
  void setRegularization(const double sto_reg);
 
  void initConstraints(const TimeDiscretization& time_discretization);
 
  bool isFeasible(const TimeDiscretization& time_discretization);
 
  void evalSTO(const TimeDiscretization& time_discretization);
 
  void evalKKT(const TimeDiscretization& time_discretization, 
               KKTMatrix& kkt_matrix, KKTResidual& kkt_residual);
 
  const PerformanceIndex& getEval() const;
 
  void computeStepSizes(const TimeDiscretization& time_discretization,
                        const Direction& d);
 
  double maxPrimalStepSize() const;
 
  double maxDualStepSize() const;
 
  void integrateSolution(const TimeDiscretization& time_discretization, 
                         const double primal_step_size,
                         const double dual_step_size, const Direction& d);
 
private:
  std::shared_ptr<STOCostFunction> sto_cost_;
  std::shared_ptr<STOConstraints> sto_constraints_;
  std::shared_ptr<ContactSequence> contact_sequence_;
  double sto_reg_;
  bool is_sto_enabled_;
  PerformanceIndex performance_index_;
  Eigen::VectorXd lt_, h_, dts_;
  Eigen::MatrixXd Qtt_;
  ConstraintComponentData constraint_data_;
};
 
} // namespace robotoc
 
#endif // ROBOTOC_SWITCHING_TIME_OPTIMIZATION_HPP_