GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2019 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_joint_composite_hxx__ |
||
6 |
#define __pinocchio_joint_composite_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
|||
10 |
namespace pinocchio |
||
11 |
{ |
||
12 |
|||
13 |
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType> |
||
14 |
struct JointCompositeCalcZeroOrderStep |
||
15 |
: fusion::JointUnaryVisitorBase< JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> > |
||
16 |
{ |
||
17 |
typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite; |
||
18 |
typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite; |
||
19 |
|||
20 |
typedef boost::fusion::vector<const JointModelComposite &, |
||
21 |
JointDataComposite &, |
||
22 |
const ConfigVectorType & |
||
23 |
> ArgsType; |
||
24 |
|||
25 |
template<typename JointModel> |
||
26 |
70 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
27 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
28 |
const JointModelComposite & model, |
||
29 |
JointDataComposite & data, |
||
30 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
31 |
{ |
||
32 |
70 |
const JointIndex & i = jmodel.id(); |
|
33 |
70 |
const JointIndex succ = i+1; // successor |
|
34 |
|||
35 |
70 |
jmodel.calc(jdata.derived(), q.derived()); |
|
36 |
|||
37 |
✓✗ | 70 |
data.pjMi[i] = model.jointPlacements[i] * jdata.M (); |
38 |
|||
39 |
✓✓ | 70 |
if ( succ == model.joints.size() ) |
40 |
{ |
||
41 |
40 |
data.iMlast[i] = data.pjMi[i]; |
|
42 |
✓✗✗✓ ✗✓✗ |
40 |
data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix(); |
43 |
} |
||
44 |
else |
||
45 |
{ |
||
46 |
30 |
const int idx_v = model.m_idx_v[i] - model.m_idx_v[0]; |
|
47 |
|||
48 |
30 |
data.iMlast[i] = data.pjMi[i] * data.iMlast[succ]; |
|
49 |
✓✗✓✗ ✓✗ |
30 |
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S()); |
50 |
} |
||
51 |
|||
52 |
} |
||
53 |
|||
54 |
}; |
||
55 |
|||
56 |
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
||
57 |
template<typename ConfigVectorType> |
||
58 |
20 |
inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>:: |
|
59 |
calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const |
||
60 |
{ |
||
61 |
✗✓ | 20 |
assert(joints.size() > 0); |
62 |
✗✓ | 20 |
assert(data.joints.size() == joints.size()); |
63 |
|||
64 |
typedef JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Algo; |
||
65 |
|||
66 |
✓✓ | 55 |
for (int i=(int)(joints.size()-1); i >= 0; --i) |
67 |
{ |
||
68 |
✓✗ | 70 |
Algo::run(joints[(size_t)i], |
69 |
35 |
data.joints[(size_t)i], |
|
70 |
typename Algo::ArgsType(*this,data,qs.derived())); |
||
71 |
} |
||
72 |
20 |
data.M = data.iMlast.front(); |
|
73 |
20 |
} |
|
74 |
|||
75 |
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
76 |
struct JointCompositeCalcFirstOrderStep |
||
77 |
: public fusion::JointUnaryVisitorBase< JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> > |
||
78 |
{ |
||
79 |
typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite; |
||
80 |
typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite; |
||
81 |
|||
82 |
typedef boost::fusion::vector<const JointModelComposite &, |
||
83 |
JointDataComposite &, |
||
84 |
const ConfigVectorType &, |
||
85 |
const TangentVectorType & |
||
86 |
> ArgsType; |
||
87 |
|||
88 |
template<typename JointModel> |
||
89 |
148 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
90 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
91 |
const JointModelComposite & model, |
||
92 |
JointDataComposite & data, |
||
93 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
94 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
95 |
{ |
||
96 |
148 |
const JointIndex & i = jmodel.id(); |
|
97 |
148 |
const JointIndex succ = i+1; // successor |
|
98 |
|||
99 |
148 |
jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
|
100 |
|||
101 |
✓✗ | 148 |
data.pjMi[i] = model.jointPlacements[i] * jdata.M(); |
102 |
|||
103 |
✓✓ | 148 |
if (succ == model.joints.size()) |
104 |
{ |
||
105 |
84 |
data.iMlast[i] = data.pjMi[i]; |
|
106 |
✓✗✗✓ ✗✓✗ |
84 |
data.S.matrix().rightCols(model.m_nvs[i]) = jdata.S().matrix(); |
107 |
84 |
data.v = jdata.v(); |
|
108 |
84 |
data.c = jdata.c(); |
|
109 |
} |
||
110 |
else |
||
111 |
{ |
||
112 |
64 |
const int idx_v = model.m_idx_v[i] - model.m_idx_v[0]; |
|
113 |
|||
114 |
✓✗ | 64 |
data.iMlast[i] = data.pjMi[i] * data.iMlast[succ]; |
115 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].actInv(jdata.S()); |
116 |
|||
117 |
✓✗✓✗ |
64 |
typename JointModelComposite::Motion v_tmp = data.iMlast[succ].actInv(jdata.v()); |
118 |
|||
119 |
✓✗ | 64 |
data.v += v_tmp; |
120 |
|||
121 |
✓✗✓✗ |
64 |
data.c -= data.v.cross(v_tmp); |
122 |
✓✗✓✗ ✓✗ |
64 |
data.c += data.iMlast[succ].actInv(jdata.c()); |
123 |
} |
||
124 |
|||
125 |
} |
||
126 |
|||
127 |
}; |
||
128 |
|||
129 |
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
||
130 |
template<typename ConfigVectorType, typename TangentVectorType> |
||
131 |
42 |
inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> |
|
132 |
::calc(JointDataDerived & jdata, |
||
133 |
const Eigen::MatrixBase<ConfigVectorType> & qs, |
||
134 |
const Eigen::MatrixBase<TangentVectorType> & vs) const |
||
135 |
{ |
||
136 |
✗✓ | 42 |
assert(joints.size() > 0); |
137 |
✗✓ | 42 |
assert(jdata.joints.size() == joints.size()); |
138 |
|||
139 |
typedef JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Algo; |
||
140 |
|||
141 |
✓✓ | 116 |
for (int i=(int)(joints.size()-1); i >= 0; --i) |
142 |
{ |
||
143 |
✓✗ | 148 |
Algo::run(joints[(size_t)i], |
144 |
74 |
jdata.joints[(size_t)i], |
|
145 |
typename Algo::ArgsType(*this,jdata,qs.derived(),vs.derived())); |
||
146 |
} |
||
147 |
|||
148 |
42 |
jdata.M = jdata.iMlast.front(); |
|
149 |
42 |
} |
|
150 |
|||
151 |
} // namespace pinocchio |
||
152 |
|||
153 |
#endif // ifndef __pinocchio_joint_composite_hxx__ |
Generated by: GCOVR (Version 4.2) |