ocp.hpp
Go to the documentation of this file.
1 #ifndef SOBEC_OCP
2 #define SOBEC_OCP
3 
4 #include <pinocchio/fwd.hpp>
5 
6 // include pinocchio first
7 
12 
13 namespace sobec {
14 
15 struct OCPSettings {
16  public:
17  // timing
18  unsigned long totalSteps = 4;
19  unsigned long T = 100;
20  unsigned long TdoubleSupport = 50;
21  unsigned long TsimpleSupport = 100;
22  unsigned long Tstep = TdoubleSupport + TsimpleSupport;
23  unsigned long ddpIteration = 1;
24 
25  double Dt = 1e-2;
26  double simu_step = 1e-3;
27 
28  unsigned long Nc = (unsigned long)round(Dt / simu_step);
29  double stepSize = 0.1;
30  double stepHeight = 0.03;
31  double stepDepth = 0.0;
32  double stepYCorrection = 0.005;
33 };
34 
35 class OCP {
36  private:
37  OCPSettings OCP_settings_;
38  RobotDesigner designer_;
39  ModelMaker modelMaker_;
40  HorizonManager horizon_;
41  FootTrajectory_ptr swing_trajectory_left_;
42  FootTrajectory_ptr swing_trajectory_right_;
43 
44  Eigen::VectorXd xc_;
45  eVector6 wrench_reference_double_;
46  eVector6 wrench_reference_simple_;
47  std::vector<unsigned long> contacts_sequence_;
48 
49  unsigned long TswitchPhase_;
50  unsigned long TswitchTraj_;
51  bool swingRightPhase_;
52  bool swingRightTraj_;
53  std::size_t steps_;
54 
55  pinocchio::SE3 starting_position_left_;
56  pinocchio::SE3 starting_position_right_;
57  pinocchio::SE3 final_position_left_;
58  pinocchio::SE3 final_position_right_;
59 
60  public:
61  OCP();
62  OCP(const OCPSettings &settings, const ModelMakerSettings &model_settings,
63  const RobotDesignerSettings &design, const Eigen::VectorXd &q0,
64  const Eigen::VectorXd &v0);
65 
66  void initialize(const OCPSettings &settings,
67  const ModelMakerSettings &model_settings,
68  const RobotDesignerSettings &design,
69  const Eigen::VectorXd &q0, const Eigen::VectorXd &v0);
70 
71  void updateEndPhase();
72  void updateOCP(const Eigen::VectorXd &qc, const Eigen::VectorXd &vc);
73  HorizonManager get_horizon() { return horizon_; }
74 
75  eVector3 get_LF_position() { return designer_.get_LF_position(); }
76  eVector3 get_RF_position() { return designer_.get_RF_position(); }
77  eVector3 get_com_position() { return designer_.get_com_position(); }
78 };
79 } // namespace sobec
80 
81 #endif // SOBEC_OCP
unsigned long Tstep
Definition: ocp.hpp:22
Definition: ocp.hpp:35
unsigned long TsimpleSupport
Definition: ocp.hpp:21
double stepDepth
Definition: ocp.hpp:31
void updateEndPhase()
Definition: ocp.cpp:109
Definition: horizon_manager.hpp:19
Definition: ocp.hpp:15
Definition: model_factory.hpp:17
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
unsigned long ddpIteration
Definition: ocp.hpp:23
double simu_step
Definition: ocp.hpp:26
Definition: designer.hpp:28
Eigen::Matrix< double, 6, 1 > eVector6
Definition: fwd.hpp:105
void updateOCP(const Eigen::VectorXd &qc, const Eigen::VectorXd &vc)
Definition: ocp.cpp:154
unsigned long TdoubleSupport
Definition: ocp.hpp:20
double Dt
Definition: ocp.hpp:25
double stepHeight
Definition: ocp.hpp:30
unsigned long totalSteps
Definition: ocp.hpp:18
double stepSize
Definition: ocp.hpp:29
const Eigen::Vector3d & get_RF_position()
Definition: designer.hpp:84
Definition: activation-quad-ref.hpp:19
OCP()
Definition: ocp.cpp:5
unsigned long Nc
Definition: ocp.hpp:28
Definition: model_factory.hpp:55
std::shared_ptr< FootTrajectory > FootTrajectory_ptr
Definition: foot_trajectory.hpp:229
Definition: designer.hpp:17
void initialize(const OCPSettings &settings, const ModelMakerSettings &model_settings, const RobotDesignerSettings &design, const Eigen::VectorXd &q0, const Eigen::VectorXd &v0)
Definition: ocp.cpp:13
eVector3 get_RF_position()
Definition: ocp.hpp:76
const Eigen::Vector3d & get_com_position()
Definition: designer.hpp:85
double stepYCorrection
Definition: ocp.hpp:32
HorizonManager get_horizon()
Definition: ocp.hpp:73
eVector3 get_com_position()
Definition: ocp.hpp:77
eVector3 get_LF_position()
Definition: ocp.hpp:75
const Eigen::Vector3d & get_LF_position()
Definition: designer.hpp:83
unsigned long T
Definition: ocp.hpp:19