horizon_manager.hpp
Go to the documentation of this file.
1 #ifndef SOBEC_HORIZON_MANAGER
2 #define SOBEC_HORIZON_MANAGER
3 
4 #include <Eigen/Dense>
5 #include <string>
6 #include <vector>
7 
8 #include "sobec/fwd.hpp"
11 
12 namespace sobec {
14  public:
15  std::string leftFootName = "left_sole_link";
16  std::string rightFootName = "right_sole_link";
17 };
18 
20  private:
21  HorizonManagerSettings settings_;
22  DDP ddp_;
23 
24  // prealocated memory:
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_;
29  unsigned long size_;
30  Eigen::VectorXd command_torque_;
31  Eigen::VectorXd tr_error_;
32  Eigen::VectorXd K_tr_error_;
33  eVector3 foot_torque_, foot_force_;
34  pinocchio::SE3 pose_;
35  int support_size_;
36  std::set<std::string> active_contacts_;
37 
38  public:
40 
41  HorizonManager(const HorizonManagerSettings &horizonSettings,
42  const Eigen::VectorXd &x0,
43  const std::vector<AMA> &runningModels,
44  const AMA &terminalModel);
45 
46  void initialize(const HorizonManagerSettings &horizonSettings,
47  const Eigen::VectorXd &x0,
48  const std::vector<AMA> &runningModels,
49  const AMA &terminalModel);
50  bool initialized_ = false;
51 
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);
62 
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,
81  const eVector3 &ref_placement);
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,
92  const eVector6 &reference);
93  void setForceReferenceRF(const unsigned long time,
94  const std::string &nameCostRF,
95  const eVector6 &reference);
96 
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);
106 
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);
111 
112  const std::set<std::string> &get_contacts(const unsigned long time);
113 
114  void recede(const AMA &new_model, const ADA &new_data);
115  void recede(const AMA &new_model);
116  void recede();
117 
118  int supportSize(const unsigned long time);
119  unsigned long size();
120 
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);
124 
125  DDP get_ddp() { return ddp_; }
126  void set_ddp(const DDP &ddp) { ddp_ = ddp; }
127 };
128 } // namespace sobec
129 #endif // SOBEC_HORIZON_MANAGER
sobec::IAD
boost::shared_ptr< crocoddyl::IntegratedActionDataEuler > IAD
Definition: fwd.hpp:110
sobec::HorizonManager::initialize
void initialize(const HorizonManagerSettings &horizonSettings, const Eigen::VectorXd &x0, const std::vector< AMA > &runningModels, const AMA &terminalModel)
Definition: horizon_manager.cpp:18
sobec::HorizonManager::setActuationReference
void setActuationReference(const unsigned long time, const std::string &nameCostActuation, const Eigen::VectorXd &reference)
Definition: horizon_manager.cpp:120
sobec::HorizonManager::setDoubleSupport
void setDoubleSupport(const unsigned long time, const std::string &nameContactLF, const std::string &nameContactRF)
Definition: horizon_manager.cpp:209
sobec::HorizonManager::iad
IAD iad(const unsigned long time)
Definition: horizon_manager.cpp:63
sobec::HorizonManager::costs
Cost costs(const unsigned long time)
Definition: horizon_manager.cpp:51
sobec::HorizonManagerSettings::leftFootName
std::string leftFootName
Definition: horizon_manager.hpp:15
fwd.hpp
sobec::DDP
boost::shared_ptr< crocoddyl::SolverFDDP > DDP
Definition: fwd.hpp:117
sobec::HorizonManager::HorizonManager
HorizonManager()
Definition: horizon_manager.cpp:9
sobec::HorizonManager::solve
void solve(const Eigen::VectorXd &measured_x, const std::size_t ddpIteration, const bool is_feasible=false)
Definition: horizon_manager.cpp:275
sobec::HorizonManager::actuation
boost::shared_ptr< crocoddyl::ActuationModelFloatingBase > actuation(const unsigned long time)
Definition: horizon_manager.cpp:75
designer.hpp
sobec::HorizonManager::contacts
Contact contacts(const unsigned long time)
Definition: horizon_manager.cpp:55
sobec::HorizonManager::state
boost::shared_ptr< crocoddyl::StateMultibody > state(const unsigned long time)
Definition: horizon_manager.cpp:68
sobec::HorizonManager::setPoseReferenceRF
void setPoseReferenceRF(const unsigned long time, const std::string &nameCostRF, const pinocchio::SE3 &ref_placement)
Definition: horizon_manager.cpp:136
sobec::HorizonManagerSettings
Definition: horizon_manager.hpp:13
sobec::HorizonManager::setForceReferenceLF
void setForceReferenceLF(const unsigned long time, const std::string &nameCostLF, const eVector6 &reference)
Definition: horizon_manager.cpp:161
sobec::HorizonManager::get_contacts
const std::set< std::string > & get_contacts(const unsigned long time)
Definition: horizon_manager.cpp:269
sobec::Contact
boost::shared_ptr< crocoddyl::ContactModelMultiple > Contact
Definition: fwd.hpp:116
sobec::DAM
boost::shared_ptr< crocoddyl::DifferentialActionModelContactFwdDynamics > DAM
Definition: fwd.hpp:114
sobec::HorizonManager::activateContactRF
void activateContactRF(const unsigned long time, const std::string &nameContactRF)
Definition: horizon_manager.cpp:85
sobec::HorizonManager
Definition: horizon_manager.hpp:19
sobec::HorizonManager::ama
AMA ama(const unsigned long time)
Definition: horizon_manager.cpp:36
sobec::AMA
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
sobec::HorizonManager::setPoseReferenceLF
void setPoseReferenceLF(const unsigned long time, const std::string &nameCostLF, const pinocchio::SE3 &ref_placement)
Definition: horizon_manager.cpp:128
sobec
Definition: activation-quad-ref.hpp:19
sobec::HorizonManager::removeContactLF
void removeContactLF(const unsigned long time, const std::string &nameContactLF)
Definition: horizon_manager.cpp:90
sobec::Cost
boost::shared_ptr< crocoddyl::CostModelSum > Cost
Definition: fwd.hpp:115
sobec::HorizonManager::getFootTorque
const eVector3 & getFootTorque(const unsigned long time, const std::string &nameFootForceCost)
Definition: horizon_manager.cpp:229
sobec::HorizonManager::getFootPoseReference
const pinocchio::SE3 & getFootPoseReference(const unsigned long time, const std::string &nameCostFootPose)
Definition: horizon_manager.cpp:144
sobec::HorizonManager::supportSize
int supportSize(const unsigned long time)
Definition: horizon_manager.cpp:259
sobec::HorizonManager::activateContactLF
void activateContactLF(const unsigned long time, const std::string &nameContacttLF)
Definition: horizon_manager.cpp:80
sobec::HorizonManager::setBalancingTorque
void setBalancingTorque(const unsigned long time, const std::string &nameCostActuation, const Eigen::VectorXd &x)
Definition: horizon_manager.cpp:100
sobec::HorizonManager::set_ddp
void set_ddp(const DDP &ddp)
Definition: horizon_manager.hpp:126
sobec::newcontacts::x
@ x
Definition: contact1d.hpp:26
sobec::HorizonManager::removeContactRF
void removeContactRF(const unsigned long time, const std::string &nameContactRF)
Definition: horizon_manager.cpp:95
sobec::HorizonManager::setVelocityRefCOM
void setVelocityRefCOM(const unsigned long time, const std::string &nameCost, const eVector3 &ref_placement)
Definition: horizon_manager.cpp:153
sobec::HorizonManager::setSwingingLF
void setSwingingLF(const unsigned long time, const std::string &nameContactLF, const std::string &nameContactRF, const std::string &nameForceContactLF)
Definition: horizon_manager.cpp:191
sobec::HorizonManager::initialized_
bool initialized_
Definition: horizon_manager.hpp:50
sobec::HorizonManager::ada
ADA ada(const unsigned long time)
Definition: horizon_manager.cpp:59
sobec::ADA
boost::shared_ptr< crocoddyl::ActionDataAbstract > ADA
Definition: fwd.hpp:112
sobec::eVector6
Eigen::Matrix< double, 6, 1 > eVector6
Definition: fwd.hpp:105
sobec::HorizonManager::setSwingingRF
void setSwingingRF(const unsigned long time, const std::string &nameContactLF, const std::string &nameContactRF, const std::string &nameForceContactRF)
Definition: horizon_manager.cpp:200
model_factory.hpp
sobec::HorizonManagerSettings::rightFootName
std::string rightFootName
Definition: horizon_manager.hpp:16
sobec::HorizonManager::recede
void recede()
Definition: horizon_manager.cpp:250
sobec::IAM
boost::shared_ptr< crocoddyl::IntegratedActionModelEuler > IAM
Definition: fwd.hpp:109
sobec::HorizonManager::size
unsigned long size()
Definition: horizon_manager.cpp:254
sobec::HorizonManager::getFootForce
const eVector3 & getFootForce(const unsigned long time, const std::string &nameFootForceCost)
Definition: horizon_manager.cpp:216
sobec::HorizonManager::get_ddp
DDP get_ddp()
Definition: horizon_manager.hpp:125
sobec::HorizonManager::currentTorques
const Eigen::VectorXd & currentTorques(const Eigen::VectorXd &measured_x)
Definition: horizon_manager.cpp:294
sobec::HorizonManager::setForceReferenceRF
void setForceReferenceRF(const unsigned long time, const std::string &nameCostRF, const eVector6 &reference)
Definition: horizon_manager.cpp:176
sobec::eVector3
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
sobec::HorizonManager::dam
DAM dam(const unsigned long time)
Definition: horizon_manager.cpp:45
sobec::HorizonManager::iam
IAM iam(const unsigned long time)
Definition: horizon_manager.cpp:40