mpc_biped_walk.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_MPC_BIPED_WALK_HPP_
#define ROBOTOC_MPC_BIPED_WALK_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/contact_wrench_cone.hpp"
#include "robotoc/constraints/impact_wrench_cone.hpp"
#include "robotoc/mpc/control_policy.hpp"
 
 
namespace robotoc {
 
class MPCBipedWalk {
public:
  MPCBipedWalk(const Robot& biped_robot, const double T, const int N);
 
  MPCBipedWalk();
 
  ~MPCBipedWalk();
 
  MPCBipedWalk(const MPCBipedWalk&) = default;
 
  MPCBipedWalk& operator=(const MPCBipedWalk&) = default;
 
  MPCBipedWalk(MPCBipedWalk&&) noexcept = default;
 
  MPCBipedWalk& operator=(MPCBipedWalk&&) noexcept = default;
 
  void setGaitPattern(const std::shared_ptr<ContactPlannerBase>& foot_step_planner,
                      const double swing_height, const double swing_time, 
                      const double double_support_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<ContactWrenchCone> getContactWrenchConeHandle();
 
  std::shared_ptr<ImpactWrenchCone> getImpactWrenchConeHandle();
 
  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_right_swing_, cs_left_swing_;
  robotoc::Solution s_;
  double step_height_, swing_time_, double_support_time_, swing_start_time_, 
         T_, dt_, dtm_, ts_last_, eps_;
  int N_, current_step_, predict_step_;
  bool enable_double_support_phase_;
 
  std::shared_ptr<ConfigurationSpaceCost> config_cost_;
  std::shared_ptr<ConfigurationSpaceCost> base_rot_cost_;
  std::shared_ptr<TaskSpace3DCost> L_foot_cost_, R_foot_cost_;
  std::shared_ptr<CoMCost> com_cost_;
  std::shared_ptr<MPCPeriodicConfigurationRef> base_rot_ref_;
  std::shared_ptr<MPCPeriodicSwingFootRef> L_foot_ref_, R_foot_ref_;
  std::shared_ptr<MPCPeriodicCoMRef> com_ref_;
  std::shared_ptr<ContactWrenchCone> contact_wrench_cone_;
  std::shared_ptr<ImpactWrenchCone> impact_wrench_cone_;
 
  bool addStep(const double t);
 
  void resetContactPlacements(const double t, const Eigen::VectorXd& q, 
                              const Eigen::VectorXd& v);
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_MPC_BIPED_WALK_HPP_