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/smooth-1norm.hpp>
16 #include <crocoddyl/core/activations/weighted-quadratic.hpp>
17 #include <crocoddyl/core/fwd.hpp>
18 #include <crocoddyl/core/integrator/euler.hpp>
19 #include <crocoddyl/core/optctrl/shooting.hpp>
20 #include <crocoddyl/core/residuals/control.hpp>
21 #include <crocoddyl/core/solver-base.hpp>
22 #include <crocoddyl/core/solvers/fddp.hpp>
23 #include <crocoddyl/core/utils/exception.hpp>
24 #include <crocoddyl/multibody/actions/contact-fwddyn.hpp>
25 #include <crocoddyl/multibody/actuations/floating-base.hpp>
26 #include <crocoddyl/multibody/contacts/contact-6d.hpp>
27 #include <crocoddyl/multibody/contacts/multiple-contacts.hpp>
28 #include <crocoddyl/multibody/frames.hpp>
29 #include <crocoddyl/multibody/fwd.hpp>
30 #include <crocoddyl/multibody/residuals/com-position.hpp>
31 #include <crocoddyl/multibody/residuals/contact-control-gravity.hpp>
32 #include <crocoddyl/multibody/residuals/contact-force.hpp>
33 #include <crocoddyl/multibody/residuals/contact-wrench-cone.hpp>
34 #include <crocoddyl/multibody/residuals/frame-placement.hpp>
35 #include <crocoddyl/multibody/residuals/frame-rotation.hpp>
36 #include <crocoddyl/multibody/residuals/frame-translation.hpp>
37 #include <crocoddyl/multibody/residuals/frame-velocity.hpp>
38 #include <crocoddyl/multibody/residuals/state.hpp>
39 #include <crocoddyl/multibody/states/multibody.hpp>
40 #include <pinocchio/algorithm/center-of-mass.hpp>
41 #include <pinocchio/algorithm/frames.hpp>
42 #include <pinocchio/algorithm/joint-configuration.hpp>
43 #include <pinocchio/algorithm/model.hpp>
44 #include <pinocchio/fwd.hpp>
45 #include <pinocchio/multibody/data.hpp>
46 #include <pinocchio/multibody/model.hpp>
47 
50 
51 namespace sobec {
52 
53 // Cost COM-vel
54 template <typename Scalar>
55 class ResidualModelCoMVelocityTpl;
56 template <typename Scalar>
60 
61 // Cost COP
62 template <typename Scalar>
64 template <typename Scalar>
68 
69 // Cost CenterOfFriction
70 template <typename Scalar>
72 template <typename Scalar>
76 
77 // Cost velocity collision
78 template <typename Scalar>
80 template <typename Scalar>
84 
85 // Cost fly high
86 template <typename Scalar>
88 template <typename Scalar>
92 
93 // Cost fly angle
94 template <typename Scalar>
96 template <typename Scalar>
100 
101 // Cost DCM
102 template <typename Scalar>
104 template <typename Scalar>
108 
109 // Cost feet collision
110 template <typename Scalar>
112 template <typename Scalar>
116 
117 // Cost 2D surface
118 template <typename Scalar>
120 template <typename Scalar>
124 
125 // Activation quad-ref
126 template <typename Scalar>
129 typedef boost::shared_ptr<ActivationModelQuadRef> ActivationModelQuadRefPtr;
130 
131 // Activation weighted-log
132 template <typename Scalar>
134 template <typename Scalar>
138 typedef boost::shared_ptr<ActivationModelWeightedLog>
140 
141 typedef Eigen::Matrix<double, 6, 1> eVector6;
142 typedef Eigen::Matrix<double, 4, 1> eVector4;
143 typedef Eigen::Vector3d eVector3;
144 typedef Eigen::Vector2d eVector2;
145 typedef boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> IAM;
146 typedef boost::shared_ptr<crocoddyl::IntegratedActionDataEuler> IAD;
147 typedef boost::shared_ptr<crocoddyl::ActionModelAbstract> AMA;
148 typedef boost::shared_ptr<crocoddyl::ActionDataAbstract> ADA;
149 typedef boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
151 typedef boost::shared_ptr<crocoddyl::DifferentialActionDataContactFwdDynamics>
153 typedef boost::shared_ptr<crocoddyl::CostModelSum> Cost;
154 typedef boost::shared_ptr<crocoddyl::ContactModelMultiple> Contact;
155 typedef boost::shared_ptr<crocoddyl::SolverFDDP> DDP;
156 
157 typedef boost::shared_ptr<crocoddyl::ResidualModelFramePlacement>
159 typedef boost::shared_ptr<crocoddyl::ResidualModelContactWrenchCone>
161 
162 // State LPF
163 template <typename Scalar>
166 
167 // IAM LPF
168 template <typename Scalar>
171 template <typename Scalar>
174 
175 // OCP
176 class OCPWalkParam;
177 class OCPWalk;
178 // MPC
179 class MPCWalk;
180 
181 } // namespace sobec
182 
183 namespace sobec {
184 namespace newcontacts {
185 
186 // contact 6D
187 template <typename Scalar>
190 template <typename Scalar>
193 
194 // contact 3D
195 template <typename Scalar>
198 template <typename Scalar>
201 
202 // contact 1D
203 template <typename Scalar>
206 template <typename Scalar>
209 
210 // multiple contacts
211 template <typename Scalar>
214 
215 // DAM contact fwd dynamics
216 template <typename Scalar>
220 
221 // Residual contact force
222 template <typename Scalar>
225 
226 using crocoddyl::ContactType;
227 
228 } // namespace newcontacts
229 } // namespace sobec
230 
231 #endif // SOBEC_FWD_HPP_
sobec::IAD
boost::shared_ptr< crocoddyl::IntegratedActionDataEuler > IAD
Definition: fwd.hpp:146
sobec::StateLPF
StateLPFTpl< double > StateLPF
Definition: fwd.hpp:164
sobec::ActivationModelQuadRefTpl
Definition: activation-quad-ref.hpp:23
sobec::ResidualDataVelCollision
ResidualDataVelCollisionTpl< double > ResidualDataVelCollision
Definition: fwd.hpp:83
sobec::ResidualDataCenterOfPressureTpl
Definition: residual-cop.hpp:118
sobec::ResidualModelContactWrenchConePtr
boost::shared_ptr< crocoddyl::ResidualModelContactWrenchCone > ResidualModelContactWrenchConePtr
Definition: fwd.hpp:160
sobec::newcontacts::ContactData6DTpl
Definition: contact6d.hpp:157
sobec::eVector2
Eigen::Vector2d eVector2
Definition: fwd.hpp:144
sobec::ResidualData2DSurface
ResidualData2DSurfaceTpl< double > ResidualData2DSurface
Definition: fwd.hpp:123
sobec::ActivationDataWeightedLog
ActivationDataWeightedLogTpl< double > ActivationDataWeightedLog
Definition: fwd.hpp:137
sobec::ResidualDataFlyHighTpl
Definition: residual-fly-high.hpp:128
sobec::OCPWalk
Definition: ocp.hpp:88
sobec::newcontacts::ContactModel3DTpl
Definition: contact3d.hpp:28
sobec::ResidualModelCenterOfPressureTpl
COP residual.
Definition: residual-cop.hpp:41
sobec::ActivationDataWeightedLogTpl
Definition: activation-weighted-log.hpp:106
sobec::ResidualModelFeetCollisionTpl
Cost penalizing distance between two frames r=||f1.translation-f2.translation||.
Definition: residual-feet-collision.hpp:29
sobec::ResidualModelCoMVelocity
ResidualModelCoMVelocityTpl< double > ResidualModelCoMVelocity
Definition: fwd.hpp:57
sobec::ActivationModelWeightedLog
ActivationModelWeightedLogTpl< double > ActivationModelWeightedLog
Definition: fwd.hpp:135
sobec::ActivationModelQuadRefPtr
boost::shared_ptr< ActivationModelQuadRef > ActivationModelQuadRefPtr
Definition: fwd.hpp:129
sobec::ResidualDataCenterOfFrictionTpl
Definition: residual-center-of-friction.hpp:118
sobec::newcontacts::ResidualModelContactForceTpl
Define a contact force residual function.
Definition: contact-force.hpp:54
sobec::ResidualModelDCMPosition
ResidualModelDCMPositionTpl< double > ResidualModelDCMPosition
Definition: fwd.hpp:105
sobec::DDP
boost::shared_ptr< crocoddyl::SolverFDDP > DDP
Definition: fwd.hpp:155
sobec::newcontacts::ContactData3D
ContactData3DTpl< double > ContactData3D
Definition: fwd.hpp:199
sobec::StateLPFTpl
Definition: state.hpp:19
sobec::ResidualModelFramePlacementPtr
boost::shared_ptr< crocoddyl::ResidualModelFramePlacement > ResidualModelFramePlacementPtr
Definition: fwd.hpp:158
sobec::ResidualModelFlyAngle
ResidualModelFlyAngleTpl< double > ResidualModelFlyAngle
Definition: fwd.hpp:89
sobec::newcontacts::ContactModel6D
ContactModel6DTpl< double > ContactModel6D
Definition: fwd.hpp:188
sobec::ResidualModelVelCollision
ResidualModelVelCollisionTpl< double > ResidualModelVelCollision
Definition: fwd.hpp:81
residual-com-velocity.hpp
sobec::ResidualDataFlyHigh
ResidualDataFlyHighTpl< double > ResidualDataFlyHigh
Definition: fwd.hpp:99
sobec::Contact
boost::shared_ptr< crocoddyl::ContactModelMultiple > Contact
Definition: fwd.hpp:154
sobec::MPCWalk
Definition: mpc.hpp:62
sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl
Differential action model for contact forward dynamics in multibody systems.
Definition: contact-fwddyn.hpp:85
sobec::newcontacts::ResidualModelContactForce
ResidualModelContactForceTpl< double > ResidualModelContactForce
Definition: fwd.hpp:223
sobec::DAM
boost::shared_ptr< crocoddyl::DifferentialActionModelContactFwdDynamics > DAM
Definition: fwd.hpp:150
sobec::ResidualModelDCMPositionTpl
DCM position residual.
Definition: residual-dcm-position.hpp:37
sobec::ResidualModelCenterOfFriction
ResidualModelCenterOfFrictionTpl< double > ResidualModelCenterOfFriction
Definition: fwd.hpp:73
sobec::newcontacts::ContactData1DTpl
Definition: contact1d.hpp:173
sobec::ResidualModel2DSurfaceTpl
Cost penalizing the position of one effector with respect to the other.
Definition: residual-2D-surface.hpp:31
sobec::ResidualDataCenterOfPressure
ResidualDataCenterOfPressureTpl< double > ResidualDataCenterOfPressure
Definition: fwd.hpp:67
sobec::AMA
boost::shared_ptr< crocoddyl::ActionModelAbstract > AMA
Definition: fwd.hpp:147
sobec::IntegratedActionDataLPF
IntegratedActionDataLPFTpl< double > IntegratedActionDataLPF
Definition: fwd.hpp:172
sobec::newcontacts::ContactModelMultiple
ContactModelMultipleTpl< double > ContactModelMultiple
Definition: fwd.hpp:212
sobec::newcontacts::ContactModel1D
ContactModel1DTpl< double > ContactModel1D
Definition: fwd.hpp:204
sobec
Definition: activation-quad-ref.hpp:19
sobec::ResidualModelFlyHigh
ResidualModelFlyHighTpl< double > ResidualModelFlyHigh
Definition: fwd.hpp:97
sobec::Cost
boost::shared_ptr< crocoddyl::CostModelSum > Cost
Definition: fwd.hpp:153
sobec::ResidualModel2DSurface
ResidualModel2DSurfaceTpl< double > ResidualModel2DSurface
Definition: fwd.hpp:121
sobec::newcontacts::ContactData3DTpl
Definition: contact3d.hpp:155
sobec::ResidualDataCoMVelocityTpl
Definition: residual-com-velocity.hpp:23
sobec::IntegratedActionModelLPF
IntegratedActionModelLPFTpl< double > IntegratedActionModelLPF
Definition: fwd.hpp:169
sobec::eVector4
Eigen::Matrix< double, 4, 1 > eVector4
Definition: fwd.hpp:142
sobec::ResidualDataDCMPositionTpl
Definition: residual-dcm-position.hpp:130
sobec::ResidualDataFlyAngleTpl
Definition: residual-fly-angle.hpp:134
sobec::newcontacts::ContactModel3D
ContactModel3DTpl< double > ContactModel3D
Definition: fwd.hpp:196
sobec::ActivationModelWeightedLogPtr
boost::shared_ptr< ActivationModelWeightedLog > ActivationModelWeightedLogPtr
Definition: fwd.hpp:139
sobec::ResidualDataVelCollisionTpl
Definition: fwd.hpp:81
sobec::IntegratedActionModelLPFTpl
Definition: action.hpp:25
sobec::ActivationModelWeightedLogTpl
Definition: activation-weighted-log.hpp:23
sobec::newcontacts::ContactData1D
ContactData1DTpl< double > ContactData1D
Definition: fwd.hpp:207
activation-quad-ref.hpp
sobec::ResidualDataFlyAngle
ResidualDataFlyAngleTpl< double > ResidualDataFlyAngle
Definition: fwd.hpp:91
sobec::ADA
boost::shared_ptr< crocoddyl::ActionDataAbstract > ADA
Definition: fwd.hpp:148
sobec::eVector6
Eigen::Matrix< double, 6, 1 > eVector6
Definition: fwd.hpp:141
sobec::newcontacts::DifferentialActionModelContactFwdDynamics
DifferentialActionModelContactFwdDynamicsTpl< double > DifferentialActionModelContactFwdDynamics
Definition: fwd.hpp:217
sobec::ResidualDataDCMPosition
ResidualDataDCMPositionTpl< double > ResidualDataDCMPosition
Definition: fwd.hpp:107
sobec::ResidualModelFlyHighTpl
Cost penalizing high horizontal velocity near zero altitude.
Definition: residual-fly-high.hpp:32
sobec::ResidualModelCenterOfPressure
ResidualModelCenterOfPressureTpl< double > ResidualModelCenterOfPressure
Definition: fwd.hpp:65
sobec::ResidualModelFeetCollision
ResidualModelFeetCollisionTpl< double > ResidualModelFeetCollision
Definition: fwd.hpp:113
sobec::ResidualDataFeetCollision
ResidualDataFeetCollisionTpl< double > ResidualDataFeetCollision
Definition: fwd.hpp:115
sobec::ResidualModelVelCollisionTpl
Definition: fwd.hpp:79
sobec::IAM
boost::shared_ptr< crocoddyl::IntegratedActionModelEuler > IAM
Definition: fwd.hpp:145
sobec::ResidualModelFlyAngleTpl
Cost penalizing high horizontal velocity near zero altitude.
Definition: residual-fly-angle.hpp:32
sobec::ResidualModelCoMVelocityTpl
CoM velocity residual.
Definition: residual-com-velocity.hpp:73
sobec::ActivationModelQuadRef
ActivationModelQuadRefTpl< double > ActivationModelQuadRef
Definition: fwd.hpp:127
sobec::newcontacts::ContactData6D
ContactData6DTpl< double > ContactData6D
Definition: fwd.hpp:191
sobec::ResidualModelCenterOfFrictionTpl
Center of friction residual.
Definition: residual-center-of-friction.hpp:41
sobec::newcontacts::ContactModel6DTpl
Definition: contact6d.hpp:28
sobec::newcontacts::ContactModel1DTpl
Definition: contact1d.hpp:29
sobec::ResidualDataCenterOfFriction
ResidualDataCenterOfFrictionTpl< double > ResidualDataCenterOfFriction
Definition: fwd.hpp:75
sobec::ResidualDataFeetCollisionTpl
Definition: residual-feet-collision.hpp:119
sobec::eVector3
Eigen::Vector3d eVector3
Definition: fwd.hpp:143
sobec::ResidualDataCoMVelocity
ResidualDataCoMVelocityTpl< double > ResidualDataCoMVelocity
Definition: fwd.hpp:59
sobec::DAD
boost::shared_ptr< crocoddyl::DifferentialActionDataContactFwdDynamics > DAD
Definition: fwd.hpp:152
sobec::newcontacts::ContactModelMultipleTpl
Define a stack of contact models.
Definition: multiple-contacts.hpp:38
sobec::IntegratedActionDataLPFTpl
Definition: action.hpp:165
sobec::ResidualData2DSurfaceTpl
Definition: residual-2D-surface.hpp:149