fwd.hpp
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, INRIA
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef SOBEC_FWD_HPP_
10 #define SOBEC_FWD_HPP_
11 
12 #include <crocoddyl/core/action-base.hpp>
13 #include <crocoddyl/core/activations/quadratic-barrier.hpp>
14 #include <crocoddyl/core/activations/quadratic-flat-log.hpp>
15 #include <crocoddyl/core/activations/weighted-quadratic.hpp>
16 #include <crocoddyl/core/fwd.hpp>
17 #include <crocoddyl/core/integrator/euler.hpp>
18 #include <crocoddyl/core/optctrl/shooting.hpp>
19 #include <crocoddyl/core/residuals/control.hpp>
20 #include <crocoddyl/core/solver-base.hpp>
21 #include <crocoddyl/core/solvers/fddp.hpp>
22 #include <crocoddyl/core/utils/exception.hpp>
23 #include <crocoddyl/multibody/actions/contact-fwddyn.hpp>
24 #include <crocoddyl/multibody/actuations/floating-base.hpp>
25 #include <crocoddyl/multibody/contacts/contact-6d.hpp>
26 #include <crocoddyl/multibody/contacts/multiple-contacts.hpp>
27 #include <crocoddyl/multibody/frames.hpp>
28 #include <crocoddyl/multibody/fwd.hpp>
29 #include <crocoddyl/multibody/residuals/com-position.hpp>
30 #include <crocoddyl/multibody/residuals/contact-control-gravity.hpp>
31 #include <crocoddyl/multibody/residuals/contact-wrench-cone.hpp>
32 #include <crocoddyl/multibody/residuals/frame-placement.hpp>
33 #include <crocoddyl/multibody/residuals/frame-rotation.hpp>
34 #include <crocoddyl/multibody/residuals/frame-translation.hpp>
35 #include <crocoddyl/multibody/residuals/frame-velocity.hpp>
36 #include <crocoddyl/multibody/residuals/state.hpp>
37 #include <crocoddyl/multibody/states/multibody.hpp>
38 #include <pinocchio/algorithm/center-of-mass.hpp>
39 #include <pinocchio/algorithm/frames.hpp>
40 #include <pinocchio/algorithm/joint-configuration.hpp>
41 #include <pinocchio/algorithm/model.hpp>
42 #include <pinocchio/fwd.hpp>
43 #include <pinocchio/multibody/data.hpp>
44 #include <pinocchio/multibody/model.hpp>
45 
48 
49 namespace sobec {
50 
51 // Cost COM-vel
52 template <typename Scalar>
53 class ResidualModelCoMVelocityTpl;
54 template <typename Scalar>
58 
59 // Cost COP
60 template <typename Scalar>
62 template <typename Scalar>
66 
67 // Cost CenterOfFriction
68 template <typename Scalar>
70 template <typename Scalar>
74 
75 // Cost velocity collision
76 template <typename Scalar>
78 template <typename Scalar>
82 
83 // Cost fly high
84 template <typename Scalar>
86 template <typename Scalar>
90 
91 // Cost fly high
92 template <typename Scalar>
94 template <typename Scalar>
98 
99 // Activation quad-ref
100 template <typename Scalar>
103 typedef boost::shared_ptr<ActivationModelQuadRef> ActivationModelQuadRefPtr;
104 
105 typedef Eigen::Matrix<double, 6, 1> eVector6;
106 typedef Eigen::Matrix<double, 4, 1> eVector4;
107 typedef Eigen::Vector3d eVector3;
108 typedef Eigen::Vector2d eVector2;
109 typedef boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> IAM;
110 typedef boost::shared_ptr<crocoddyl::IntegratedActionDataEuler> IAD;
111 typedef boost::shared_ptr<crocoddyl::ActionModelAbstract> AMA;
112 typedef boost::shared_ptr<crocoddyl::ActionDataAbstract> ADA;
113 typedef boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
115 typedef boost::shared_ptr<crocoddyl::CostModelSum> Cost;
116 typedef boost::shared_ptr<crocoddyl::ContactModelMultiple> Contact;
117 typedef boost::shared_ptr<crocoddyl::SolverFDDP> DDP;
118 
119 typedef boost::shared_ptr<crocoddyl::ResidualModelFramePlacement>
121 typedef boost::shared_ptr<crocoddyl::ResidualModelContactWrenchCone>
123 
124 // State LPF
125 template <typename Scalar>
128 
129 // IAM LPF
130 template <typename Scalar>
133 template <typename Scalar>
136 
137 // OCP
138 class OCPWalkParam;
139 class OCPWalk;
140 // MPC
141 class MPCWalk;
142 
143 } // namespace sobec
144 
145 namespace sobec {
146 namespace newcontacts {
147 
148 // contact 3D
149 template <typename Scalar>
152 template <typename Scalar>
155 
156 // contact 1D
157 template <typename Scalar>
160 template <typename Scalar>
163 
164 // multiple contacts
165 template <typename Scalar>
168 
169 // DAM contact fwd dynamics
170 template <typename Scalar>
174 
175 // Residual contact force
176 template <typename Scalar>
179 
186 };
187 
188 } // namespace newcontacts
189 } // namespace sobec
190 
191 #endif // SOBEC_FWD_HPP_
Definition: contact3d.hpp:155
boost::shared_ptr< crocoddyl::SolverFDDP > DDP
Definition: fwd.hpp:117
Cost penalizing high horizontal velocity near zero altitude.
Definition: residual-fly-high.hpp:32
IntegratedActionDataLPFTpl< double > IntegratedActionDataLPF
Definition: fwd.hpp:134
ResidualModelCenterOfPressureTpl< double > ResidualModelCenterOfPressure
Definition: fwd.hpp:63
ContactModelMultipleTpl< double > ContactModelMultiple
Definition: fwd.hpp:166
Definition: contact1d.hpp:29
Definition: mpc.hpp:62
IntegratedActionModelLPFTpl< double > IntegratedActionModelLPF
Definition: fwd.hpp:131
DifferentialActionModelContactFwdDynamicsTpl< double > DifferentialActionModelContactFwdDynamics
Definition: fwd.hpp:171
Definition: fwd.hpp:184
boost::shared_ptr< crocoddyl::IntegratedActionDataEuler > IAD
Definition: fwd.hpp:110
Definition: contact1d.hpp:173
ResidualDataCenterOfFrictionTpl< double > ResidualDataCenterOfFriction
Definition: fwd.hpp:73
ContactType
Definition: fwd.hpp:180
Eigen::Vector3d eVector3
Definition: fwd.hpp:107
Eigen::Matrix< double, 6, 1 > eVector6
Definition: fwd.hpp:105
StateLPFTpl< double > StateLPF
Definition: fwd.hpp:126
ContactModel3DTpl< double > ContactModel3D
Definition: fwd.hpp:150
boost::shared_ptr< crocoddyl::ResidualModelContactWrenchCone > ResidualModelContactWrenchConePtr
Definition: fwd.hpp:122
ContactData1DTpl< double > ContactData1D
Definition: fwd.hpp:161
Eigen::Vector2d eVector2
Definition: fwd.hpp:108
Definition: ocp.hpp:88
Definition: fwd.hpp:185
Definition: residual-feet-collision.hpp:119
Define a contact force residual function.
Definition: contact-force.hpp:53
Definition: residual-fly-high.hpp:128
ResidualDataFlyHighTpl< double > ResidualDataFlyHigh
Definition: fwd.hpp:89
Cost penalizing distance between two frames r=||f1.translation-f2.translation||.
Definition: residual-feet-collision.hpp:29
ResidualDataCoMVelocityTpl< double > ResidualDataCoMVelocity
Definition: fwd.hpp:57
boost::shared_ptr< crocoddyl::ContactModelMultiple > Contact
Definition: fwd.hpp:116
Define a stack of contact models.
Definition: multiple-contacts.hpp:37
boost::shared_ptr< crocoddyl::DifferentialActionModelContactFwdDynamics > DAM
Definition: fwd.hpp:114
Definition: activation-quad-ref.hpp:19
ResidualModelVelCollisionTpl< double > ResidualModelVelCollision
Definition: fwd.hpp:79
ResidualDataVelCollisionTpl< double > ResidualDataVelCollision
Definition: fwd.hpp:81
ResidualModelCoMVelocityTpl< double > ResidualModelCoMVelocity
Definition: fwd.hpp:55
Definition: fwd.hpp:182
boost::shared_ptr< ActivationModelQuadRef > ActivationModelQuadRefPtr
Definition: fwd.hpp:103
Definition: activation-quad-ref.hpp:23
Definition: action.hpp:165
ContactModel1DTpl< double > ContactModel1D
Definition: fwd.hpp:158
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:111
ResidualModelFlyHighTpl< double > ResidualModelFlyHigh
Definition: fwd.hpp:87
ResidualDataCenterOfPressureTpl< double > ResidualDataCenterOfPressure
Definition: fwd.hpp:65
boost::shared_ptr< crocoddyl::ActionDataAbstract > ADA
Definition: fwd.hpp:112
ActivationModelQuadRefTpl< double > ActivationModelQuadRef
Definition: fwd.hpp:101
Definition: action.hpp:25
Definition: residual-cop.hpp:118
ContactData3DTpl< double > ContactData3D
Definition: fwd.hpp:153
ResidualModelFeetCollisionTpl< double > ResidualModelFeetCollision
Definition: fwd.hpp:95
ResidualDataFeetCollisionTpl< double > ResidualDataFeetCollision
Definition: fwd.hpp:97
boost::shared_ptr< crocoddyl::IntegratedActionModelEuler > IAM
Definition: fwd.hpp:109
Center of friction residual.
Definition: residual-center-of-friction.hpp:41
ResidualModelContactForceTpl< double > ResidualModelContactForce
Definition: fwd.hpp:177
Definition: residual-center-of-friction.hpp:118
boost::shared_ptr< crocoddyl::CostModelSum > Cost
Definition: fwd.hpp:115
boost::shared_ptr< crocoddyl::ResidualModelFramePlacement > ResidualModelFramePlacementPtr
Definition: fwd.hpp:120
Definition: fwd.hpp:183
ResidualModelCenterOfFrictionTpl< double > ResidualModelCenterOfFriction
Definition: fwd.hpp:71
CoM velocity residual.
Definition: residual-com-velocity.hpp:73
Definition: contact3d.hpp:28
Eigen::Matrix< double, 4, 1 > eVector4
Definition: fwd.hpp:106
Definition: residual-com-velocity.hpp:23
Differential action model for contact forward dynamics in multibody systems.
Definition: contact-fwddyn.hpp:85
Definition: state.hpp:19
COP residual.
Definition: residual-cop.hpp:41