| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_joint_configuration_hxx__ |
||
6 |
#define __pinocchio_algorithm_joint_configuration_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp" |
||
10 |
|||
11 |
/* --- Details -------------------------------------------------------------------- */ |
||
12 |
namespace pinocchio |
||
13 |
{ |
||
14 |
|||
15 |
// --------------- API with return value as argument ---------------------- // |
||
16 |
|||
17 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType> |
||
18 |
void |
||
19 |
2430 |
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
20 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
21 |
const Eigen::MatrixBase<TangentVectorType> & v, |
||
22 |
const Eigen::MatrixBase<ReturnType> & qout) |
||
23 |
{ |
||
24 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2430 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size"); |
25 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2430 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of the right size"); |
26 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2430 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size"); |
27 |
|||
28 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
29 |
typedef typename Model::JointIndex JointIndex; |
||
30 |
2430 |
ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout); |
|
31 |
|||
32 |
typedef IntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,ReturnType> Algo; |
||
33 |
✓✗ | 2430 |
typename Algo::ArgsType args(q.derived(),v.derived(),res); |
34 |
✓✓ | 59492 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
35 |
{ |
||
36 |
✓✗✓✗ |
57062 |
Algo::run(model.joints[i], args); |
37 |
} |
||
38 |
2430 |
} |
|
39 |
|||
40 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
||
41 |
void |
||
42 |
2 |
interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
43 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
44 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
45 |
const Scalar & u, |
||
46 |
const Eigen::MatrixBase<ReturnType> & qout) |
||
47 |
{ |
||
48 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The first configuration vector is not of the right size"); |
49 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The second configuration vector is not of the right size"); |
50 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size"); |
51 |
|||
52 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
53 |
typedef typename Model::JointIndex JointIndex; |
||
54 |
2 |
ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout); |
|
55 |
|||
56 |
typedef InterpolateStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar,ReturnType> Algo; |
||
57 |
✓✓ | 20 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
58 |
{ |
||
59 |
✓✗ | 18 |
Algo::run(model.joints[i], |
60 |
typename Algo::ArgsType(q0.derived(), q1.derived(), u, res.derived())); |
||
61 |
} |
||
62 |
2 |
} |
|
63 |
|||
64 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
||
65 |
void |
||
66 |
115 |
difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
67 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
68 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
69 |
const Eigen::MatrixBase<ReturnType> & dvout) |
||
70 |
{ |
||
71 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
115 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The first configuration vector is not of the right size"); |
72 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
115 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The second configuration vector is not of the right size"); |
73 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
115 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(dvout.size(), model.nv, "The output argument is not of the right size"); |
74 |
|||
75 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
76 |
typedef typename Model::JointIndex JointIndex; |
||
77 |
115 |
ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout); |
|
78 |
|||
79 |
typedef DifferenceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,ReturnType> Algo; |
||
80 |
✓✗ | 115 |
typename Algo::ArgsType args(q0.derived(),q1.derived(),res); |
81 |
✓✓ | 1150 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
82 |
{ |
||
83 |
✓✗✓✗ |
1035 |
Algo::run(model.joints[i], args); |
84 |
} |
||
85 |
115 |
} |
|
86 |
|||
87 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
||
88 |
void |
||
89 |
1 |
squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
90 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
91 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
92 |
const Eigen::MatrixBase<ReturnType> & out) |
||
93 |
{ |
||
94 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The first configuration vector is not of the right size"); |
95 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The second configuration vector is not of the right size"); |
96 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(out.size(), (model.njoints-1), "The output argument is not of the right size"); |
97 |
|||
98 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
99 |
typedef typename Model::JointIndex JointIndex; |
||
100 |
1 |
ReturnType & distances = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out); |
|
101 |
|||
102 |
typedef SquaredDistanceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,ReturnType> Algo; |
||
103 |
✓✓ | 10 |
for(JointIndex i=0; i<(JointIndex) model.njoints-1; ++i) |
104 |
{ |
||
105 |
✓✗ | 9 |
typename Algo::ArgsType args(i,q0.derived(),q1.derived(), distances.derived()); |
106 |
✓✗✓✗ |
9 |
Algo::run(model.joints[i+1], args); |
107 |
} |
||
108 |
1 |
} |
|
109 |
|||
110 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType> |
||
111 |
void |
||
112 |
162 |
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
113 |
const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits, |
||
114 |
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits, |
||
115 |
const Eigen::MatrixBase<ReturnType> & qout) |
||
116 |
{ |
||
117 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
162 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(lowerLimits.size(), model.nq, "The lower limits vector is not of the right size"); |
118 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
162 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(upperLimits.size(), model.nq, "The upper limits vector is not of the right size"); |
119 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
162 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size"); |
120 |
|||
121 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
122 |
typedef typename Model::JointIndex JointIndex; |
||
123 |
162 |
ReturnType & q = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout); |
|
124 |
|||
125 |
typedef RandomConfigurationStep<LieGroup_t,ReturnType,ConfigVectorIn1,ConfigVectorIn2> Algo; |
||
126 |
✓✗ | 162 |
typename Algo::ArgsType args(PINOCCHIO_EIGEN_CONST_CAST(ReturnType,q), lowerLimits.derived(), upperLimits.derived()); |
127 |
✓✓ | 2473 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
128 |
{ |
||
129 |
✓✗✓✗ |
2311 |
Algo::run(model.joints[i], args); |
130 |
} |
||
131 |
162 |
} |
|
132 |
|||
133 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType> |
||
134 |
void |
||
135 |
31 |
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, const Eigen::MatrixBase<ReturnType> & qout) |
|
136 |
{ |
||
137 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
31 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size"); |
138 |
|||
139 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
140 |
typedef typename Model::JointIndex JointIndex; |
||
141 |
31 |
ReturnType & neutral_elt = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout); |
|
142 |
|||
143 |
✓✗ | 31 |
typename NeutralStep<LieGroup_t,ReturnType>::ArgsType args(neutral_elt.derived()); |
144 |
✓✓ | 609 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i ) |
145 |
{ |
||
146 |
✓✗✓✗ |
578 |
NeutralStep<LieGroup_t,ReturnType>::run(model.joints[i],args); |
147 |
} |
||
148 |
31 |
} |
|
149 |
|||
150 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType> |
||
151 |
22 |
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
152 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
153 |
const Eigen::MatrixBase<TangentVectorType> & v, |
||
154 |
const Eigen::MatrixBase<JacobianMatrixType> & J, |
||
155 |
const ArgumentPosition arg, |
||
156 |
const AssignmentOperatorType op) |
||
157 |
{ |
||
158 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
22 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size"); |
159 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
22 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of the right size"); |
160 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
22 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), model.nv, "The output argument is not of the right size"); |
161 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
22 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv, "The output argument is not of the right size"); |
162 |
|||
163 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
164 |
typedef typename Model::JointIndex JointIndex; |
||
165 |
|||
166 |
typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo; |
||
167 |
✓✗ | 22 |
typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op); |
168 |
✓✓ | 220 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
169 |
{ |
||
170 |
✓✗✓✗ |
198 |
Algo::run(model.joints[i], args); |
171 |
} |
||
172 |
22 |
} |
|
173 |
|||
174 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2> |
||
175 |
2 |
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
176 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
177 |
const Eigen::MatrixBase<TangentVectorType> & v, |
||
178 |
const Eigen::MatrixBase<JacobianMatrixType1> & Jin, |
||
179 |
const Eigen::MatrixBase<JacobianMatrixType2> & Jout, |
||
180 |
const ArgumentPosition arg) |
||
181 |
{ |
||
182 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size"); |
183 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of the right size"); |
184 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), model.nv, "The input matrix is not of the right size"); |
185 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), Jin.rows(), "The output argument should be the same size as input matrix"); |
186 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), Jin.cols(), "The output argument should be the same size as input matrix"); |
187 |
|||
188 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
189 |
typedef typename Model::JointIndex JointIndex; |
||
190 |
|||
191 |
typedef dIntegrateTransportStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2> Algo; |
||
192 |
✓✗ | 2 |
typename Algo::ArgsType args(q.derived(),v.derived(),Jin.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2,Jout),arg); |
193 |
✓✓ | 20 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
194 |
{ |
||
195 |
✓✗✓✗ |
18 |
Algo::run(model.joints[i], args); |
196 |
} |
||
197 |
2 |
} |
|
198 |
|||
199 |
|||
200 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType> |
||
201 |
2 |
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
202 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
203 |
const Eigen::MatrixBase<TangentVectorType> & v, |
||
204 |
const Eigen::MatrixBase<JacobianMatrixType> & J, |
||
205 |
const ArgumentPosition arg) |
||
206 |
{ |
||
207 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size"); |
208 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of the right size"); |
209 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), model.nv, "The input matrix is not of the right size"); |
210 |
|||
211 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
212 |
typedef typename Model::JointIndex JointIndex; |
||
213 |
|||
214 |
typedef dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo; |
||
215 |
✓✗ | 2 |
typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg); |
216 |
✓✓ | 20 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
217 |
{ |
||
218 |
✓✗✓✗ |
18 |
Algo::run(model.joints[i], args); |
219 |
} |
||
220 |
2 |
} |
|
221 |
|||
222 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix> |
||
223 |
6 |
void dDifference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
224 |
const Eigen::MatrixBase<ConfigVector1> & q0, |
||
225 |
const Eigen::MatrixBase<ConfigVector2> & q1, |
||
226 |
const Eigen::MatrixBase<JacobianMatrix> & J, |
||
227 |
const ArgumentPosition arg) |
||
228 |
{ |
||
229 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The configuration vector q0 is not of the right size"); |
230 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The configuration vector q1 is not of the right size"); |
231 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), model.nv, "The output argument is not of the right size"); |
232 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv, "The output argument is not of the right size"); |
233 |
|||
234 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
235 |
typedef typename Model::JointIndex JointIndex; |
||
236 |
|||
237 |
typedef dDifferenceStep<LieGroup_t,ConfigVector1,ConfigVector2,JacobianMatrix> Algo; |
||
238 |
✓✗ | 6 |
typename Algo::ArgsType args(q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg); |
239 |
✓✓ | 60 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
240 |
{ |
||
241 |
✓✗✓✗ |
54 |
Algo::run(model.joints[i], args); |
242 |
} |
||
243 |
6 |
} |
|
244 |
|||
245 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
246 |
Scalar |
||
247 |
2 |
squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
248 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
249 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
||
250 |
{ |
||
251 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The first configuration vector is not of the right size"); |
252 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The second configuration vector is not of the right size"); |
253 |
|||
254 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
255 |
typedef typename Model::JointIndex JointIndex; |
||
256 |
2 |
typename ConfigVectorIn1::Scalar squaredDistance = 0.0; |
|
257 |
|||
258 |
typedef SquaredDistanceSumStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> Algo; |
||
259 |
✓✓ | 20 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
260 |
{ |
||
261 |
✓✗ | 18 |
typename Algo::ArgsType args(q0.derived(),q1.derived(), squaredDistance); |
262 |
✓✗✓✗ |
18 |
Algo::run(model.joints[i], args); |
263 |
} |
||
264 |
|||
265 |
2 |
return squaredDistance; |
|
266 |
} |
||
267 |
|||
268 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
269 |
Scalar |
||
270 |
2 |
distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
271 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
272 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
||
273 |
{ |
||
274 |
2 |
const Scalar & squaredDistance = squaredDistanceSum<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived()); |
|
275 |
2 |
return math::sqrt(squaredDistance); |
|
276 |
} |
||
277 |
|||
278 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
279 |
11 |
inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
280 |
const Eigen::MatrixBase<ConfigVectorType> & qout) |
||
281 |
{ |
||
282 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
11 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size"); |
283 |
|||
284 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
285 |
typedef typename Model::JointIndex JointIndex; |
||
286 |
|||
287 |
typedef NormalizeStep<LieGroup_t,ConfigVectorType> Algo; |
||
288 |
✓✓ | 164 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
289 |
{ |
||
290 |
✓✗ | 153 |
Algo::run(model.joints[i], |
291 |
153 |
typename Algo::ArgsType(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout))); |
|
292 |
} |
||
293 |
11 |
} |
|
294 |
|||
295 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn> |
||
296 |
inline bool |
||
297 |
9 |
isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
298 |
const Eigen::MatrixBase<ConfigVectorIn> & q, |
||
299 |
const Scalar& prec) |
||
300 |
{ |
||
301 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
9 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size"); |
302 |
✗✓✗✗ |
9 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(prec >= 0, "The precision should be positive"); |
303 |
|||
304 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
305 |
typedef typename Model::JointIndex JointIndex; |
||
306 |
|||
307 |
9 |
bool result = true; |
|
308 |
typedef IsNormalizedStep<LieGroup_t,ConfigVectorIn,Scalar> Algo; |
||
309 |
✓✗ | 9 |
typename Algo::ArgsType args(q.derived(),prec,result); |
310 |
✓✓ | 72 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
311 |
{ |
||
312 |
✓✗✓✗ |
65 |
Algo::run(model.joints[i], args); |
313 |
✓✓ | 65 |
if(!result) |
314 |
2 |
return false; |
|
315 |
} |
||
316 |
|||
317 |
7 |
return true; |
|
318 |
} |
||
319 |
|||
320 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
321 |
inline bool |
||
322 |
3 |
isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
323 |
const Eigen::MatrixBase<ConfigVectorIn1> & q1, |
||
324 |
const Eigen::MatrixBase<ConfigVectorIn2> & q2, |
||
325 |
const Scalar & prec) |
||
326 |
{ |
||
327 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The first configuration vector is not of the right size"); |
328 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q2.size(), model.nq, "The second configuration vector is not of the right size"); |
329 |
✗✓✗✗ |
3 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(prec >= 0, "The precision should be positive"); |
330 |
|||
331 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
332 |
typedef typename Model::JointIndex JointIndex; |
||
333 |
|||
334 |
3 |
bool result = true; |
|
335 |
typedef IsSameConfigurationStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> Algo; |
||
336 |
✓✗ | 3 |
typename Algo::ArgsType args(result,q1.derived(),q2.derived(),prec); |
337 |
✓✓ | 30 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
338 |
{ |
||
339 |
✓✗✓✗ |
27 |
Algo::run(model.joints[i], args); |
340 |
✗✓ | 27 |
if(!result) |
341 |
return false; |
||
342 |
} |
||
343 |
|||
344 |
3 |
return true; |
|
345 |
} |
||
346 |
|||
347 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix> |
||
348 |
inline void |
||
349 |
1 |
integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
350 |
const Eigen::MatrixBase<ConfigVector> & q, |
||
351 |
const Eigen::MatrixBase<JacobianMatrix> & jacobian) |
||
352 |
{ |
||
353 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size"); |
354 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(jacobian.rows(), model.nq); |
355 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(jacobian.cols(), model.nv, "The jacobian does not have the right dimension"); |
356 |
|||
357 |
typedef IntegrateCoeffWiseJacobianStep<LieGroup_t,ConfigVector,JacobianMatrix> Algo; |
||
358 |
✓✗ | 1 |
typename Algo::ArgsType args(q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian)); |
359 |
✓✓ | 28 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
360 |
{ |
||
361 |
✓✗✓✗ |
27 |
Algo::run(model.joints[i],args); |
362 |
} |
||
363 |
1 |
} |
|
364 |
|||
365 |
// ----------------- API that allocates memory ---------------------------- // |
||
366 |
|||
367 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
368 |
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) |
||
369 |
2430 |
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
370 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
371 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
372 |
{ |
||
373 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) ReturnType; |
||
374 |
2430 |
ReturnType res(model.nq); |
|
375 |
✓✗ | 2430 |
integrate<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), res); |
376 |
2430 |
return res; |
|
377 |
} |
||
378 |
|||
379 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
380 |
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
||
381 |
2 |
interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
382 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
383 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1, |
||
384 |
const Scalar & u) |
||
385 |
{ |
||
386 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType; |
||
387 |
2 |
ReturnType res(model.nq); |
|
388 |
✓✗ | 2 |
interpolate<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, res); |
389 |
2 |
return res; |
|
390 |
} |
||
391 |
|||
392 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
393 |
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
||
394 |
115 |
difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
395 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
396 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
||
397 |
{ |
||
398 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType; |
||
399 |
115 |
ReturnType res(model.nv); |
|
400 |
✓✗ | 115 |
difference<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),res); |
401 |
115 |
return res; |
|
402 |
} |
||
403 |
|||
404 |
template<typename LieGroup_t,typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
405 |
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) |
||
406 |
1 |
squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
407 |
const Eigen::MatrixBase<ConfigVectorIn1> & q0, |
||
408 |
const Eigen::MatrixBase<ConfigVectorIn2> & q1) |
||
409 |
{ |
||
410 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType; |
||
411 |
✓✗ | 1 |
ReturnType distances(ReturnType::Zero(model.njoints-1)); |
412 |
✓✗ | 1 |
squaredDistance<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),distances); |
413 |
1 |
return distances; |
|
414 |
} |
||
415 |
|||
416 |
template<typename LieGroup_t,typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2> |
||
417 |
typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) |
||
418 |
141 |
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
419 |
const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits, |
||
420 |
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits) |
||
421 |
{ |
||
422 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) ReturnType; |
||
423 |
141 |
ReturnType q(model.nq); |
|
424 |
✓✗ | 141 |
randomConfiguration<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), q); |
425 |
141 |
return q; |
|
426 |
} |
||
427 |
|||
428 |
template<typename LieGroup_t,typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
429 |
typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType)) |
||
430 |
94 |
randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) |
|
431 |
{ |
||
432 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
433 |
typedef typename Model::ConfigVectorType ConfigVectorType; |
||
434 |
94 |
return randomConfiguration<LieGroup_t,Scalar,Options,JointCollectionTpl,ConfigVectorType,ConfigVectorType>(model, model.lowerPositionLimit, model.upperPositionLimit); |
|
435 |
} |
||
436 |
|||
437 |
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
438 |
inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> |
||
439 |
23 |
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) |
|
440 |
{ |
||
441 |
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ReturnType; |
||
442 |
23 |
ReturnType q(model.nq); |
|
443 |
✓✗ | 23 |
neutral<LieGroup_t,Scalar,Options,JointCollectionTpl,ReturnType>(model,q); |
444 |
23 |
return q; |
|
445 |
} |
||
446 |
|||
447 |
} // namespace pinocchio |
||
448 |
|||
449 |
#endif // ifndef __pinocchio_algorithm_joint_configuration_hxx__ |
| Generated by: GCOVR (Version 4.2) |