mpc_jump.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_MPC_JUMP_HPP_
#define ROBOTOC_MPC_JUMP_HPP_
 
#include <vector>
#include <memory>
#include <limits>
 
#include "Eigen/Core"
 
#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/utils/aligned_vector.hpp"
#include "robotoc/robot/se3.hpp"
#include "robotoc/mpc/contact_planner_base.hpp"
#include "robotoc/cost/configuration_space_cost.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/contact_wrench_cone.hpp"
#include "robotoc/mpc/control_policy.hpp"
 
 
namespace robotoc {
 
class MPCJump {
public:
  MPCJump(const Robot& robot, const double T, const int N);
 
  MPCJump();
 
  ~MPCJump();
 
  MPCJump(const MPCJump&) = default;
 
  MPCJump& operator=(const MPCJump&) = default;
 
  MPCJump(MPCJump&&) noexcept = default;
 
  MPCJump& operator=(MPCJump&&) noexcept = default;
 
  void setJumpPattern(const std::shared_ptr<ContactPlannerBase>& foot_step_planner,
                      const double flying_time, const double min_flying_time, 
                      const double ground_time, const double min_ground_time);
 
  void init(const double t, const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
            const SolverOptions& solver_options, const bool sto=false);
 
  void reset();
 
  void reset(const Eigen::VectorXd& q, const Eigen::VectorXd& v);
 
  void reset(const double t, const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
             const SolverOptions& solver_options, const bool sto=false);
 
  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<Constraints> getConstraintsHandle();
 
  std::shared_ptr<FrictionCone> getFrictionConeHandle();
 
  std::shared_ptr<ContactWrenchCone> getContactWrenchConeHandle();
 
  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_;
  std::shared_ptr<robotoc::STOCostFunction> sto_cost_;
  std::shared_ptr<robotoc::STOConstraints> sto_constraints_;
  OCPSolver ocp_solver_;
  SolverOptions solver_options_;
  ContactStatus cs_ground_, cs_flying_;
  robotoc::Solution s_;
  double flying_time_, min_flying_time_, ground_time_, min_ground_time_,
         T_, dt_, dtm_, t_mpc_start_, eps_;
  int N_, current_step_;
 
  std::shared_ptr<ConfigurationSpaceCost> config_cost_;
  std::shared_ptr<FrictionCone> friction_cone_;
  std::shared_ptr<ContactWrenchCone> contact_wrench_cone_;
 
  void resetMinimumDwellTimes(const double t, const double min_dt);
 
  void resetGoalContactPlacements(const Eigen::VectorXd& q);
 
  void resetContactPlacements(const double t, const Eigen::VectorXd& q, 
                              const Eigen::VectorXd& v);
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_MPC_JUMP_HPP_