Go to the documentation of this file. 1 #ifndef SOBEC_HORIZON_MANAGER
2 #define SOBEC_HORIZON_MANAGER
25 boost::shared_ptr<crocoddyl::CostModelResidual> cone_;
26 std::vector<Eigen::VectorXd> warm_xs_;
27 std::vector<Eigen::VectorXd> warm_us_;
28 Eigen::VectorXd new_ref_;
30 Eigen::VectorXd command_torque_;
31 Eigen::VectorXd tr_error_;
32 Eigen::VectorXd K_tr_error_;
36 std::set<std::string> active_contacts_;
42 const Eigen::VectorXd &x0,
43 const std::vector<AMA> &runningModels,
44 const AMA &terminalModel);
47 const Eigen::VectorXd &x0,
48 const std::vector<AMA> &runningModels,
49 const AMA &terminalModel);
52 AMA ama(
const unsigned long time);
53 IAM iam(
const unsigned long time);
54 DAM dam(
const unsigned long time);
55 ADA ada(
const unsigned long time);
56 IAD iad(
const unsigned long time);
59 boost::shared_ptr<crocoddyl::StateMultibody>
state(
const unsigned long time);
60 boost::shared_ptr<crocoddyl::ActuationModelFloatingBase>
actuation(
61 const unsigned long time);
64 const std::string &nameCostActuation,
65 const Eigen::VectorXd &reference);
67 const std::string &nameCostActuation,
68 const Eigen::VectorXd &
x);
70 const std::string &nameCostActuation,
71 const std::string &nameCostState);
73 const std::string &nameCostLF,
74 const pinocchio::SE3 &ref_placement);
76 const std::string &nameCostRF,
77 const pinocchio::SE3 &ref_placement);
79 const unsigned long time,
const std::string &nameCostFootPose);
83 const std::string &nameContacttLF);
85 const std::string &nameContactRF);
87 const std::string &nameContactLF);
89 const std::string &nameContactRF);
91 const std::string &nameCostLF,
94 const std::string &nameCostRF,
97 void setSwingingLF(
const unsigned long time,
const std::string &nameContactLF,
98 const std::string &nameContactRF,
99 const std::string &nameForceContactLF);
100 void setSwingingRF(
const unsigned long time,
const std::string &nameContactLF,
101 const std::string &nameContactRF,
102 const std::string &nameForceContactRF);
104 const std::string &nameContactLF,
105 const std::string &nameContactRF);
108 const std::string &nameFootForceCost);
110 const std::string &nameFootForceCost);
112 const std::set<std::string> &
get_contacts(
const unsigned long time);
119 unsigned long size();
121 void solve(
const Eigen::VectorXd &measured_x,
const std::size_t ddpIteration,
122 const bool is_feasible =
false);
123 const Eigen::VectorXd &
currentTorques(
const Eigen::VectorXd &measured_x);
129 #endif // SOBEC_HORIZON_MANAGER
boost::shared_ptr< crocoddyl::IntegratedActionDataEuler > IAD
Definition: fwd.hpp:110
void initialize(const HorizonManagerSettings &horizonSettings, const Eigen::VectorXd &x0, const std::vector< AMA > &runningModels, const AMA &terminalModel)
Definition: horizon_manager.cpp:18
void setActuationReference(const unsigned long time, const std::string &nameCostActuation, const Eigen::VectorXd &reference)
Definition: horizon_manager.cpp:120
void setDoubleSupport(const unsigned long time, const std::string &nameContactLF, const std::string &nameContactRF)
Definition: horizon_manager.cpp:209
IAD iad(const unsigned long time)
Definition: horizon_manager.cpp:63
Cost costs(const unsigned long time)
Definition: horizon_manager.cpp:51
std::string leftFootName
Definition: horizon_manager.hpp:15
boost::shared_ptr< crocoddyl::SolverFDDP > DDP
Definition: fwd.hpp:117
HorizonManager()
Definition: horizon_manager.cpp:9
void solve(const Eigen::VectorXd &measured_x, const std::size_t ddpIteration, const bool is_feasible=false)
Definition: horizon_manager.cpp:275
boost::shared_ptr< crocoddyl::ActuationModelFloatingBase > actuation(const unsigned long time)
Definition: horizon_manager.cpp:75
Contact contacts(const unsigned long time)
Definition: horizon_manager.cpp:55
boost::shared_ptr< crocoddyl::StateMultibody > state(const unsigned long time)
Definition: horizon_manager.cpp:68
void setPoseReferenceRF(const unsigned long time, const std::string &nameCostRF, const pinocchio::SE3 &ref_placement)
Definition: horizon_manager.cpp:136
Definition: horizon_manager.hpp:13
void setForceReferenceLF(const unsigned long time, const std::string &nameCostLF, const eVector6 &reference)
Definition: horizon_manager.cpp:161
const std::set< std::string > & get_contacts(const unsigned long time)
Definition: horizon_manager.cpp:269
boost::shared_ptr< crocoddyl::ContactModelMultiple > Contact
Definition: fwd.hpp:116
boost::shared_ptr< crocoddyl::DifferentialActionModelContactFwdDynamics > DAM
Definition: fwd.hpp:114
void activateContactRF(const unsigned long time, const std::string &nameContactRF)
Definition: horizon_manager.cpp:85
Definition: horizon_manager.hpp:19
AMA ama(const unsigned long time)
Definition: horizon_manager.cpp:36
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
void setPoseReferenceLF(const unsigned long time, const std::string &nameCostLF, const pinocchio::SE3 &ref_placement)
Definition: horizon_manager.cpp:128
Definition: activation-quad-ref.hpp:19
void removeContactLF(const unsigned long time, const std::string &nameContactLF)
Definition: horizon_manager.cpp:90
boost::shared_ptr< crocoddyl::CostModelSum > Cost
Definition: fwd.hpp:115
const eVector3 & getFootTorque(const unsigned long time, const std::string &nameFootForceCost)
Definition: horizon_manager.cpp:229
const pinocchio::SE3 & getFootPoseReference(const unsigned long time, const std::string &nameCostFootPose)
Definition: horizon_manager.cpp:144
int supportSize(const unsigned long time)
Definition: horizon_manager.cpp:259
void activateContactLF(const unsigned long time, const std::string &nameContacttLF)
Definition: horizon_manager.cpp:80
void setBalancingTorque(const unsigned long time, const std::string &nameCostActuation, const Eigen::VectorXd &x)
Definition: horizon_manager.cpp:100
void set_ddp(const DDP &ddp)
Definition: horizon_manager.hpp:126
void removeContactRF(const unsigned long time, const std::string &nameContactRF)
Definition: horizon_manager.cpp:95
void setVelocityRefCOM(const unsigned long time, const std::string &nameCost, const eVector3 &ref_placement)
Definition: horizon_manager.cpp:153
void setSwingingLF(const unsigned long time, const std::string &nameContactLF, const std::string &nameContactRF, const std::string &nameForceContactLF)
Definition: horizon_manager.cpp:191
bool initialized_
Definition: horizon_manager.hpp:50
ADA ada(const unsigned long time)
Definition: horizon_manager.cpp:59
boost::shared_ptr< crocoddyl::ActionDataAbstract > ADA
Definition: fwd.hpp:112
Eigen::Matrix< double, 6, 1 > eVector6
Definition: fwd.hpp:105
void setSwingingRF(const unsigned long time, const std::string &nameContactLF, const std::string &nameContactRF, const std::string &nameForceContactRF)
Definition: horizon_manager.cpp:200
std::string rightFootName
Definition: horizon_manager.hpp:16
void recede()
Definition: horizon_manager.cpp:250
boost::shared_ptr< crocoddyl::IntegratedActionModelEuler > IAM
Definition: fwd.hpp:109
unsigned long size()
Definition: horizon_manager.cpp:254
const eVector3 & getFootForce(const unsigned long time, const std::string &nameFootForceCost)
Definition: horizon_manager.cpp:216
DDP get_ddp()
Definition: horizon_manager.hpp:125
const Eigen::VectorXd & currentTorques(const Eigen::VectorXd &measured_x)
Definition: horizon_manager.cpp:294
void setForceReferenceRF(const unsigned long time, const std::string &nameCostRF, const eVector6 &reference)
Definition: horizon_manager.cpp:176
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
DAM dam(const unsigned long time)
Definition: horizon_manager.cpp:45
IAM iam(const unsigned long time)
Definition: horizon_manager.cpp:40