crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
force-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, University of Edinburgh
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/multibody/fwd.hpp"
16 #include "crocoddyl/core/mathbase.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 
29  template <template <typename Scalar> class Model>
30  ForceDataAbstractTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
31  : pinocchio(data),
32  frame(0),
33  jMf(pinocchio::SE3Tpl<Scalar>::Identity()),
34  Jc(model->get_nc(), model->get_state()->get_nv()),
35  f(pinocchio::ForceTpl<Scalar>::Zero()),
36  df_dx(model->get_nc(), model->get_state()->get_ndx()),
37  df_du(model->get_nc(), model->get_nu()) {
38  Jc.setZero();
39  df_dx.setZero();
40  df_du.setZero();
41  }
42  virtual ~ForceDataAbstractTpl() {}
43 
44  typename pinocchio::DataTpl<Scalar>* pinocchio;
45  pinocchio::FrameIndex frame;
46  typename pinocchio::SE3Tpl<Scalar> jMf;
47  MatrixXs Jc;
48  pinocchio::ForceTpl<Scalar> f;
49  MatrixXs df_dx;
51  MatrixXs df_du;
52 };
53 
54 } // namespace crocoddyl
55 
56 #endif // CROCODDYL_MULTIBODY_FORCE_BASE_HPP_
crocoddyl::ForceDataAbstractTpl
Definition: force-base.hpp:21
crocoddyl::ForceDataAbstractTpl::df_du
MatrixXs df_du
Jacobian of the contact forces.
Definition: force-base.hpp:51
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ForceDataAbstractTpl::frame
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
crocoddyl::ForceDataAbstractTpl::df_dx
MatrixXs df_dx
Jacobian of the contact forces.
Definition: force-base.hpp:50
crocoddyl::ForceDataAbstractTpl::jMf
pinocchio::SE3Tpl< Scalar > jMf
Local frame placement of the contact frame.
Definition: force-base.hpp:46
crocoddyl::ForceDataAbstractTpl::Jc
MatrixXs Jc
Contact Jacobian.
Definition: force-base.hpp:47
crocoddyl::ForceDataAbstractTpl::f
pinocchio::ForceTpl< Scalar > f
Definition: force-base.hpp:48
crocoddyl::ForceDataAbstractTpl::pinocchio
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: force-base.hpp:44