4 #include <pinocchio/fwd.hpp>
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>
65 std::vector<int> takeoff_RF_, takeoff_LF_, land_RF_, land_LF_;
68 void updateStepTrackerReferences();
69 void updateStepTrackerLastReference();
70 void updateNonThinkingReferences();
73 std::vector<pinocchio::SE3> ref_LF_poses_, ref_RF_poses_;
74 std::vector<eVector3> ref_com_vel_;
77 bool initialized_ =
false;
78 void rewindWalkingCycle();
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_;
93 const Eigen::VectorXd &v0,
const std::string &actuationCostName);
97 const Eigen::VectorXd &v0,
98 const std::string &actuationCostName);
105 const unsigned long after);
107 const Eigen::VectorXd &
shapeState(
const Eigen::VectorXd &q,
108 const Eigen::VectorXd &v);
118 void iterate(
const Eigen::VectorXd &q_current,
119 const Eigen::VectorXd &v_current,
bool is_feasible);
121 void iterate(
int iteration,
const Eigen::VectorXd &q_current,
122 const Eigen::VectorXd &v_current,
bool is_feasible);
130 const Eigen::VectorXd &
get_x0()
const {
return x0_; }
131 void set_x0(
const Eigen::VectorXd &x0) { x0_ = x0; }
135 walkingCycle_ = walkingCycle;
140 standingCycle_ = standingCycle;
155 const std::vector<pinocchio::SE3> &
getPoseRef_LF() {
return ref_LF_poses_; }
157 return ref_LF_poses_[time];
163 ref_LF_poses_[time] = ref_LF_pose;
166 const std::vector<pinocchio::SE3> &
getPoseRef_RF() {
return ref_RF_poses_; }
168 return ref_RF_poses_[time];
174 ref_RF_poses_[time] = ref_RF_pose;
179 return ref_com_vel_[time];
Definition: horizon_manager.hpp:19
Definition: model_factory.hpp:55
Definition: designer.hpp:28
const std::vector< int > & get_takeoff_LF()
Definition: wbc.hpp:151
const Eigen::VectorXd & shapeState(const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: wbc.cpp:196
HorizonManager & get_walkingCycle()
Definition: wbc.hpp:133
void set_standingCycle(const HorizonManager &standingCycle)
Definition: wbc.hpp:139
const std::vector< int > & get_land_LF()
Definition: wbc.hpp:149
void setPoseRef_LF(const std::vector< pinocchio::SE3 > &ref_LF_poses)
Definition: wbc.hpp:159
RobotDesigner & get_designer()
Definition: wbc.hpp:146
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
void set_horizon(const HorizonManager &horizon)
Definition: wbc.hpp:144
const pinocchio::SE3 & getPoseRef_RF(unsigned long time)
Definition: wbc.hpp:167
void initializeSupportTiming()
Definition: wbc.cpp:220
HorizonManager & get_standingCycle()
Definition: wbc.hpp:138
void updateStepCycleTiming()
std::vector< pinocchio::SE3 > & ref_LF_poses()
Definition: wbc.hpp:188
void setPoseRef_RF(const std::vector< pinocchio::SE3 > &ref_RF_poses)
Definition: wbc.hpp:170
const eVector3 & getVelRef_COM(unsigned long time)
Definition: wbc.hpp:178
bool timeToSolveDDP(int iteration)
Definition: wbc.cpp:98
void iterate(const Eigen::VectorXd &q_current, const Eigen::VectorXd &v_current, bool is_feasible)
Definition: wbc.cpp:103
void setPoseRef_LF(const pinocchio::SE3 &ref_LF_pose, unsigned long time)
Definition: wbc.hpp:162
const std::vector< pinocchio::SE3 > & getPoseRef_LF()
Definition: wbc.hpp:155
const std::vector< pinocchio::SE3 > & getPoseRef_RF()
Definition: wbc.hpp:166
void set_x0(const Eigen::VectorXd &x0)
Definition: wbc.hpp:131
void setPoseRef_RF(const pinocchio::SE3 &ref_RF_pose, unsigned long time)
Definition: wbc.hpp:173
void setVelRef_COM(const std::vector< eVector3 > &ref_com_vel)
Definition: wbc.hpp:181
void set_designer(const RobotDesigner &designer)
Definition: wbc.hpp:147
const std::vector< int > & get_takeoff_RF()
Definition: wbc.hpp:152
std::vector< pinocchio::SE3 > & ref_RF_poses()
Definition: wbc.hpp:189
LocomotionType currentLocomotion()
Definition: wbc.hpp:194
void switchToStand()
Definition: wbc.hpp:193
void recedeWithCycle()
Definition: wbc.cpp:161
WBC()
Definition: wbc.cpp:5
HorizonManager & get_horizon()
Definition: wbc.hpp:143
const std::vector< eVector3 > & getVelRef_COM()
Definition: wbc.hpp:177
WBCSettings & get_settings()
Definition: wbc.hpp:128
const pinocchio::SE3 & getPoseRef_LF(unsigned long time)
Definition: wbc.hpp:156
const Eigen::VectorXd & get_x0() const
Definition: wbc.hpp:130
std::vector< eVector3 > & ref_com_vel()
Definition: wbc.hpp:190
void generateStandingCycle(ModelMaker &mm)
Definition: wbc.cpp:88
const std::vector< int > & get_land_RF()
Definition: wbc.hpp:150
void set_walkingCycle(const HorizonManager &walkingCycle)
Definition: wbc.hpp:134
const supportSwitch & getSwitches(const unsigned long before, const unsigned long after)
Definition: wbc.cpp:262
void updateSupportTiming()
Definition: wbc.cpp:234
void switchToWalk()
Definition: wbc.hpp:192
void generateWalkingCycle(ModelMaker &mm)
Definition: wbc.cpp:62
void setVelRef_COM(const eVector3 &ref_com_vel, unsigned long time)
Definition: wbc.hpp:184
Definition: activation-quad-ref.hpp:19
ControlForm
Definition: wbc.hpp:23
@ NonThinking
Definition: wbc.hpp:23
@ StepTracker
Definition: wbc.hpp:23
@ StairClimber
Definition: wbc.hpp:23
LocomotionType
Definition: wbc.hpp:24
@ WALKING
Definition: wbc.hpp:24
@ STANDING
Definition: wbc.hpp:24
supportSwitch
Definition: wbc.hpp:25
@ TAKEOFF_RF
Definition: wbc.hpp:25
@ LAND_LF
Definition: wbc.hpp:25
@ TAKEOFF_LF
Definition: wbc.hpp:25
@ LAND_RF
Definition: wbc.hpp:25
@ NO_SWITCH
Definition: wbc.hpp:25
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
int Nc
Definition: wbc.hpp:42
int TdoubleSupport
Definition: wbc.hpp:34
int horizonSteps
Definition: wbc.hpp:31
ControlForm typeOfCommand
Definition: wbc.hpp:43
int T
Definition: wbc.hpp:33
double Dt
Definition: wbc.hpp:39
int TsingleSupport
Definition: wbc.hpp:35
int Tstep
Definition: wbc.hpp:36
double simu_step
Definition: wbc.hpp:40
int ddpIteration
Definition: wbc.hpp:37
int totalSteps
Definition: wbc.hpp:32