DdpSingleRigidBody.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <nmpc_ddp/DDPSolver.h>
 
namespace ForceColl
{
class Contact;
}
 
namespace CCC
{
class DdpSingleRigidBody
{
public:
  struct MotionParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    std::vector<std::shared_ptr<ForceColl::Contact>> contact_list;
 
    Eigen::Matrix3d inertia_mat = Eigen::Matrix3d::Identity();
  };
 
  struct RefData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d ori = Eigen::Vector3d::Zero();
  };
 
  struct WeightParam
  {
    Eigen::Vector3d running_pos;
 
    Eigen::Vector3d running_ori;
 
    Eigen::Vector3d running_linear_vel;
 
    Eigen::Vector3d running_angular_vel;
 
    double running_force;
 
    Eigen::Vector3d terminal_pos;
 
    Eigen::Vector3d terminal_ori;
 
    Eigen::Vector3d terminal_linear_vel;
 
    Eigen::Vector3d terminal_angular_vel;
 
    WeightParam(const Eigen::Vector3d & _running_pos = Eigen::Vector3d::Constant(1.0),
                const Eigen::Vector3d & _running_ori = Eigen::Vector3d::Constant(1.0),
                const Eigen::Vector3d & _running_linear_vel = Eigen::Vector3d::Constant(0.01),
                const Eigen::Vector3d & _running_angular_vel = Eigen::Vector3d::Constant(0.01),
                double _running_force = 1e-6,
                const Eigen::Vector3d & _terminal_pos = Eigen::Vector3d::Constant(1.0),
                const Eigen::Vector3d & _terminal_ori = Eigen::Vector3d::Constant(1.0),
                const Eigen::Vector3d & _terminal_linear_vel = Eigen::Vector3d::Constant(0.01),
                const Eigen::Vector3d & _terminal_angular_vel = Eigen::Vector3d::Constant(0.01))
    : running_pos(_running_pos), running_ori(_running_ori), running_linear_vel(_running_linear_vel),
      running_angular_vel(_running_angular_vel), running_force(_running_force), terminal_pos(_terminal_pos),
      terminal_ori(_terminal_ori), terminal_linear_vel(_terminal_linear_vel),
      terminal_angular_vel(_terminal_angular_vel)
    {
    }
  };
 
  class DdpProblem : public nmpc_ddp::DDPProblem<12, Eigen::Dynamic>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    DdpProblem(double horizon_dt, double mass, const WeightParam & weight_param)
    : nmpc_ddp::DDPProblem<12, Eigen::Dynamic>(horizon_dt), mass_(mass), weight_param_(weight_param)
    {
    }
 
    inline void setMotionParamFunc(const std::function<MotionParam(double)> & motion_param_func)
    {
      motion_param_func_ = motion_param_func;
    }
 
    inline void setRefDataFunc(const std::function<RefData(double)> & ref_data_func)
    {
      ref_data_func_ = ref_data_func;
    }
 
    using DDPProblem::inputDim;
 
    virtual int inputDim(double t) const override;
 
    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;
 
  public:
    double mass_ = 0;
 
  protected:
    WeightParam weight_param_;
 
    std::function<MotionParam(double)> motion_param_func_;
 
    std::function<RefData(double)> ref_data_func_;
  };
 
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d ori = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d linear_vel = Eigen::Vector3d::Zero();
 
    Eigen::Vector3d angular_vel = Eigen::Vector3d::Zero();
 
    std::vector<DdpProblem::InputDimVector> u_list = {};
 
    InitialParam() {}
 
    InitialParam(const DdpProblem::StateDimVector & state);
 
    DdpProblem::StateDimVector toState() const;
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  DdpSingleRigidBody(double mass,
                     double horizon_dt,
                     int horizon_steps,
                     const WeightParam & weight_param = WeightParam());
 
  Eigen::VectorXd planOnce(const std::function<MotionParam(double)> & motion_param_func,
                           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<12, Eigen::Dynamic>> ddp_solver_;
 
  std::array<double, 2> force_scale_limits_ = {0.0, 1e6};
};
} // namespace CCC