Skip to content

Commit 3fa5eaa

Browse files
author
Megane Millan
committed
Added assert for non supported algorithms and apply precommit
1 parent 064c2a5 commit 3fa5eaa

21 files changed

+127
-73
lines changed

‎include/pinocchio/algorithm/aba-derivatives.hxx

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,11 @@ namespace pinocchio
4343
const Eigen::MatrixBase<ConfigVectorType> & q,
4444
const Eigen::MatrixBase<TangentVectorType> & v)
4545
{
46+
assert(
47+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
48+
== false)
49+
&& "Algorithm not supported for mimic joints");
50+
4651
typedef typename Model::JointIndex JointIndex;
4752

4853
const JointIndex i = jmodel.id();
@@ -162,8 +167,8 @@ namespace pinocchio
162167
{
163168
Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
164169

165-
fi.toVector().noalias() += Ia * data.oa_gf[i].toVector()
166-
+ jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
170+
fi.toVector().noalias() +=
171+
Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
167172
data.oYaba[parent] += Ia;
168173
data.of[parent] += fi;
169174
}

‎include/pinocchio/algorithm/aba.hxx

Lines changed: 25 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,11 @@ namespace pinocchio
106106
const Eigen::MatrixBase<ConfigVectorType> & q,
107107
const Eigen::MatrixBase<TangentVectorType> & v)
108108
{
109+
assert(
110+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
111+
== false)
112+
&& "Algorithm not supported for mimic joints");
113+
109114
typedef typename Model::JointIndex JointIndex;
110115
typedef typename Data::Motion Motion;
111116

@@ -169,7 +174,8 @@ namespace pinocchio
169174

170175
Force & fi = data.of[i];
171176

172-
jmodel.jointVelocityExtendedModelSelector(data.u).noalias() -= Jcols.transpose() * fi.toVector();
177+
jmodel.jointVelocityExtendedModelSelector(data.u).noalias() -=
178+
Jcols.transpose() * fi.toVector();
173179

174180
jdata.U().noalias() = Ia * Jcols;
175181
jdata.StU().noalias() = Jcols.transpose() * jdata.U();
@@ -184,8 +190,9 @@ namespace pinocchio
184190
{
185191
Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
186192

187-
fi.toVector().noalias() += Ia * data.oa_gf[i].toVector()
188-
+ jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
193+
fi.toVector().noalias() +=
194+
Ia * data.oa_gf[i].toVector()
195+
+ jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
189196
data.oYaba[parent] += Ia;
190197
data.of[parent] += fi;
191198
}
@@ -379,6 +386,11 @@ namespace pinocchio
379386
const Eigen::MatrixBase<ConfigVectorType> & q,
380387
const Eigen::MatrixBase<TangentVectorType> & v)
381388
{
389+
assert(
390+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
391+
== false)
392+
&& "Algorithm not supported for mimic joints");
393+
382394
typedef typename Model::JointIndex JointIndex;
383395

384396
const JointIndex i = jmodel.id();
@@ -426,13 +438,15 @@ namespace pinocchio
426438

427439
jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i];
428440
jmodel.calc_aba(
429-
jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia, parent > 0);
441+
jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia,
442+
parent > 0);
430443

431444
if (parent > 0)
432445
{
433446
Force & pa = data.f[i];
434-
pa.toVector().noalias() += Ia * data.a_gf[i].toVector()
435-
+ jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
447+
pa.toVector().noalias() +=
448+
Ia * data.a_gf[i].toVector()
449+
+ jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
436450
data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia);
437451
data.f[parent] += data.liMi[i].act(pa);
438452
}
@@ -621,6 +635,11 @@ namespace pinocchio
621635
Data & data,
622636
const Eigen::MatrixBase<ConfigVectorType> & q)
623637
{
638+
assert(
639+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
640+
== false)
641+
&& "Algorithm not supported for mimic joints");
642+
624643
typedef typename Model::JointIndex JointIndex;
625644

626645
const JointIndex & i = jmodel.id();

‎include/pinocchio/algorithm/center-of-mass-derivatives.hxx

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,11 @@ namespace pinocchio
3535
Data & data,
3636
const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq)
3737
{
38+
assert(
39+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
40+
== false)
41+
&& "Algorithm not supported for mimic joints");
42+
3843
typedef typename Model::JointIndex JointIndex;
3944
typedef typename Data::Motion Motion;
4045

‎include/pinocchio/algorithm/compute-all-terms.hxx

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,11 @@ namespace pinocchio
4141
const Eigen::MatrixBase<ConfigVectorType> & q,
4242
const Eigen::MatrixBase<TangentVectorType> & v)
4343
{
44+
assert(
45+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
46+
== false)
47+
&& "Algorithm not supported for mimic joints");
48+
4449
typedef typename Model::JointIndex JointIndex;
4550
typedef
4651
typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type

‎include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,11 @@ namespace pinocchio
4141
const Model & model,
4242
Data & data)
4343
{
44+
assert(
45+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
46+
== false)
47+
&& "Algorithm not supported for mimic joints");
48+
4449
typedef typename Model::JointIndex JointIndex;
4550
typedef typename Data::Motion Motion;
4651

@@ -81,8 +86,8 @@ namespace pinocchio
8186
Motion & oa_gf = data.oa_gf[i];
8287
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
8388
const typename Data::TangentVectorType & a = data.ddq;
84-
data.a[i] =
85-
jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
89+
data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c()
90+
+ (data.v[i] ^ jdata.v());
8691
if (parent > 0)
8792
data.a[i] += data.liMi[i].actInv(data.a[parent]);
8893
oa = data.oMi[i].act(data.a[i]);

‎include/pinocchio/algorithm/constrained-dynamics.hxx

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,11 @@ namespace pinocchio
9292
const Eigen::MatrixBase<ConfigVectorType> & q,
9393
const Eigen::MatrixBase<TangentVectorType> & v)
9494
{
95+
assert(
96+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
97+
== false)
98+
&& "Algorithm not supported for mimic joints");
99+
95100
typedef typename Model::JointIndex JointIndex;
96101
typedef typename Data::Motion Motion;
97102
typedef typename Data::Force Force;

‎include/pinocchio/algorithm/energy.hxx

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,11 @@ namespace pinocchio
2727
const Model & model,
2828
Data & data)
2929
{
30+
assert(
31+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
32+
== false)
33+
&& "Algorithm not supported for mimic joints");
34+
3035
const JointIndex & i = jmodel.id();
3136
data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
3237
data.kinetic_energy += (jmodel.jointVelocityExtendedModelSelector(model.armature).array()

‎include/pinocchio/algorithm/geometry.hxx

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -45,9 +45,7 @@ namespace pinocchio
4545
{
4646
const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint;
4747
if (joint_id > 0)
48-
geom_data.oMg[i] =
49-
(data.oMi[joint_id]
50-
* geom_model.geometryObjects[i].placement);
48+
geom_data.oMg[i] = (data.oMi[joint_id] * geom_model.geometryObjects[i].placement);
5149
else
5250
geom_data.oMg[i] = geom_model.geometryObjects[i].placement;
5351
}

‎include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,11 @@ namespace pinocchio
5454
const Eigen::MatrixBase<Matrix3xOut1> & v_partial_dq,
5555
const Eigen::MatrixBase<Matrix3xOut2> & v_partial_dv)
5656
{
57+
assert(
58+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
59+
== false)
60+
&& "Algorithm not supported for mimic joints");
61+
5762
typedef typename Model::JointIndex JointIndex;
5863
typedef typename Data::SE3 SE3;
5964
typedef typename Data::Motion Motion;
@@ -160,6 +165,7 @@ namespace pinocchio
160165
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
161166
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
162167
{
168+
163169
typedef typename Model::JointIndex JointIndex;
164170
typedef typename Data::SE3 SE3;
165171
typedef typename Data::Motion Motion;

‎include/pinocchio/algorithm/jacobian.hxx

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -371,8 +371,7 @@ namespace pinocchio
371371
for (JointIndex i = jointId; i > 0; i = model.parents[i])
372372
{
373373
Pass::run(
374-
model.joints[i], data.joints[i],
375-
typename Pass::ArgsType(model, data, q.derived(), J_));
374+
model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J_));
376375
}
377376
}
378377

‎include/pinocchio/algorithm/kinematics-derivatives.hxx

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,11 @@ namespace pinocchio
5151
const Eigen::MatrixBase<TangentVectorType1> & v,
5252
const Eigen::MatrixBase<TangentVectorType2> & a)
5353
{
54+
assert(
55+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
56+
== false)
57+
&& "Algorithm not supported for mimic joints");
58+
5459
typedef typename Model::JointIndex JointIndex;
5560
typedef typename Data::SE3 SE3;
5661
typedef typename Data::Motion Motion;
@@ -76,7 +81,8 @@ namespace pinocchio
7681
if (parent > 0)
7782
vi += data.liMi[i].actInv(data.v[parent]);
7883

79-
ai = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (vi ^ jdata.v());
84+
ai =
85+
jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (vi ^ jdata.v());
8086
if (parent > 0)
8187
ai += data.liMi[i].actInv(data.a[parent]);
8288

‎include/pinocchio/algorithm/kinematics.hxx

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -233,8 +233,8 @@ namespace pinocchio
233233
else
234234
data.oMi[i] = data.liMi[i];
235235

236-
data.a[i] =
237-
jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
236+
data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c()
237+
+ (data.v[i] ^ jdata.v());
238238
data.a[i] += data.liMi[i].actInv(data.a[parent]);
239239
}
240240
};

‎include/pinocchio/algorithm/pv.hxx

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -191,8 +191,9 @@ namespace pinocchio
191191

192192
if (parent > 0)
193193
{
194-
pa.toVector().noalias() += Ia * data.a_bias[i].toVector()
195-
+ jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
194+
pa.toVector().noalias() +=
195+
Ia * data.a_bias[i].toVector()
196+
+ jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u);
196197
data.Yaba[parent] += impl::internal::SE3actOn<Scalar>::run(data.liMi[i], Ia);
197198
data.f[parent] += data.liMi[i].act(pa);
198199
}
@@ -311,7 +312,8 @@ namespace pinocchio
311312
const JointIndex parent = model.parents[i];
312313

313314
// Abusing otherwise unused data.tau below.
314-
jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() = jdata.S().transpose() * data.f[i];
315+
jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() =
316+
jdata.S().transpose() * data.f[i];
315317
jmodel.jointVelocitySelector(data.u) -= jmodel.jointVelocitySelector(data.tau);
316318
data.f[i].toVector().noalias() -=
317319
jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.tau);

‎include/pinocchio/algorithm/regressor.hxx

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -363,6 +363,11 @@ namespace pinocchio
363363
const Eigen::MatrixBase<TangentVectorType1> & v,
364364
const Eigen::MatrixBase<TangentVectorType2> & a)
365365
{
366+
assert(
367+
(std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
368+
== false)
369+
&& "Algorithm not supported for mimic joints");
370+
366371
typedef typename Model::JointIndex JointIndex;
367372

368373
const JointIndex i = jmodel.id();

‎include/pinocchio/algorithm/rnea-derivatives.hxx

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -306,8 +306,8 @@ namespace pinocchio
306306
else
307307
data.oMi[i] = data.liMi[i];
308308

309-
data.a[i] =
310-
jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
309+
data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c()
310+
+ (data.v[i] ^ jdata.v());
311311
if (parent > 0)
312312
{
313313
data.a[i] += data.liMi[i].actInv(data.a[parent]);

‎include/pinocchio/algorithm/rnea-second-order-derivatives.hxx

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -81,13 +81,13 @@ namespace pinocchio
8181
typedef
8282
typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
8383
ColsBlock;
84-
ColsBlock J_cols =
85-
jmodel.jointExtendedModelCols(data.J); // data.J has all the phi (in world frame) stacked in columns
86-
ColsBlock psid_cols =
87-
jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame
84+
ColsBlock J_cols = jmodel.jointExtendedModelCols(
85+
data.J); // data.J has all the phi (in world frame) stacked in columns
86+
ColsBlock psid_cols = jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame
8887
ColsBlock psidd_cols =
8988
jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame
90-
ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); // This here is phi_dot in world frame
89+
ColsBlock dJ_cols =
90+
jmodel.jointExtendedModelCols(data.dJ); // This here is phi_dot in world frame
9191

9292
J_cols.noalias() =
9393
data.oMi[i].act(jdata.S()); // J_cols is just the phi in world frame for a joint
@@ -98,8 +98,8 @@ namespace pinocchio
9898
ov, psid_cols,
9999
psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs
100100
ov += vJ;
101-
oa +=
102-
(ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c());
101+
oa += (ov ^ vJ)
102+
+ data.oMi[i].act(jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c());
103103
motionSet::motionAction(ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i))
104104
// + vJ Composite rigid body inertia
105105
Inertia & oY = data.oYcrb[i];

‎include/pinocchio/multibody/joint/joint-basic-visitors.hxx

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -195,8 +195,9 @@ namespace pinocchio
195195

196196
template<typename InputType, typename ReturnType>
197197
struct jointConfigExtendedModelSelectorVisitor
198-
: fusion::
199-
JointUnaryVisitorBase<jointConfigExtendedModelSelectorVisitor<InputType, ReturnType>, ReturnType>
198+
: fusion::JointUnaryVisitorBase<
199+
jointConfigExtendedModelSelectorVisitor<InputType, ReturnType>,
200+
ReturnType>
200201
{
201202
typedef boost::fusion::vector<InputType> ArgsType;
202203

‎include/pinocchio/multibody/joint/joint-composite.hpp

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -542,8 +542,7 @@ namespace pinocchio
542542
return A.middleCols(Base::i_j, nj());
543543
}
544544
template<typename D>
545-
typename SizeDepType<NV>::template ColsReturn<D>::Type
546-
jointCols(Eigen::MatrixBase<D> & A) const
545+
typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
547546
{
548547
return A.middleCols(Base::i_v, nv());
549548
}

‎include/pinocchio/multibody/joint/joint-model-base.hpp

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -435,8 +435,7 @@ namespace pinocchio
435435
// Non-const access
436436
// TODO rename Jac/Vel into Full/Red (for full system and reduced system ?)
437437
template<typename D>
438-
typename SizeDepType<NV>::template ColsReturn<D>::Type
439-
jointCols(Eigen::MatrixBase<D> & A) const
438+
typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
440439
{
441440
return derived().jointCols_impl(A.derived());
442441
}
@@ -494,8 +493,7 @@ namespace pinocchio
494493

495494
// Non-const access
496495
template<typename D>
497-
typename SizeDepType<NV>::template RowsReturn<D>::Type
498-
jointRows(Eigen::MatrixBase<D> & A) const
496+
typename SizeDepType<NV>::template RowsReturn<D>::Type jointRows(Eigen::MatrixBase<D> & A) const
499497
{
500498
return derived().jointRows_impl(A.derived());
501499
}

0 commit comments

Comments
 (0)