constraints.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_CONSTRAINTS_HPP_
#define ROBOTOC_CONSTRAINTS_HPP_
 
#include <vector>
#include <memory>
#include <unordered_map>
#include <iostream>
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/robot/impact_status.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/constraints/constraint_component_base.hpp"
#include "robotoc/constraints/impact_constraint_component_base.hpp"
#include "robotoc/constraints/constraint_component_data.hpp"
#include "robotoc/constraints/constraints_data.hpp"
 
 
namespace robotoc {
 
class Constraints {
public:
   using ConstraintComponentBasePtr = std::shared_ptr<ConstraintComponentBase>;
   using ImpactConstraintComponentBasePtr 
      = std::shared_ptr<ImpactConstraintComponentBase>;
 
  Constraints(const double barrier_param=1.0e-03, 
              const double fraction_to_boundary_rule=0.995);
 
  ~Constraints() = default;
 
  Constraints(const Constraints&) = default;
 
  Constraints& operator=(const Constraints&) = default;
 
  Constraints(Constraints&&) noexcept = default;
 
  Constraints& operator=(Constraints&&) noexcept = default;
 
  bool exist(const std::string& name) const;
 
  void add(const std::string& name, ConstraintComponentBasePtr constraint);
 
  void add(const std::string& name, ImpactConstraintComponentBasePtr constraint);
 
  void erase(const std::string& name);
 
  ConstraintComponentBasePtr getConstraintComponent(const std::string& name) const;
 
  ImpactConstraintComponentBasePtr getImpactConstraintComponent(const std::string& name) const;
 
  void clear();
 
  ConstraintsData createConstraintsData(const Robot& robot, 
                                        const int time_stage=-1) const;
 
  bool isFeasible(Robot& robot, const ContactStatus& contact_status,
                  ConstraintsData& data, const SplitSolution& s) const;
 
  bool isFeasible(Robot& robot, const ImpactStatus& impact_status, 
                  ConstraintsData& data, const SplitSolution& s) const;
 
  void setSlackAndDual(Robot& robot, const ContactStatus& contact_status, 
                       ConstraintsData& data, const SplitSolution& s) const;
 
  void setSlackAndDual(Robot& robot, const ImpactStatus& impact_status, 
                       ConstraintsData& data, 
                       const SplitSolution& s) const;
 
  void evalConstraint(Robot& robot, const ContactStatus& contact_status, 
                      ConstraintsData& data, const SplitSolution& s) const;
 
  void evalConstraint(Robot& robot, const ImpactStatus& impact_status, 
                      ConstraintsData& data, 
                      const SplitSolution& s) const;
 
  void linearizeConstraints(Robot& robot, const ContactStatus& contact_status, 
                            ConstraintsData& data, const SplitSolution& s, 
                            SplitKKTResidual& kkt_residual) const;
 
  void linearizeConstraints(Robot& robot, const ImpactStatus& impact_status, 
                            ConstraintsData& data, const SplitSolution& s,
                            SplitKKTResidual& kkt_residual) const;
 
  void condenseSlackAndDual(const ContactStatus& contact_status, 
                            ConstraintsData& data, SplitKKTMatrix& kkt_matrix, 
                            SplitKKTResidual& kkt_residual) const;
 
  void condenseSlackAndDual(const ImpactStatus& impact_status, 
                            ConstraintsData& data, 
                            SplitKKTMatrix& kkt_matrix, 
                            SplitKKTResidual& kkt_residual) const;
 
  void expandSlackAndDual(const ContactStatus& contact_status, 
                          ConstraintsData& data, const SplitDirection& d) const;
 
  void expandSlackAndDual(const ImpactStatus& impact_status, 
                          ConstraintsData& data, 
                          const SplitDirection& d) const;
 
  double maxSlackStepSize(const ConstraintsData& data) const;
 
  double maxDualStepSize(const ConstraintsData& data) const;
 
  static void updateSlack(ConstraintsData& data, const double step_size);
 
  static void updateDual(ConstraintsData& data, const double step_size);
 
  void setBarrierParam(const double barrier_param);
 
  void setFractionToBoundaryRule(const double fraction_to_boundary_rule);
 
  double getBarrierParam() const;
 
  double getFractionToBoundaryRule() const;
 
  std::vector<std::string> getPositionLevelConstraintList() const;
 
  std::vector<std::string> getVelocityLevelConstraintList() const;
 
  std::vector<std::string> getAccelerationLevelConstraintList() const;
 
  std::vector<std::string> getImpactLevelConstraintList() const;
 
  std::vector<std::string> getConstraintList() const;
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, const Constraints& constraints);
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const std::shared_ptr<Constraints>& constraints);
 
private:
  std::vector<ConstraintComponentBasePtr> position_level_constraints_, 
                                          velocity_level_constraints_, 
                                          acceleration_level_constraints_;
  std::vector<ImpactConstraintComponentBasePtr> impact_level_constraints_;
  std::unordered_map<std::string, size_t> position_level_constraint_names_, 
                                          velocity_level_constraint_names_, 
                                          acceleration_level_constraint_names_,
                                          impact_level_constraint_names_;
  double barrier_, fraction_to_boundary_rule_;
};
 
} // namespace robotoc
 
#endif // ROBOTOC_CONSTRAINTS_HPP_