model_factory.hpp
Go to the documentation of this file.
1 #ifndef SOBEC_MODEL_FACTORY
2 #define SOBEC_MODEL_FACTORY
3 
4 #include <pinocchio/fwd.hpp>
5 // include pinocchio first
6 #include <Eigen/Dense>
7 #include <string>
8 #include <vector>
9 
10 #include "sobec/fwd.hpp"
12 
13 namespace sobec {
14 
15 enum Support { LEFT, RIGHT, DOUBLE };
16 
18  public:
19  // Timing
20  double timeStep = 0.01;
21 
22  // physics
23  eVector3 gravity = eVector3(0, 0, -9.81);
24 
25  // geometry
26  double footSize = 0.05; //[m]
27 
28  double mu = 0.3;
29  eVector2 coneBox = eVector2(0.1, 0.05); // half lenght and width
30  double minNforce = 200.0;
31  double maxNforce = 1200;
32 
33  double comHeight = 0.87;
34  double omega = -comHeight / gravity(2);
35 
36  // Croco configuration
37  double wFootPlacement = 0; // 1000;
38  double wStateReg = 0; // 100;
39  double wControlReg = 0; // 0.001;
40  double wLimit = 0; // 1e3;
41  double wVCoM = 0; // 0;
42  double wWrenchCone = 0; // 0.05;
43  double wFootTrans = 0; // 100;
44  double wFootXYTrans = 0; // 0;
45  double wFootRot = 0; // 100;
46  double wGroundCol = 0; // 0.05;
47  double wCoP = 0; // 1;
48 
49  Eigen::VectorXd stateWeights;
50  Eigen::VectorXd controlWeights;
51 
52  double th_stop = 1e-6; // threshold for stopping criterion
53  double th_grad = 1e-9; // threshold for zero gradient.
54 };
55 class ModelMaker {
56  private:
57  ModelMakerSettings settings_;
58  RobotDesigner designer_;
59 
60  boost::shared_ptr<crocoddyl::StateMultibody> state_;
61  boost::shared_ptr<crocoddyl::ActuationModelFloatingBase> actuation_;
62  Eigen::VectorXd x0_;
63 
64  public:
65  ModelMaker();
66  ModelMaker(const ModelMakerSettings &settings, const RobotDesigner &design);
67  void initialize(const ModelMakerSettings &settings,
68  const RobotDesigner &design);
69  bool initialized_ = false;
70 
73 
74  std::vector<AMA> formulateHorizon(const std::vector<Support> &supports);
75  std::vector<AMA> formulateHorizon(const int &T);
76  ModelMakerSettings &get_settings() { return settings_; }
77 
78  // formulation parts:
79  void defineFeetContact(Contact &contactCollector,
80  const Support &support = Support::DOUBLE);
81  void defineFeetWrenchCost(Cost &costCollector,
82  const Support &support = Support::DOUBLE);
83  void defineFeetTracking(Cost &costCollector);
84  void definePostureTask(Cost &costCollector);
85  void defineActuationTask(Cost &costCollector);
86  void defineJointLimits(Cost &costCollector);
87  void defineCoMVelocity(Cost &costCollector);
88  void defineCoPTask(Cost &costCollector,
89  const Support &support = Support::DOUBLE);
90 
91  boost::shared_ptr<crocoddyl::StateMultibody> getState() { return state_; }
92  void setState(const boost::shared_ptr<crocoddyl::StateMultibody> &new_state) {
93  state_ = new_state;
94  }
95  boost::shared_ptr<crocoddyl::ActuationModelFloatingBase> getActuation() {
96  return actuation_;
97  }
99  const boost::shared_ptr<crocoddyl::ActuationModelFloatingBase>
100  &new_actuation) {
101  actuation_ = new_actuation;
102  }
103 };
104 
105 } // namespace sobec
106 #endif // SOBEC_MODEL_FACTORY
Definition: model_factory.hpp:55
boost::shared_ptr< crocoddyl::ActuationModelFloatingBase > getActuation()
Definition: model_factory.hpp:95
void defineJointLimits(Cost &costCollector)
Definition: model_factory.cpp:178
ModelMaker()
Definition: model_factory.cpp:10
void defineFeetContact(Contact &contactCollector, const Support &support=Support::DOUBLE)
Definition: model_factory.cpp:33
AMA formulate_stair_climber(const Support &support=Support::DOUBLE)
boost::shared_ptr< crocoddyl::StateMultibody > getState()
Definition: model_factory.hpp:91
void defineCoMVelocity(Cost &costCollector)
Definition: model_factory.cpp:205
void defineCoPTask(Cost &costCollector, const Support &support=Support::DOUBLE)
Definition: model_factory.cpp:216
void defineActuationTask(Cost &costCollector)
Definition: model_factory.cpp:161
void setActuation(const boost::shared_ptr< crocoddyl::ActuationModelFloatingBase > &new_actuation)
Definition: model_factory.hpp:98
ModelMakerSettings & get_settings()
Definition: model_factory.hpp:76
void defineFeetWrenchCost(Cost &costCollector, const Support &support=Support::DOUBLE)
Definition: model_factory.cpp:57
void defineFeetTracking(Cost &costCollector)
Definition: model_factory.cpp:114
void setState(const boost::shared_ptr< crocoddyl::StateMultibody > &new_state)
Definition: model_factory.hpp:92
std::vector< AMA > formulateHorizon(const std::vector< Support > &supports)
Definition: model_factory.cpp:272
void definePostureTask(Cost &costCollector)
Definition: model_factory.cpp:143
void initialize(const ModelMakerSettings &settings, const RobotDesigner &design)
Definition: model_factory.cpp:17
AMA formulateStepTracker(const Support &support=Support::DOUBLE)
Definition: model_factory.cpp:248
bool initialized_
Definition: model_factory.hpp:69
Definition: designer.hpp:28
Definition: activation-quad-ref.hpp:19
Eigen::Vector2d eVector2
Definition: fwd.hpp:108
boost::shared_ptr< crocoddyl::ContactModelMultiple > Contact
Definition: fwd.hpp:116
boost::shared_ptr< crocoddyl::CostModelSum > Cost
Definition: fwd.hpp:115
Support
Definition: model_factory.hpp:15
@ LEFT
Definition: model_factory.hpp:15
@ RIGHT
Definition: model_factory.hpp:15
@ DOUBLE
Definition: model_factory.hpp:15
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
Definition: model_factory.hpp:17
eVector3 gravity
Definition: model_factory.hpp:23
double wFootRot
Definition: model_factory.hpp:45
double wFootTrans
Definition: model_factory.hpp:43
double maxNforce
Definition: model_factory.hpp:31
double th_stop
Definition: model_factory.hpp:52
double wVCoM
Definition: model_factory.hpp:41
double wFootPlacement
Definition: model_factory.hpp:37
double comHeight
Definition: model_factory.hpp:33
double wCoP
Definition: model_factory.hpp:47
double wGroundCol
Definition: model_factory.hpp:46
double wFootXYTrans
Definition: model_factory.hpp:44
double omega
Definition: model_factory.hpp:34
double wControlReg
Definition: model_factory.hpp:39
Eigen::VectorXd controlWeights
Definition: model_factory.hpp:50
double mu
Definition: model_factory.hpp:28
double timeStep
Definition: model_factory.hpp:20
Eigen::VectorXd stateWeights
Definition: model_factory.hpp:49
double wStateReg
Definition: model_factory.hpp:38
double th_grad
Definition: model_factory.hpp:53
eVector2 coneBox
Definition: model_factory.hpp:29
double wWrenchCone
Definition: model_factory.hpp:42
double wLimit
Definition: model_factory.hpp:40
double footSize
Definition: model_factory.hpp:26
double minNforce
Definition: model_factory.hpp:30