DynamicsFeedback.hpp

Go to the documentation of this file.

#pragma once

#include <solver_lqr/SolverLqr.hpp>
#include <solver_lqr/OcpDescription.hpp>
#include <momentumopt/dynopt/DynamicsState.hpp>
#include <momentumopt/setting/PlannerSetting.hpp>

namespace momentumopt {

  class DynamicsFeedback : public solverlqr::OcpBase
  {
    public:
      DynamicsFeedback() {}
      ~DynamicsFeedback() {}

      void setPlannerSetting(const PlannerSetting& setting) { setting_ = &setting; }
      void setStatesAndControls(const DynamicsState& ini_state, const DynamicsSequence& dyn_sequence);

    private:
      void configure(const YAML::Node& user_parameters);

      double objective(const solverlqr::StateBase& state, const solverlqr::ControlBase& control, int time_id, bool is_final_timestep);

      solverlqr::StateBase dynamics(const solverlqr::StateBase& state, const solverlqr::ControlBase& control, int time_id);

      inline const PlannerSetting& getSetting() const { return *setting_; }
      inline const DynamicsSequence& dynamicsSequence() const { return *ref_dyn_sequence_; }

    private:
      const PlannerSetting* setting_;

      const DynamicsSequence* ref_dyn_sequence_;

      Eigen::MatrixXd control_cost_, com_tracking_, lmom_tracking_, amom_tracking_,
                      com_final_tracking_, lmom_final_tracking_, amom_final_tracking_;
  };

  /*
   * Wrapper class for easily using DynamicsFeedback within Python
   */
  class DynamicsFeedbackWrapper
  {
    public:
      DynamicsFeedbackWrapper(){}
      ~DynamicsFeedbackWrapper(){}

      void initialize(solverlqr::SolverLqrSetting& lqr_stg, const PlannerSetting& plan_stg);
      void optimize(const DynamicsState& ini_state, const DynamicsSequence& dynamics_sequence);
      Eigen::MatrixXd forceGain(int time_id);

    private:
      const PlannerSetting* plan_stg_;
      solverlqr::SolverLqrSetting* lqr_stg_;

      DynamicsFeedback dynamics_description_;
      solverlqr::SolverLqr dynamics_lqrsolver_;
  };
}