1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// 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. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
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> |
21 |
|
|
struct ForceDataAbstractTpl { |
22 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
23 |
|
|
|
24 |
|
|
typedef _Scalar Scalar; |
25 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
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 |
|
347567 |
ForceDataAbstractTpl(Model<Scalar>* const model, PinocchioData* const data) |
34 |
|
|
: pinocchio(data), |
35 |
|
|
frame(0), |
36 |
|
|
type(model->get_type()), |
37 |
|
|
jMf(SE3::Identity()), |
38 |
|
347567 |
Jc(model->get_nc(), model->get_state()->get_nv()), |
39 |
|
|
f(Force::Zero()), |
40 |
|
|
fext(Force::Zero()), |
41 |
|
347567 |
df_dx(model->get_nc(), model->get_state()->get_ndx()), |
42 |
✓✗✓✗
✓✗✓✗
✓✗ |
1042701 |
df_du(model->get_nc(), model->get_nu()) { |
43 |
✓✗ |
347567 |
Jc.setZero(); |
44 |
✓✗ |
347567 |
df_dx.setZero(); |
45 |
✓✗ |
347567 |
df_du.setZero(); |
46 |
|
347567 |
} |
47 |
|
174012 |
virtual ~ForceDataAbstractTpl() {} |
48 |
|
|
|
49 |
|
|
PinocchioData* pinocchio; //!< Pinocchio data |
50 |
|
|
pinocchio::FrameIndex frame; //!< Frame index of the contact frame |
51 |
|
|
pinocchio::ReferenceFrame type; //!< Type of contact |
52 |
|
|
SE3 jMf; //!< Local frame placement of the contact frame |
53 |
|
|
MatrixXs Jc; //!< Contact Jacobian |
54 |
|
|
Force f; //!< Contact force expressed in the coordinate defined by type |
55 |
|
|
Force fext; //!< External spatial force at the parent joint level |
56 |
|
|
MatrixXs df_dx; //!< Jacobian of the contact forces expressed in the |
57 |
|
|
//!< coordinate defined by type |
58 |
|
|
MatrixXs df_du; //!< Jacobian of the contact forces expressed in the |
59 |
|
|
//!< coordinate defined by type |
60 |
|
|
}; |
61 |
|
|
|
62 |
|
|
} // namespace crocoddyl |
63 |
|
|
|
64 |
|
|
#endif // CROCODDYL_MULTIBODY_FORCE_BASE_HPP_ |