constraint_component_base.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_CONSTRAINT_COMPONENT_BASE_HPP_
#define ROBOTOC_CONSTRAINT_COMPONENT_BASE_HPP_
 
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_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_data.hpp"
 
 
namespace robotoc {
 
enum class KinematicsLevel {
  PositionLevel,
  VelocityLevel,
  AccelerationLevel
};
 
class ConstraintComponentBase : public std::enable_shared_from_this<ConstraintComponentBase> {
public:
  ConstraintComponentBase(const double barrier_param=1.0e-03, 
                          const double fraction_to_boundary_rule=0.995);
 
  virtual ~ConstraintComponentBase() {}
 
  ConstraintComponentBase(const ConstraintComponentBase&) = default;
 
  ConstraintComponentBase& operator=(const ConstraintComponentBase&) = default;
 
  ConstraintComponentBase(ConstraintComponentBase&&) noexcept = default;
 
  ConstraintComponentBase& operator=(ConstraintComponentBase&&) noexcept = default;
 
  virtual KinematicsLevel kinematicsLevel() const = 0;
 
  virtual void allocateExtraData(ConstraintComponentData& data) const = 0;
 
  virtual bool isFeasible(Robot& robot, const ContactStatus& contact_status, 
                          ConstraintComponentData& data, 
                          const SplitSolution& s) const = 0;
 
  virtual void setSlack(Robot& robot, const ContactStatus& contact_status, 
                        ConstraintComponentData& data, 
                        const SplitSolution& s) const = 0;
 
  virtual void evalConstraint(Robot& robot, const ContactStatus& contact_status, 
                              ConstraintComponentData& data, 
                              const SplitSolution& s) const = 0;
 
  virtual void evalDerivatives(Robot& robot, const ContactStatus& contact_status, 
                               ConstraintComponentData& data, 
                               const SplitSolution& s, 
                               SplitKKTResidual& kkt_residual) const = 0;
 
  virtual void condenseSlackAndDual(const ContactStatus& contact_status,
                                    ConstraintComponentData& data,
                                    SplitKKTMatrix& kkt_matrix,
                                    SplitKKTResidual& kkt_residual) const = 0;
 
  virtual void expandSlackAndDual(const ContactStatus& contact_status, 
                                  ConstraintComponentData& data, 
                                  const SplitDirection& d) const = 0;
 
  virtual int dimc() const = 0;
 
  void setSlackAndDualPositive(ConstraintComponentData& data) const;
 
  double maxSlackStepSize(const ConstraintComponentData& data) const;
 
  double maxDualStepSize(const ConstraintComponentData& data) const;
 
  static void updateSlack(ConstraintComponentData& data, const double step_size);
 
  static void updateDual(ConstraintComponentData& data, const double step_size);
 
  virtual double getBarrierParam() const final;
 
  virtual double getFractionToBoundaryRule() const final;
 
  virtual void setBarrierParam(const double barrier_param) final;
 
  virtual void setFractionToBoundaryRule(
      const double fraction_to_boundary_rule) final;
 
  template <typename Derived>
  std::shared_ptr<Derived> as_shared_ptr();
 
protected:
  void computeComplementarySlackness(ConstraintComponentData& data) const;
 
  void computeComplementarySlackness(ConstraintComponentData& data,
                                     const int start, 
                                     const int size) const;
 
  template <int Size>
  void computeComplementarySlackness(ConstraintComponentData& data,
                                     const int start) const;
 
  double computeComplementarySlackness(const double slack, 
                                       const double dual) const;
 
  static void computeCondensingCoeffcient(ConstraintComponentData& data);
 
  static void computeCondensingCoeffcient(ConstraintComponentData& data,
                                          const int start, const int size);
 
  template <int Size>
  static void computeCondensingCoeffcient(ConstraintComponentData& data,
                                          const int start);
 
  static double computeCondensingCoeffcient(const double slack, 
                                            const double dual, 
                                            const double residual, 
                                            const double cmpl);
 
  static void computeDualDirection(ConstraintComponentData& data);
 
  static void computeDualDirection(ConstraintComponentData& data, 
                                   const int start, const int size);
 
  template <int Size>
  static void computeDualDirection(ConstraintComponentData& data, 
                                   const int start);
 
  static double computeDualDirection(const double slack, const double dual,
                                     const double dslack, const double cmpl);
 
  template <typename VectorType>
  double logBarrier(const Eigen::MatrixBase<VectorType>& slack) const;
 
private:
  double barrier_, fraction_to_boundary_rule_;
 
};
 
} // namespace robotoc
 
#include "robotoc/constraints/constraint_component_base.hxx"
 
#endif // ROBOTOC_CONSTRAINT_COMPONENT_BASE_HPP_