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 f. |
24 |
|
|
/// |
25 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
26 |
|
|
inline void |
27 |
|
|
copy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
28 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & origin, |
29 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & dest, |
30 |
|
|
KinematicLevel kinematic_level); |
31 |
|
|
|
32 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
33 |
|
|
PINOCCHIO_DEPRECATED inline void |
34 |
|
|
copy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
35 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & origin, |
36 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & dest, |
37 |
|
|
int kinematic_level) |
38 |
|
|
{ |
39 |
|
|
copy(model,origin,dest,static_cast<KinematicLevel>(kinematic_level)); |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
} // namespace pinocchio |
43 |
|
|
|
44 |
|
|
/* --- Details -------------------------------------------------------------------- */ |
45 |
|
|
//#include "pinocchio/algorithm/copy.hxx" |
46 |
|
|
|
47 |
|
|
/// \internal |
48 |
|
|
namespace pinocchio |
49 |
|
|
{ |
50 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
51 |
|
|
inline void |
52 |
|
3 |
copy(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
53 |
|
|
const DataTpl<Scalar,Options,JointCollectionTpl> & origin, |
54 |
|
|
DataTpl<Scalar,Options,JointCollectionTpl> & dest, |
55 |
|
|
KinematicLevel kinematic_level) |
56 |
|
|
{ |
57 |
|
|
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
58 |
|
|
typedef typename Model::JointIndex JointIndex; |
59 |
|
|
|
60 |
✗✓✗✗
|
3 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(kinematic_level>=POSITION); |
61 |
|
|
|
62 |
✓✓ |
84 |
for(JointIndex jid=1; jid<(JointIndex)model.njoints; ++jid) |
63 |
|
|
{ |
64 |
|
81 |
dest.oMi[jid] = origin.oMi [jid]; |
65 |
✓✓ |
81 |
if(kinematic_level >= VELOCITY) |
66 |
|
|
{ |
67 |
|
54 |
dest.v[jid] = origin.v [jid]; |
68 |
|
|
} |
69 |
✓✓ |
81 |
if(kinematic_level >= ACCELERATION) |
70 |
|
|
{ |
71 |
|
27 |
dest.a[jid] = origin.a [jid]; |
72 |
|
27 |
dest.a_gf[jid] = origin.a_gf[jid]; |
73 |
|
27 |
dest.f[jid] = origin.f [jid]; |
74 |
|
|
} |
75 |
|
|
} |
76 |
|
3 |
} |
77 |
|
|
|
78 |
|
|
|
79 |
|
|
} // namespace pinocchio |
80 |
|
|
/// \endinternal |
81 |
|
|
|
82 |
|
|
#endif // ifndef __pinocchio_copy_hpp__ |