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];
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
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
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
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
const std::vector< int > & get_land_LF()
Definition: wbc.hpp:149
void set_standingCycle(const HorizonManager &standingCycle)
Definition: wbc.hpp:139
double simu_step
Definition: wbc.hpp:40
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: 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
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
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
int T
Definition: wbc.hpp:33
void initializeSupportTiming()
Definition: wbc.cpp:220