mpc_pace.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_MPC_PACE_HPP_
#define ROBOTOC_MPC_PACE_HPP_
 
#include <memory>
 
#include "Eigen/Core"
#include "Eigen/Geometry"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/solver/ocp_solver.hpp"
#include "robotoc/planner/contact_sequence.hpp"
#include "robotoc/cost/cost_function.hpp"
#include "robotoc/constraints/constraints.hpp"
#include "robotoc/solver/solver_options.hpp"
#include "robotoc/mpc/contact_planner_base.hpp"
#include "robotoc/cost/configuration_space_cost.hpp"
#include "robotoc/cost/task_space_3d_cost.hpp"
#include "robotoc/cost/com_cost.hpp"
#include "robotoc/mpc/mpc_periodic_swing_foot_ref.hpp"
#include "robotoc/mpc/mpc_periodic_com_ref.hpp"
#include "robotoc/mpc/mpc_periodic_configuration_ref.hpp"
#include "robotoc/constraints/joint_position_lower_limit.hpp"
#include "robotoc/constraints/joint_position_upper_limit.hpp"
#include "robotoc/constraints/joint_velocity_lower_limit.hpp"
#include "robotoc/constraints/joint_velocity_upper_limit.hpp"
#include "robotoc/constraints/joint_torques_lower_limit.hpp"
#include "robotoc/constraints/joint_torques_upper_limit.hpp"
#include "robotoc/constraints/friction_cone.hpp"
#include "robotoc/constraints/impact_friction_cone.hpp"
#include "robotoc/mpc/control_policy.hpp"
 
 
namespace robotoc {
 
class MPCPace {
public:
  MPCPace(const Robot& quadruped_robot, const double T, const int N);
 
  MPCPace();
 
  ~MPCPace();
 
  MPCPace(const MPCPace&) = default;
 
  MPCPace& operator=(const MPCPace&) = default;
 
  MPCPace(MPCPace&&) noexcept = default;
 
  MPCPace& operator=(MPCPace&&) noexcept = default;
 
  void setGaitPattern(const std::shared_ptr<ContactPlannerBase>& foot_step_planner,
                      const double swing_height, const double swing_time, 
                      const double stance_time, const double swing_start_time);
 
  void init(const double t, const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
            const SolverOptions& solver_options);
 
  void reset();
 
  void reset(const Eigen::VectorXd& q, const Eigen::VectorXd& v);
 
  void setSolverOptions(const SolverOptions& solver_options);
 
  void updateSolution(const double t, const double dt, const Eigen::VectorXd& q, 
                      const Eigen::VectorXd& v);
 
  const Eigen::VectorXd& getInitialControlInput() const;
 
  const Solution& getSolution() const;
 
  const aligned_vector<LQRPolicy>& getLQRPolicy() const;
 
  ControlPolicy getControlPolicy(const double t) const { 
    return ControlPolicy(ocp_solver_, t); 
  }
 
  double KKTError(const double t, const Eigen::VectorXd& q, 
                  const Eigen::VectorXd& v);
 
  double KKTError() const;
 
  std::shared_ptr<CostFunction> getCostHandle();
 
  std::shared_ptr<ConfigurationSpaceCost> getConfigCostHandle();
 
  std::shared_ptr<ConfigurationSpaceCost> getBaseRotationCostHandle();
 
  std::vector<std::shared_ptr<TaskSpace3DCost>> getSwingFootCostHandle();
 
  std::shared_ptr<CoMCost> getCoMCostHandle();
 
  std::shared_ptr<Constraints> getConstraintsHandle();
 
  std::shared_ptr<FrictionCone> getFrictionConeHandle();
 
  const OCPSolver& getSolver() const { return ocp_solver_; }
 
  const std::shared_ptr<ContactSequence>& getContactSequence() const { 
    return contact_sequence_; 
  }
 
  void setRobotProperties(const RobotProperties& properties);
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
private:
  std::shared_ptr<ContactPlannerBase> foot_step_planner_;
  std::shared_ptr<ContactSequence> contact_sequence_;
  std::shared_ptr<CostFunction> cost_;
  std::shared_ptr<Constraints> constraints_;
  OCPSolver ocp_solver_;
  SolverOptions solver_options_;
  ContactStatus cs_standing_, cs_left_swing_, cs_right_swing_;
  robotoc::Solution s_;
  double swing_height_, swing_time_, stance_time_, swing_start_time_, 
         T_, dt_, dtm_, ts_last_, eps_;
  int N_, current_step_, predict_step_;
  bool enable_stance_phase_;
 
  std::shared_ptr<ConfigurationSpaceCost> config_cost_;
  std::shared_ptr<ConfigurationSpaceCost> base_rot_cost_;
  std::shared_ptr<TaskSpace3DCost> LF_foot_cost_, LH_foot_cost_,
                                              RF_foot_cost_, RH_foot_cost_;
  std::shared_ptr<CoMCost> com_cost_;
  std::shared_ptr<MPCPeriodicConfigurationRef> base_rot_ref_;
  std::shared_ptr<MPCPeriodicSwingFootRef> LF_foot_ref_, LH_foot_ref_,
                                           RF_foot_ref_, RH_foot_ref_;
  std::shared_ptr<MPCPeriodicCoMRef> com_ref_;
  std::shared_ptr<FrictionCone> friction_cone_;
 
  bool addStep(const double t);
 
  void resetContactPlacements(const double t, const Eigen::VectorXd& q, 
                              const Eigen::VectorXd& v);
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_MPC_PACE_HPP_