1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2022, 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_CORE_DATA_JOINT_HPP_ |
10 |
|
|
#define CROCODDYL_CORE_DATA_JOINT_HPP_ |
11 |
|
|
|
12 |
|
|
#include <boost/shared_ptr.hpp> |
13 |
|
|
|
14 |
|
|
#include "crocoddyl/core/data-collector-base.hpp" |
15 |
|
|
#include "crocoddyl/core/data/actuation.hpp" |
16 |
|
|
#include "crocoddyl/core/fwd.hpp" |
17 |
|
|
#include "crocoddyl/core/state-base.hpp" |
18 |
|
|
|
19 |
|
|
namespace crocoddyl { |
20 |
|
|
|
21 |
|
|
template <typename _Scalar> |
22 |
|
|
struct JointDataAbstractTpl { |
23 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
24 |
|
|
|
25 |
|
|
typedef _Scalar Scalar; |
26 |
|
|
typedef MathBaseTpl<Scalar> MathBase; |
27 |
|
|
typedef StateAbstractTpl<Scalar> StateAbstract; |
28 |
|
|
typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract; |
29 |
|
|
typedef typename MathBase::VectorXs VectorXs; |
30 |
|
|
typedef typename MathBase::MatrixXs MatrixXs; |
31 |
|
|
|
32 |
|
|
/** |
33 |
|
|
* @brief Initialize a joint data structure containing generalized |
34 |
|
|
* accelerations and joint efforts, and their derivatives. |
35 |
|
|
* |
36 |
|
|
* @param state State description |
37 |
|
|
* @param actuation Actuation model |
38 |
|
|
* @param nu Dimension of control input |
39 |
|
|
*/ |
40 |
|
91139 |
JointDataAbstractTpl(boost::shared_ptr<StateAbstract> state, |
41 |
|
|
boost::shared_ptr<ActuationModelAbstract> actuation, |
42 |
|
|
const std::size_t nu) |
43 |
|
|
: tau(actuation->get_nu()), |
44 |
|
|
a(state->get_nv()), |
45 |
|
|
dtau_dx(actuation->get_nu(), state->get_ndx()), |
46 |
|
|
dtau_du(actuation->get_nu(), nu), |
47 |
|
|
da_dx(state->get_nv(), state->get_ndx()), |
48 |
✓✗✓✗
✓✗✓✗
✓✗✓✗
|
91139 |
da_du(state->get_nv(), nu) { |
49 |
✓✗ |
91139 |
tau.setZero(); |
50 |
✓✗ |
91139 |
a.setZero(); |
51 |
✓✗ |
91139 |
dtau_dx.setZero(); |
52 |
✓✗ |
91139 |
dtau_du.setZero(); |
53 |
✓✗ |
91139 |
da_dx.setZero(); |
54 |
✓✗ |
91139 |
da_du.setZero(); |
55 |
|
91139 |
} |
56 |
|
182258 |
virtual ~JointDataAbstractTpl() {} |
57 |
|
|
|
58 |
|
|
VectorXs tau; //!< Joint efforts |
59 |
|
|
VectorXs a; //!< Generalized joint acceleration |
60 |
|
|
MatrixXs dtau_dx; //!< Partial derivatives of the joint efforts w.r.t. the |
61 |
|
|
//!< state point |
62 |
|
|
MatrixXs dtau_du; //!< Partial derivatives of the joint efforts w.r.t. the |
63 |
|
|
//!< control input |
64 |
|
|
MatrixXs da_dx; //!< Partial derivatives of the generalized joint |
65 |
|
|
//!< accelerations w.r.t. the state point |
66 |
|
|
MatrixXs da_du; //!< Partial derivatives of the generalized joint |
67 |
|
|
//!< accelerations w.r.t. the control input |
68 |
|
|
}; |
69 |
|
|
|
70 |
|
|
template <typename Scalar> |
71 |
|
|
struct DataCollectorJointTpl : virtual DataCollectorAbstractTpl<Scalar> { |
72 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
73 |
|
|
|
74 |
|
91143 |
DataCollectorJointTpl(boost::shared_ptr<JointDataAbstractTpl<Scalar> > joint) |
75 |
|
91143 |
: DataCollectorAbstractTpl<Scalar>(), joint(joint) {} |
76 |
|
91151 |
virtual ~DataCollectorJointTpl() {} |
77 |
|
|
|
78 |
|
|
boost::shared_ptr<JointDataAbstractTpl<Scalar> > joint; |
79 |
|
|
}; |
80 |
|
|
|
81 |
|
|
template <typename Scalar> |
82 |
|
|
struct DataCollectorJointActuationTpl : DataCollectorActuationTpl<Scalar> { |
83 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
84 |
|
|
|
85 |
|
|
/** |
86 |
|
|
* @brief Initialize the joint-actuation data collector |
87 |
|
|
* |
88 |
|
|
* @param[in] actuation Actuation data |
89 |
|
|
* @param[in] joint Joint data |
90 |
|
|
*/ |
91 |
|
|
DataCollectorJointActuationTpl( |
92 |
|
|
boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > actuation, |
93 |
|
|
boost::shared_ptr<JointDataAbstractTpl<Scalar> > joint) |
94 |
|
|
: DataCollectorActuationTpl<Scalar>(actuation), joint(joint) {} |
95 |
|
|
virtual ~DataCollectorJointActuationTpl() {} |
96 |
|
|
|
97 |
|
|
boost::shared_ptr<JointDataAbstractTpl<Scalar> > joint; |
98 |
|
|
}; |
99 |
|
|
|
100 |
|
|
} // namespace crocoddyl |
101 |
|
|
|
102 |
|
|
#endif // CROCODDYL_CORE_DATA_JOINT_HPP_ |