Go to the documentation of this file.
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];
std::vector< pinocchio::SE3 > & ref_RF_poses()
Definition: wbc.hpp:189
ControlForm
Definition: wbc.hpp:23
int Tstep
Definition: wbc.hpp:36
void setVelRef_COM(const eVector3 &ref_com_vel, unsigned long time)
Definition: wbc.hpp:184
@ WALKING
Definition: wbc.hpp:24
@ STANDING
Definition: wbc.hpp:24
void iterate(const Eigen::VectorXd &q_current, const Eigen::VectorXd &v_current, bool is_feasible)
Definition: wbc.cpp:103
void setPoseRef_RF(const pinocchio::SE3 &ref_RF_pose, unsigned long time)
Definition: wbc.hpp:173
@ StepTracker
Definition: wbc.hpp:23
@ TAKEOFF_LF
Definition: wbc.hpp:25
void setVelRef_COM(const std::vector< eVector3 > &ref_com_vel)
Definition: wbc.hpp:181
void generateStandingCycle(ModelMaker &mm)
Definition: wbc.cpp:88
const Eigen::VectorXd & get_x0() const
Definition: wbc.hpp:130
const eVector3 & getVelRef_COM(unsigned long time)
Definition: wbc.hpp:178
const std::vector< int > & get_takeoff_RF()
Definition: wbc.hpp:152
HorizonManager & get_standingCycle()
Definition: wbc.hpp:138
const std::vector< eVector3 > & getVelRef_COM()
Definition: wbc.hpp:177
void generateWalkingCycle(ModelMaker &mm)
Definition: wbc.cpp:62
HorizonManager & get_horizon()
Definition: wbc.hpp:143
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
@ LAND_RF
Definition: wbc.hpp:25
double simu_step
Definition: wbc.hpp:40
int T
Definition: wbc.hpp:33
Definition: designer.hpp:28
int totalSteps
Definition: wbc.hpp:32
const std::vector< pinocchio::SE3 > & getPoseRef_LF()
Definition: wbc.hpp:155
void setPoseRef_RF(const std::vector< pinocchio::SE3 > &ref_RF_poses)
Definition: wbc.hpp:170
WBCSettings & get_settings()
Definition: wbc.hpp:128
Definition: horizon_manager.hpp:19
ControlForm typeOfCommand
Definition: wbc.hpp:43
void updateStepCycleTiming()
void set_horizon(const HorizonManager &horizon)
Definition: wbc.hpp:144
HorizonManager & get_walkingCycle()
Definition: wbc.hpp:133
LocomotionType currentLocomotion()
Definition: wbc.hpp:194
WBC()
Definition: wbc.cpp:5
Definition: activation-quad-ref.hpp:19
void set_x0(const Eigen::VectorXd &x0)
Definition: wbc.hpp:131
void setPoseRef_LF(const std::vector< pinocchio::SE3 > &ref_LF_poses)
Definition: wbc.hpp:159
void set_standingCycle(const HorizonManager &standingCycle)
Definition: wbc.hpp:139
const pinocchio::SE3 & getPoseRef_LF(unsigned long time)
Definition: wbc.hpp:156
const std::vector< int > & get_land_RF()
Definition: wbc.hpp:150
const std::vector< int > & get_land_LF()
Definition: wbc.hpp:149
@ StairClimber
Definition: wbc.hpp:23
@ NO_SWITCH
Definition: wbc.hpp:25
const pinocchio::SE3 & getPoseRef_RF(unsigned long time)
Definition: wbc.hpp:167
const Eigen::VectorXd & shapeState(const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: wbc.cpp:196
std::vector< pinocchio::SE3 > & ref_LF_poses()
Definition: wbc.hpp:188
const std::vector< int > & get_takeoff_LF()
Definition: wbc.hpp:151
double Dt
Definition: wbc.hpp:39
void switchToWalk()
Definition: wbc.hpp:192
bool timeToSolveDDP(int iteration)
Definition: wbc.cpp:98
void set_walkingCycle(const HorizonManager &walkingCycle)
Definition: wbc.hpp:134
int ddpIteration
Definition: wbc.hpp:37
void recedeWithCycle()
Definition: wbc.cpp:161
void setPoseRef_LF(const pinocchio::SE3 &ref_LF_pose, unsigned long time)
Definition: wbc.hpp:162
int TsingleSupport
Definition: wbc.hpp:35
LocomotionType
Definition: wbc.hpp:24
@ TAKEOFF_RF
Definition: wbc.hpp:25
void updateSupportTiming()
Definition: wbc.cpp:234
void set_designer(const RobotDesigner &designer)
Definition: wbc.hpp:147
int TdoubleSupport
Definition: wbc.hpp:34
int Nc
Definition: wbc.hpp:42
@ NonThinking
Definition: wbc.hpp:23
Definition: model_factory.hpp:55
std::vector< eVector3 > & ref_com_vel()
Definition: wbc.hpp:190
const supportSwitch & getSwitches(const unsigned long before, const unsigned long after)
Definition: wbc.cpp:262
int horizonSteps
Definition: wbc.hpp:31
void initializeSupportTiming()
Definition: wbc.cpp:220
void switchToStand()
Definition: wbc.hpp:193
const std::vector< pinocchio::SE3 > & getPoseRef_RF()
Definition: wbc.hpp:166
RobotDesigner & get_designer()
Definition: wbc.hpp:146
supportSwitch
Definition: wbc.hpp:25
@ LAND_LF
Definition: wbc.hpp:25
Eigen::Vector3d eVector3
Definition: fwd.hpp:107