impact_constraint_component_base.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_IMPACT_CONSTRAINT_COMPONENT_BASE_HPP_
#define ROBOTOC_IMPACT_CONSTRAINT_COMPONENT_BASE_HPP_
 
#include <memory>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.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/constraint_component_data.hpp"
 
 
namespace robotoc {
 
class ImpactConstraintComponentBase : public std::enable_shared_from_this<ImpactConstraintComponentBase> {
public:
  ImpactConstraintComponentBase(const double barrier=1.0e-03, 
                                const double fraction_to_boundary_rule=0.995);
 
  virtual ~ImpactConstraintComponentBase() {}
 
  ImpactConstraintComponentBase(
      const ImpactConstraintComponentBase&) = default;
 
  ImpactConstraintComponentBase& operator=(
      const ImpactConstraintComponentBase&) = default;
 
  ImpactConstraintComponentBase(
      ImpactConstraintComponentBase&&) noexcept = default;
 
  ImpactConstraintComponentBase& operator=(
      ImpactConstraintComponentBase&&) noexcept = default;
 
  virtual KinematicsLevel kinematicsLevel() const = 0;
 
  virtual void allocateExtraData(ConstraintComponentData& data) const = 0;
 
  virtual bool isFeasible(Robot& robot, const ImpactStatus& impact_status, 
                          ConstraintComponentData& data, 
                          const SplitSolution& s) const = 0;
 
  virtual void setSlack(Robot& robot, const ImpactStatus& impact_status, 
                        ConstraintComponentData& data, 
                        const SplitSolution& s) const = 0;
 
  virtual void evalConstraint(Robot& robot, const ImpactStatus& impact_status, 
                              ConstraintComponentData& data, 
                              const SplitSolution& s) const = 0;
 
  virtual void evalDerivatives(Robot& robot, const ImpactStatus& impact_status, 
                               ConstraintComponentData& data, 
                               const SplitSolution& s,
                               SplitKKTResidual& kkt_residual) const = 0;
 
  virtual void condenseSlackAndDual(
      const ImpactStatus& impact_status, ConstraintComponentData& data, 
      SplitKKTMatrix& kkt_matrix, 
      SplitKKTResidual& kkt_residual) const = 0;
 
  virtual void expandSlackAndDual(const ImpactStatus& impact_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/impact_constraint_component_base.hxx"
 
#endif // ROBOTOC_IMPACT_CONSTRAINT_COMPONENT_BASE_HPP_