Crocoddyl
force-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2023, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_MULTIBODY_FORCE_BASE_HPP_
10 #define CROCODDYL_MULTIBODY_FORCE_BASE_HPP_
11 
12 #include <pinocchio/multibody/data.hpp>
13 #include <pinocchio/spatial/force.hpp>
14 
15 #include "crocoddyl/core/mathbase.hpp"
16 #include "crocoddyl/multibody/fwd.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
22  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 
24  typedef _Scalar Scalar;
26  typedef typename MathBase::VectorXs VectorXs;
27  typedef typename MathBase::MatrixXs MatrixXs;
28  typedef typename pinocchio::DataTpl<Scalar> PinocchioData;
29  typedef typename pinocchio::SE3Tpl<Scalar> SE3;
30  typedef typename pinocchio::ForceTpl<Scalar> Force;
31 
32  template <template <typename Scalar> class Model>
33  ForceDataAbstractTpl(Model<Scalar>* const model, PinocchioData* const data)
34  : pinocchio(data),
35  frame(0),
36  type(model->get_type()),
37  jMf(SE3::Identity()),
38  Jc(model->get_nc(), model->get_state()->get_nv()),
39  f(Force::Zero()),
40  fext(Force::Zero()),
41  df_dx(model->get_nc(), model->get_state()->get_ndx()),
42  df_du(model->get_nc(), model->get_nu()) {
43  Jc.setZero();
44  df_dx.setZero();
45  df_du.setZero();
46  }
47  virtual ~ForceDataAbstractTpl() {}
48 
49  PinocchioData* pinocchio;
50  pinocchio::FrameIndex frame;
51  pinocchio::ReferenceFrame type;
52  SE3 jMf;
53  MatrixXs Jc;
54  Force f;
55  Force fext;
56  MatrixXs df_dx;
58  MatrixXs df_du;
60 };
61 
62 } // namespace crocoddyl
63 
64 #endif // CROCODDYL_MULTIBODY_FORCE_BASE_HPP_
Force fext
External spatial force at the parent joint level.
Definition: force-base.hpp:55
PinocchioData * pinocchio
Pinocchio data.
Definition: force-base.hpp:49
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:50
SE3 jMf
Local frame placement of the contact frame.
Definition: force-base.hpp:52
pinocchio::ReferenceFrame type
Type of contact.
Definition: force-base.hpp:51
MatrixXs Jc
Contact Jacobian.
Definition: force-base.hpp:53
Force f
Contact force expressed in the coordinate defined by type.
Definition: force-base.hpp:54