sto_constraints.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_STO_CONSTRAINTS_HPP_
#define ROBOTOC_STO_CONSTRAINTS_HPP_
 
#include <limits>
#include <cmath>
#include <vector>
 
#include "Eigen/Core"
 
#include "robotoc/ocp/time_discretization.hpp"
#include "robotoc/core/kkt_residual.hpp"
#include "robotoc/core/kkt_matrix.hpp"
#include "robotoc/core/direction.hpp"
#include "robotoc/constraints/constraint_component_data.hpp"
 
 
namespace robotoc {
 
class STOConstraints {
public:
  STOConstraints(const int num_switches, 
                 const double minimum_dwell_time=std::sqrt(std::numeric_limits<double>::epsilon()),
                 const double barrier_param=1.0e-03, 
                 const double fraction_to_boundary_rule=0.995);
 
  STOConstraints(const std::vector<double>& minimum_dwell_times,
                 const double barrier_param=1.0e-03, 
                 const double fraction_to_boundary_rule=0.995);
 
  STOConstraints(const Eigen::VectorXd& minimum_dwell_times,
                 const double barrier_param=1.0e-03, 
                 const double fraction_to_boundary_rule=0.995);
 
  STOConstraints();
 
  ~STOConstraints() = default;
 
  STOConstraints(const STOConstraints&) = default;
 
  STOConstraints& operator=(const STOConstraints&) = default;
 
  STOConstraints(STOConstraints&&) noexcept = default;
 
  STOConstraints& operator=(STOConstraints&&) noexcept = default;
 
  void setMinimumDwellTimes(
      const double minimum_dwell_time=std::sqrt(std::numeric_limits<double>::epsilon()));
 
  void setMinimumDwellTimes(const std::vector<double>& minimum_dwell_times);
 
  void setMinimumDwellTimes(const Eigen::VectorXd& minimum_dwell_times);
 
  const Eigen::VectorXd& getMinimumDwellTimes() const;
 
  void setBarrierParam(const double barrier_param);
 
  void setFractionToBoundaryRule(const double fraction_to_boundary_rule);
 
  double getBarrierParam() const;
 
  double getFractionToBoundaryRule() const;
 
  ConstraintComponentData createConstraintsData(
      const TimeDiscretization& time_discretization) const;
 
  bool isFeasible(const TimeDiscretization& time_discretization,
                  ConstraintComponentData& data) const;
 
  void setSlackAndDual(const TimeDiscretization& time_discretization,
                       ConstraintComponentData& data) const;
 
  void evalConstraint(const TimeDiscretization& time_discretization, 
                      ConstraintComponentData& data) const;
 
  void linearizeConstraints(const TimeDiscretization& time_discretization, 
                            ConstraintComponentData& data, 
                            Eigen::VectorXd& lt) const;
 
  void condenseSlackAndDual(ConstraintComponentData& data, Eigen::VectorXd& lt,
                            Eigen::MatrixXd& Qtt) const;
 
  void expandSlackAndDual(ConstraintComponentData& data, Eigen::VectorXd& dts) const;
 
  double maxSlackStepSize(const ConstraintComponentData& data) const;
 
  double maxDualStepSize(const ConstraintComponentData& data) const;
 
  void updateSlack(ConstraintComponentData& data, const double step_size) const;
 
  void updateDual(ConstraintComponentData& data, const double step_size) const;
 
  static void computeDwellTimes(const TimeDiscretization& time_discretization,
                                Eigen::VectorXd& dwell_times);
 
private:
  double barrier_, fraction_to_boundary_rule_;
  int num_switches_;
 
  Eigen::VectorXd primal_step_size_, dual_step_size_;
  Eigen::VectorXd minimum_dwell_times_;
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_STO_CONSTRAINTS_HPP_