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);
50 bool initialized_ =
false;
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);
57 Cost costs(
const unsigned long time);
58 Contact contacts(
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);
63 void setActuationReference(
const unsigned long time,
64 const std::string &nameCostActuation,
65 const Eigen::VectorXd &reference);
66 void setBalancingTorque(
const unsigned long time,
67 const std::string &nameCostActuation,
68 const Eigen::VectorXd &
x);
69 void setBalancingTorque(
const unsigned long time,
70 const std::string &nameCostActuation,
71 const std::string &nameCostState);
72 void setPoseReferenceLF(
const unsigned long time,
73 const std::string &nameCostLF,
74 const pinocchio::SE3 &ref_placement);
75 void setPoseReferenceRF(
const unsigned long time,
76 const std::string &nameCostRF,
77 const pinocchio::SE3 &ref_placement);
78 const pinocchio::SE3 &getFootPoseReference(
79 const unsigned long time,
const std::string &nameCostFootPose);
80 void setVelocityRefCOM(
const unsigned long time,
const std::string &nameCost,
82 void activateContactLF(
const unsigned long time,
83 const std::string &nameContacttLF);
84 void activateContactRF(
const unsigned long time,
85 const std::string &nameContactRF);
86 void removeContactLF(
const unsigned long time,
87 const std::string &nameContactLF);
88 void removeContactRF(
const unsigned long time,
89 const std::string &nameContactRF);
90 void setForceReferenceLF(
const unsigned long time,
91 const std::string &nameCostLF,
93 void setForceReferenceRF(
const unsigned long time,
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);
103 void setDoubleSupport(
const unsigned long time,
104 const std::string &nameContactLF,
105 const std::string &nameContactRF);
107 const eVector3 &getFootForce(
const unsigned long time,
108 const std::string &nameFootForceCost);
109 const eVector3 &getFootTorque(
const unsigned long time,
110 const std::string &nameFootForceCost);
112 const std::set<std::string> &get_contacts(
const unsigned long time);
114 void recede(
const AMA &new_model,
const ADA &new_data);
115 void recede(
const AMA &new_model);
118 int supportSize(
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 ¤tTorques(
const Eigen::VectorXd &measured_x);
129 #endif // SOBEC_HORIZON_MANAGER boost::shared_ptr< crocoddyl::SolverFDDP > DDP
Definition: fwd.hpp:117
Definition: horizon_manager.hpp:19
boost::shared_ptr< crocoddyl::IntegratedActionDataEuler > IAD
Definition: fwd.hpp:110
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
Eigen::Matrix< double, 6, 1 > eVector6
Definition: fwd.hpp:105
std::string leftFootName
Definition: horizon_manager.hpp:15
boost::shared_ptr< crocoddyl::ContactModelMultiple > Contact
Definition: fwd.hpp:116
boost::shared_ptr< crocoddyl::DifferentialActionModelContactFwdDynamics > DAM
Definition: fwd.hpp:114
Definition: activation-quad-ref.hpp:19
Definition: horizon_manager.hpp:13
std::string rightFootName
Definition: horizon_manager.hpp:16
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
void set_ddp(const DDP &ddp)
Definition: horizon_manager.hpp:126
boost::shared_ptr< crocoddyl::ActionDataAbstract > ADA
Definition: fwd.hpp:112
boost::shared_ptr< crocoddyl::IntegratedActionModelEuler > IAM
Definition: fwd.hpp:109
boost::shared_ptr< crocoddyl::CostModelSum > Cost
Definition: fwd.hpp:115
DDP get_ddp()
Definition: horizon_manager.hpp:125