wbc.hpp
Go to the documentation of this file.
1 #ifndef SOBEC_WBC
2 #define SOBEC_WBC
3 
4 #include <pinocchio/fwd.hpp>
5 
6 // include pinocchio first
7 
8 #include <ndcurves/bezier_curve.h>
9 #include <ndcurves/fwd.h>
10 #include <ndcurves/piecewise_curve.h>
11 #include <ndcurves/polynomial.h>
12 #include <ndcurves/se3_curve.h>
13 
17 
18 namespace sobec {
19 
26 
27 struct WBCSettings {
29  public:
30  // timing
31  int horizonSteps = 2;
32  int totalSteps = 4;
33  int T = 100;
34  int TdoubleSupport = 50;
35  int TsingleSupport = 100;
37  int ddpIteration = 1;
38 
39  double Dt = 1e-2;
40  double simu_step = 1e-3;
41 
42  int Nc = (int)round(Dt / simu_step);
44 };
45 class WBC {
53  private:
54  WBCSettings settings_;
55  RobotDesigner designer_;
56  HorizonManager horizon_;
57  HorizonManager walkingCycle_;
58  HorizonManager standingCycle_;
59 
60  Eigen::VectorXd x0_;
61 
62  LocomotionType now_ = WALKING;
63 
64  // timings
65  std::vector<int> takeoff_RF_, takeoff_LF_, land_RF_, land_LF_;
66 
67  // INTERNAL UPDATING functions
68  void updateStepTrackerReferences();
69  void updateStepTrackerLastReference();
70  void updateNonThinkingReferences();
71 
72  // References for costs:
73  std::vector<pinocchio::SE3> ref_LF_poses_, ref_RF_poses_;
74  std::vector<eVector3> ref_com_vel_;
75 
76  // Security management
77  bool initialized_ = false;
78  void rewindWalkingCycle();
79 
80  // Memory preallocations:
81  std::vector<unsigned long> controlled_joints_id_;
82  Eigen::VectorXd x_internal_;
83  bool time_to_solve_ddp_ = false;
84  bool first_switch_to_stand_ = true;
85  std::set<std::string> contacts_before_, contacts_after_;
86  supportSwitch switch_;
87  int horizon_end_;
88 
89  public:
90  WBC();
91  WBC(const WBCSettings &settings, const RobotDesigner &design,
92  const HorizonManager &horizon, const Eigen::VectorXd &q0,
93  const Eigen::VectorXd &v0, const std::string &actuationCostName);
94 
95  void initialize(const WBCSettings &settings, const RobotDesigner &design,
96  const HorizonManager &horizon, const Eigen::VectorXd &q0,
97  const Eigen::VectorXd &v0,
98  const std::string &actuationCostName);
99 
101 
102  void updateSupportTiming();
103 
104  const supportSwitch &getSwitches(const unsigned long before,
105  const unsigned long after);
106 
107  const Eigen::VectorXd &shapeState(const Eigen::VectorXd &q,
108  const Eigen::VectorXd &v);
109 
111 
113 
114  void updateStepCycleTiming();
115 
116  bool timeToSolveDDP(int iteration);
117 
118  void iterate(const Eigen::VectorXd &q_current,
119  const Eigen::VectorXd &v_current, bool is_feasible);
120 
121  void iterate(int iteration, const Eigen::VectorXd &q_current,
122  const Eigen::VectorXd &v_current, bool is_feasible);
123 
124  void recedeWithCycle();
125  void recedeWithCycle(HorizonManager &cycle);
126 
127  // getters and setters
128  WBCSettings &get_settings() { return settings_; }
129 
130  const Eigen::VectorXd &get_x0() const { return x0_; }
131  void set_x0(const Eigen::VectorXd &x0) { x0_ = x0; }
132 
133  HorizonManager &get_walkingCycle() { return walkingCycle_; }
134  void set_walkingCycle(const HorizonManager &walkingCycle) {
135  walkingCycle_ = walkingCycle;
136  }
137 
138  HorizonManager &get_standingCycle() { return standingCycle_; }
139  void set_standingCycle(const HorizonManager &standingCycle) {
140  standingCycle_ = standingCycle;
141  }
142 
143  HorizonManager &get_horizon() { return horizon_; }
144  void set_horizon(const HorizonManager &horizon) { horizon_ = horizon; }
145 
146  RobotDesigner &get_designer() { return designer_; }
147  void set_designer(const RobotDesigner &designer) { designer_ = designer; }
148 
149  const std::vector<int> &get_land_LF() { return land_LF_; }
150  const std::vector<int> &get_land_RF() { return land_RF_; }
151  const std::vector<int> &get_takeoff_LF() { return takeoff_LF_; }
152  const std::vector<int> &get_takeoff_RF() { return takeoff_RF_; }
153 
154  // USER REFERENCE SETTERS AND GETTERS
155  const std::vector<pinocchio::SE3> &getPoseRef_LF() { return ref_LF_poses_; }
156  const pinocchio::SE3 &getPoseRef_LF(unsigned long time) {
157  return ref_LF_poses_[time];
158  }
159  void setPoseRef_LF(const std::vector<pinocchio::SE3> &ref_LF_poses) {
160  ref_LF_poses_ = ref_LF_poses;
161  }
162  void setPoseRef_LF(const pinocchio::SE3 &ref_LF_pose, unsigned long time) {
163  ref_LF_poses_[time] = ref_LF_pose;
164  }
165 
166  const std::vector<pinocchio::SE3> &getPoseRef_RF() { return ref_RF_poses_; }
167  const pinocchio::SE3 &getPoseRef_RF(unsigned long time) {
168  return ref_RF_poses_[time];
169  }
170  void setPoseRef_RF(const std::vector<pinocchio::SE3> &ref_RF_poses) {
171  ref_RF_poses_ = ref_RF_poses;
172  }
173  void setPoseRef_RF(const pinocchio::SE3 &ref_RF_pose, unsigned long time) {
174  ref_RF_poses_[time] = ref_RF_pose;
175  }
176 
177  const std::vector<eVector3> &getVelRef_COM() { return ref_com_vel_; }
178  const eVector3 &getVelRef_COM(unsigned long time) {
179  return ref_com_vel_[time];
180  }
181  void setVelRef_COM(const std::vector<eVector3> &ref_com_vel) {
182  ref_com_vel_ = ref_com_vel;
183  }
184  void setVelRef_COM(const eVector3 &ref_com_vel, unsigned long time) {
185  ref_com_vel_[time] = ref_com_vel;
186  }
187  // For the python bindings:
188  std::vector<pinocchio::SE3> &ref_LF_poses() { return ref_LF_poses_; }
189  std::vector<pinocchio::SE3> &ref_RF_poses() { return ref_RF_poses_; }
190  std::vector<eVector3> &ref_com_vel() { return ref_com_vel_; }
191 
192  void switchToWalk() { now_ = WALKING; }
193  void switchToStand() { now_ = STANDING; }
195 };
196 } // namespace sobec
197 
198 #endif // SOBEC_OCP
const Eigen::VectorXd & shapeState(const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: wbc.cpp:196
void recedeWithCycle()
Definition: wbc.cpp:161
void iterate(const Eigen::VectorXd &q_current, const Eigen::VectorXd &v_current, bool is_feasible)
Definition: wbc.cpp:103
Definition: wbc.hpp:23
Definition: wbc.hpp:23
void set_designer(const RobotDesigner &designer)
Definition: wbc.hpp:147
Definition: horizon_manager.hpp:19
HorizonManager & get_horizon()
Definition: wbc.hpp:143
int TsingleSupport
Definition: wbc.hpp:35
void generateWalkingCycle(ModelMaker &mm)
Definition: wbc.cpp:62
WBC()
Definition: wbc.cpp:5
Definition: wbc.hpp:24
Definition: wbc.hpp:25
void setPoseRef_RF(const std::vector< pinocchio::SE3 > &ref_RF_poses)
Definition: wbc.hpp:170
const std::vector< pinocchio::SE3 > & getPoseRef_RF()
Definition: wbc.hpp:166
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
int totalSteps
Definition: wbc.hpp:32
const pinocchio::SE3 & getPoseRef_LF(unsigned long time)
Definition: wbc.hpp:156
int horizonSteps
Definition: wbc.hpp:31
Definition: designer.hpp:28
const std::vector< pinocchio::SE3 > & getPoseRef_LF()
Definition: wbc.hpp:155
const supportSwitch & getSwitches(const unsigned long before, const unsigned long after)
Definition: wbc.cpp:262
HorizonManager & get_standingCycle()
Definition: wbc.hpp:138
Definition: wbc.hpp:25
double Dt
Definition: wbc.hpp:39
int Nc
Definition: wbc.hpp:42
void switchToStand()
Definition: wbc.hpp:193
void setPoseRef_RF(const pinocchio::SE3 &ref_RF_pose, unsigned long time)
Definition: wbc.hpp:173
supportSwitch
Definition: wbc.hpp:25
std::vector< pinocchio::SE3 > & ref_RF_poses()
Definition: wbc.hpp:189
const std::vector< int > & get_takeoff_RF()
Definition: wbc.hpp:152
Definition: wbc.hpp:45
const std::vector< int > & get_land_LF()
Definition: wbc.hpp:149
void set_standingCycle(const HorizonManager &standingCycle)
Definition: wbc.hpp:139
Definition: wbc.hpp:25
double simu_step
Definition: wbc.hpp:40
Definition: wbc.hpp:25
const pinocchio::SE3 & getPoseRef_RF(unsigned long time)
Definition: wbc.hpp:167
ControlForm
Definition: wbc.hpp:23
const std::vector< int > & get_takeoff_LF()
Definition: wbc.hpp:151
WBCSettings & get_settings()
Definition: wbc.hpp:128
std::vector< pinocchio::SE3 > & ref_LF_poses()
Definition: wbc.hpp:188
Definition: wbc.hpp:25
Definition: activation-quad-ref.hpp:19
int ddpIteration
Definition: wbc.hpp:37
void set_horizon(const HorizonManager &horizon)
Definition: wbc.hpp:144
LocomotionType currentLocomotion()
Definition: wbc.hpp:194
ControlForm typeOfCommand
Definition: wbc.hpp:43
Definition: model_factory.hpp:55
Definition: wbc.hpp:24
HorizonManager & get_walkingCycle()
Definition: wbc.hpp:133
void setVelRef_COM(const std::vector< eVector3 > &ref_com_vel)
Definition: wbc.hpp:181
LocomotionType
Definition: wbc.hpp:24
Definition: wbc.hpp:23
void generateStandingCycle(ModelMaker &mm)
Definition: wbc.cpp:88
void setPoseRef_LF(const std::vector< pinocchio::SE3 > &ref_LF_poses)
Definition: wbc.hpp:159
std::vector< eVector3 > & ref_com_vel()
Definition: wbc.hpp:190
void set_walkingCycle(const HorizonManager &walkingCycle)
Definition: wbc.hpp:134
void setPoseRef_LF(const pinocchio::SE3 &ref_LF_pose, unsigned long time)
Definition: wbc.hpp:162
void initialize(const WBCSettings &settings, const RobotDesigner &design, const HorizonManager &horizon, const Eigen::VectorXd &q0, const Eigen::VectorXd &v0, const std::string &actuationCostName)
Definition: wbc.cpp:13
bool timeToSolveDDP(int iteration)
Definition: wbc.cpp:98
void set_x0(const Eigen::VectorXd &x0)
Definition: wbc.hpp:131
int Tstep
Definition: wbc.hpp:36
void setVelRef_COM(const eVector3 &ref_com_vel, unsigned long time)
Definition: wbc.hpp:184
void updateSupportTiming()
Definition: wbc.cpp:234
const eVector3 & getVelRef_COM(unsigned long time)
Definition: wbc.hpp:178
const Eigen::VectorXd & get_x0() const
Definition: wbc.hpp:130
const std::vector< int > & get_land_RF()
Definition: wbc.hpp:150
void switchToWalk()
Definition: wbc.hpp:192
void updateStepCycleTiming()
int TdoubleSupport
Definition: wbc.hpp:34
RobotDesigner & get_designer()
Definition: wbc.hpp:146
const std::vector< eVector3 > & getVelRef_COM()
Definition: wbc.hpp:177
Definition: wbc.hpp:27
int T
Definition: wbc.hpp:33
void initializeSupportTiming()
Definition: wbc.cpp:220