Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_copy_hpp__ |
6 |
|
|
#define __pinocchio_copy_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/model.hpp" |
9 |
|
|
#include "pinocchio/multibody/data.hpp" |
10 |
|
|
#include "pinocchio/algorithm/check.hpp" |
11 |
|
|
|
12 |
|
|
namespace pinocchio |
13 |
|
|
{ |
14 |
|
|
/// |
15 |
|
|
/// \brief Copy part of the data from \c origin to \c dest. Template parameter can be |
16 |
|
|
/// used to select at which differential level the copy should occur. |
17 |
|
|
/// |
18 |
|
|
/// \tparam JointCollection Collection of Joint types. |
19 |
|
|
/// |
20 |
|
|
/// \param[in] model The model structure of the rigid body system. |
21 |
|
|
/// \param[in] orig Data from which the values are copied. |
22 |
|
|
/// \param[out] dest Data to which the values are copied |
23 |
|
|
/// \param[in] kinematic_level if =0, copy oMi. If =1, also copy v. If =2, also copy a, a_gf and |
24 |
|
|
/// f. |
25 |
|
|
/// |
26 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
27 |
|
|
inline void copy( |
28 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
29 |
|
|
const DataTpl<Scalar, Options, JointCollectionTpl> & origin, |
30 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & dest, |
31 |
|
|
KinematicLevel kinematic_level); |
32 |
|
|
|
33 |
|
|
} // namespace pinocchio |
34 |
|
|
|
35 |
|
|
/* --- Details -------------------------------------------------------------------- */ |
36 |
|
|
// #include "pinocchio/algorithm/copy.hxx" |
37 |
|
|
|
38 |
|
|
/// \internal |
39 |
|
|
namespace pinocchio |
40 |
|
|
{ |
41 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
42 |
|
✗ |
inline void copy( |
43 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
44 |
|
|
const DataTpl<Scalar, Options, JointCollectionTpl> & origin, |
45 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & dest, |
46 |
|
|
KinematicLevel kinematic_level) |
47 |
|
|
{ |
48 |
|
|
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
49 |
|
|
typedef typename Model::JointIndex JointIndex; |
50 |
|
|
|
51 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT(kinematic_level >= POSITION); |
52 |
|
|
|
53 |
|
✗ |
for (JointIndex jid = 1; jid < (JointIndex)model.njoints; ++jid) |
54 |
|
|
{ |
55 |
|
✗ |
dest.oMi[jid] = origin.oMi[jid]; |
56 |
|
✗ |
if (kinematic_level >= VELOCITY) |
57 |
|
|
{ |
58 |
|
✗ |
dest.v[jid] = origin.v[jid]; |
59 |
|
|
} |
60 |
|
✗ |
if (kinematic_level >= ACCELERATION) |
61 |
|
|
{ |
62 |
|
✗ |
dest.a[jid] = origin.a[jid]; |
63 |
|
✗ |
dest.a_gf[jid] = origin.a_gf[jid]; |
64 |
|
✗ |
dest.f[jid] = origin.f[jid]; |
65 |
|
|
} |
66 |
|
|
} |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
} // namespace pinocchio |
70 |
|
|
/// \endinternal |
71 |
|
|
|
72 |
|
|
#endif // ifndef __pinocchio_copy_hpp__ |
73 |
|
|
|