ocp.hpp
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2022 LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef SOBEC_OCP_WALK_HPP_
10 #define SOBEC_OCP_WALK_HPP_
11 
12 #include <crocoddyl/core/fwd.hpp>
13 #include <crocoddyl/core/solvers/fddp.hpp>
14 #include <crocoddyl/multibody/fwd.hpp>
15 #include <crocoddyl/multibody/states/multibody.hpp>
16 #include <pinocchio/spatial/force.hpp>
17 
19 #include "sobec/fwd.hpp"
20 
21 namespace sobec {
22 using namespace crocoddyl;
27 struct OCPWalkParams {
28  double DT;
29  std::vector<std::string> mainJointIds;
30  Eigen::Vector2d baumgartGains;
31  Eigen::VectorXd stateImportance;
32  Eigen::VectorXd stateTerminalImportance;
33  Eigen::VectorXd controlImportance;
34  Eigen::VectorXd vcomImportance;
35  Eigen::VectorXd forceImportance;
36 
37  Eigen::Vector3d vcomRef;
38 
39  double footSize;
42 
45  double comWeight;
46  double vcomWeight;
47  double copWeight;
56  double flyHighSlope;
57  double flyHighWeight;
61  double kktDamping;
65 
66  void readParamsFromYamlStr(std::string& StringToParse);
67  void readParamsFromYamlFile(const std::string& Filename);
68 };
69 
71  boost::shared_ptr<pinocchio::Model> model;
72  boost::shared_ptr<pinocchio::Data> data;
73  std::vector<pinocchio::FrameIndex> contactIds;
74  std::map<pinocchio::FrameIndex, pinocchio::FrameIndex> towIds, heelIds;
75  Eigen::VectorXd x0;
76  Eigen::Vector3d com0;
78 
79  OCPRobotWrapper(boost::shared_ptr<pinocchio::Model> model,
80  const std::string& contactKey,
81  const std::string& referencePosture = "half_sitting");
82 };
83 
84 Eigen::MatrixXd computeWeightShareSmoothProfile(
85  const Eigen::Ref<const Eigen::MatrixXd> contact_pattern, int duration,
86  double saturation);
87 
88 class OCPWalk {
89  typedef typename MathBaseTpl<double>::VectorXs VectorXd;
90  typedef typename MathBaseTpl<double>::VectorXs Vector3d;
91  typedef typename Eigen::Matrix<double, Eigen::Dynamic, 2> MatrixX2d;
92  typedef typename Eigen::Matrix<double, Eigen::Dynamic, 6> MatrixX6d;
93  typedef std::vector<AMA> ActionList;
94  typedef
95  typename crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<double>
96  DAM;
97 
98  public:
99  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100 
101  explicit OCPWalk(boost::shared_ptr<OCPRobotWrapper> robot,
102  boost::shared_ptr<OCPWalkParams> params,
103  const Eigen::Ref<const Eigen::MatrixXd> contact_pattern);
104 
105  virtual ~OCPWalk() {}
106 
107  std::vector<AMA> buildRunningModels();
108  AMA buildTerminalModel();
109  void buildSolver();
110  std::pair<std::vector<Eigen::VectorXd>, std::vector<Eigen::VectorXd>>
111  buildInitialGuess();
112 
113  void computeReferenceForces();
114 
115  public:
117  boost::shared_ptr<ShootingProblem> problem;
118 
120  boost::shared_ptr<SolverFDDP> solver;
121 
123  boost::shared_ptr<StateMultibody> state;
124 
125  boost::shared_ptr<ActuationModelFloatingBase> actuation;
126 
127  std::vector<std::vector<pinocchio::Force>> referenceForces;
128  Eigen::MatrixXd contact_pattern;
129 
130  protected:
131  boost::shared_ptr<OCPWalkParams> params;
132  boost::shared_ptr<OCPRobotWrapper> robot;
133 };
134 
135 } // namespace sobec
136 
137 #endif // SOBEC_OCP_WALK_HPP_
double comWeight
Definition: ocp.hpp:45
double groundColWeight
Definition: ocp.hpp:58
boost::shared_ptr< OCPWalkParams > params
Definition: ocp.hpp:131
double kktDamping
Definition: ocp.hpp:61
Eigen::Vector3d vcomRef
Definition: ocp.hpp:37
boost::shared_ptr< ActuationModelFloatingBase > actuation
Definition: ocp.hpp:125
double impactVelocityWeight
Definition: ocp.hpp:52
double footSize
Definition: ocp.hpp:39
boost::shared_ptr< pinocchio::Data > data
Definition: ocp.hpp:72
Eigen::MatrixXd contact_pattern
Definition: ocp.hpp:128
double refMainJointsAtImpactWeight
Definition: ocp.hpp:54
double flyHighSlope
Definition: ocp.hpp:56
Eigen::VectorXd stateImportance
Definition: ocp.hpp:31
std::vector< std::string > mainJointIds
Definition: ocp.hpp:29
double verticalFootVelWeight
Definition: ocp.hpp:55
Eigen::Vector2d baumgartGains
Definition: ocp.hpp:30
Eigen::VectorXd x0
Definition: ocp.hpp:75
virtual ~OCPWalk()
Definition: ocp.hpp:105
double solver_th_stop
Definition: ocp.hpp:63
double refForceWeight
Definition: ocp.hpp:50
double robotGravityForce
Definition: ocp.hpp:77
Definition: ocp.hpp:88
Definition: ocp.hpp:70
boost::shared_ptr< OCPRobotWrapper > robot
Definition: ocp.hpp:132
double DT
Definition: ocp.hpp:28
double feetCollisionWeight
Definition: ocp.hpp:60
double copWeight
Definition: ocp.hpp:47
std::map< pinocchio::FrameIndex, pinocchio::FrameIndex > towIds
Definition: ocp.hpp:74
Eigen::VectorXd forceImportance
Definition: ocp.hpp:35
double conePenaltyWeight
Definition: ocp.hpp:48
std::vector< pinocchio::FrameIndex > contactIds
Definition: ocp.hpp:73
double impactRotationWeight
Definition: ocp.hpp:53
double coneAxisWeight
Definition: ocp.hpp:49
double impactAltitudeWeight
Definition: ocp.hpp:51
boost::shared_ptr< crocoddyl::DifferentialActionModelContactFwdDynamics > DAM
Definition: fwd.hpp:114
Definition: activation-quad-ref.hpp:19
Eigen::VectorXd vcomImportance
Definition: ocp.hpp:34
Eigen::MatrixXd computeWeightShareSmoothProfile(const Eigen::Ref< const Eigen::MatrixXd > contact_pattern, int duration, double saturation)
Definition: ocp-ref-forces.cpp:16
boost::shared_ptr< StateMultibody > state
Keep a direct reference to the terminal state.
Definition: ocp.hpp:123
double refTorqueWeight
Definition: ocp.hpp:44
double vcomWeight
Definition: ocp.hpp:46
double flyHighWeight
Definition: ocp.hpp:57
bool withNormalForceBoundOnly
Definition: ocp.hpp:40
OCP builder.
Definition: ocp.hpp:27
double footMinimalDistance
Definition: ocp.hpp:59
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
double stateTerminalWeight
Definition: ocp.hpp:62
boost::shared_ptr< SolverFDDP > solver
the OCP solver.
Definition: ocp.hpp:120
double minimalNormalForce
Definition: ocp.hpp:41
Eigen::VectorXd controlImportance
Definition: ocp.hpp:33
std::vector< std::vector< pinocchio::Force > > referenceForces
Definition: ocp.hpp:127
double refStateWeight
Definition: ocp.hpp:43
Eigen::VectorXd stateTerminalImportance
Definition: ocp.hpp:32
boost::shared_ptr< pinocchio::Model > model
Definition: ocp.hpp:71
Eigen::Vector3d com0
Definition: ocp.hpp:76
int transitionDuration
Definition: ocp.hpp:64
boost::shared_ptr< ShootingProblem > problem
the OCP problem used for solving.
Definition: ocp.hpp:117