DdpZmp.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <nmpc_ddp/DDPSolver.h>
 
namespace CCC
{
class DdpZmp
{
public:
  struct RefData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector3d zmp = Eigen::Vector3d::Zero();
 
    double com_z = 0;
  };
 
  struct PlannedData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d zmp = Eigen::Vector2d::Zero();
 
    double force_z = 0;
  };
 
  struct WeightParam
  {
    double running_com_pos_z;
 
    double running_zmp;
 
    double running_force_z;
 
    double terminal_com_pos_xy;
 
    double terminal_com_pos_z;
 
    double terminal_com_vel;
 
    WeightParam(double _running_com_pos_z = 1e2,
                double _running_zmp = 1e-1,
                double _running_force_z = 1e-4,
                double _terminal_com_pos_xy = 1.0,
                double _terminal_com_pos_z = 1e2,
                double _terminal_com_vel = 1.0)
    : running_com_pos_z(_running_com_pos_z), running_zmp(_running_zmp), running_force_z(_running_force_z),
      terminal_com_pos_xy(_terminal_com_pos_xy), terminal_com_pos_z(_terminal_com_pos_z),
      terminal_com_vel(_terminal_com_vel)
    {
    }
  };
 
  class DdpProblem : public nmpc_ddp::DDPProblem<6, 3>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    DdpProblem(double horizon_dt, double mass, const WeightParam & weight_param)
    : nmpc_ddp::DDPProblem<6, 3>(horizon_dt), mass_(mass), weight_param_(weight_param)
    {
    }
 
    void setRefDataFunc(const std::function<RefData(double)> & ref_data_func)
    {
      ref_data_func_ = ref_data_func;
    }
 
    virtual StateDimVector stateEq(double t, const StateDimVector & x, const InputDimVector & u) const override;
 
    virtual double runningCost(double t, const StateDimVector & x, const InputDimVector & u) const override;
 
    virtual double terminalCost(double t, const StateDimVector & x) const override;
 
    virtual void calcStateEqDeriv(double t,
                                  const StateDimVector & x,
                                  const InputDimVector & u,
                                  Eigen::Ref<StateStateDimMatrix> state_eq_deriv_x,
                                  Eigen::Ref<StateInputDimMatrix> state_eq_deriv_u) const override;
 
    inline virtual void calcStateEqDeriv(double, // t
                                         const StateDimVector &, // x
                                         const InputDimVector &, // u
                                         Eigen::Ref<StateStateDimMatrix>, // state_eq_deriv_x
                                         Eigen::Ref<StateInputDimMatrix>, // state_eq_deriv_u
                                         std::vector<StateStateDimMatrix> &, // state_eq_deriv_xx
                                         std::vector<InputInputDimMatrix> &, // state_eq_deriv_uu
                                         std::vector<StateInputDimMatrix> & // state_eq_deriv_xu
    ) const override
    {
      throw std::runtime_error("Second-order derivatives of state equation are not implemented.");
    }
 
    virtual void calcRunningCostDeriv(double t,
                                      const StateDimVector & x,
                                      const InputDimVector & u,
                                      Eigen::Ref<StateDimVector> running_cost_deriv_x,
                                      Eigen::Ref<InputDimVector> running_cost_deriv_u) const override;
 
    virtual void calcRunningCostDeriv(double t,
                                      const StateDimVector & x,
                                      const InputDimVector & u,
                                      Eigen::Ref<StateDimVector> running_cost_deriv_x,
                                      Eigen::Ref<InputDimVector> running_cost_deriv_u,
                                      Eigen::Ref<StateStateDimMatrix> running_cost_deriv_xx,
                                      Eigen::Ref<InputInputDimMatrix> running_cost_deriv_uu,
                                      Eigen::Ref<StateInputDimMatrix> running_cost_deriv_xu) const override;
 
    virtual void calcTerminalCostDeriv(double t,
                                       const StateDimVector & x,
                                       Eigen::Ref<StateDimVector> terminal_cost_deriv_x) const override;
 
    virtual void calcTerminalCostDeriv(double t,
                                       const StateDimVector & x,
                                       Eigen::Ref<StateDimVector> terminal_cost_deriv_x,
                                       Eigen::Ref<StateStateDimMatrix> terminal_cost_deriv_xx) const override;
 
  protected:
    double mass_;
 
    WeightParam weight_param_;
 
    std::function<RefData(double)> ref_data_func_;
  };
 
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d vel = Eigen::Vector3d::Zero();
 
    std::vector<DdpProblem::InputDimVector> u_list = {};
 
    DdpProblem::StateDimVector toState() const;
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  DdpZmp(double mass, double horizon_dt, int horizon_steps, const WeightParam & weight_param = WeightParam())
  : ddp_problem_(std::make_shared<DdpProblem>(horizon_dt, mass, weight_param)),
    ddp_solver_(std::make_shared<nmpc_ddp::DDPSolver<6, 3>>(ddp_problem_))
  {
    ddp_solver_->config().horizon_steps = horizon_steps;
  }
 
  PlannedData planOnce(const std::function<RefData(double)> & ref_data_func,
                       const InitialParam & initial_param,
                       double current_time);
 
public:
  std::shared_ptr<DdpProblem> ddp_problem_;
 
  std::shared_ptr<nmpc_ddp::DDPSolver<6, 3>> ddp_solver_;
};
} // namespace CCC