GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-jacobian.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 75 83 90.4%
Branches: 95 277 34.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2021 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_contact_jacobian_hxx__
6 #define __pinocchio_algorithm_contact_jacobian_hxx__
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 template<
16 typename Scalar,
17 int Options,
18 template<typename, int> class JointCollectionTpl,
19 typename Matrix6or3Like>
20 31 void getConstraintJacobian(
21 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
22 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
23 const RigidConstraintModelTpl<Scalar, Options> & constraint_model,
24 RigidConstraintDataTpl<Scalar, Options> & constraint_data,
25 const Eigen::MatrixBase<Matrix6or3Like> & J_)
26 {
27
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 31 assert(model.check(data) && "data is not consistent with model.");
28
2/4
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 31 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
29
30
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 31 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 31 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.rows(), constraint_model.size());
31
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 31 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.cols(), model.nv);
32
33 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
34 typedef typename Model::Motion Motion;
35 typedef typename Model::SE3 SE3;
36 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
37
38 31 Matrix6or3Like & J = J_.const_cast_derived();
39
40 typedef RigidConstraintModelTpl<Scalar, Options> ConstraintModel;
41 31 const typename ConstraintModel::BooleanVector & colwise_joint1_sparsity =
42 constraint_model.colwise_joint1_sparsity;
43 31 const typename ConstraintModel::BooleanVector & colwise_joint2_sparsity =
44 constraint_model.colwise_joint2_sparsity;
45 31 const typename ConstraintModel::IndexVector & colwise_span_indexes =
46 constraint_model.colwise_span_indexes;
47
48 31 SE3 & oMc1 = constraint_data.oMc1;
49
1/2
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 31 oMc1 = data.oMi[constraint_model.joint1_id] * constraint_model.joint1_placement;
50 31 SE3 & oMc2 = constraint_data.oMc2;
51
1/2
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 31 oMc2 = data.oMi[constraint_model.joint2_id] * constraint_model.joint2_placement;
52 31 SE3 & c1Mc2 = constraint_data.c1Mc2;
53
1/2
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
 31 c1Mc2 = oMc1.actInv(oMc2);
54
55
2/2
✓ Branch 1 taken 390 times.
✓ Branch 2 taken 31 times.
 421 for (size_t k = 0; k < colwise_span_indexes.size(); ++k)
56 {
57 390 const Eigen::DenseIndex col_id = colwise_span_indexes[k];
58
59
2/4
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 390 times.
✗ Branch 5 not taken.
 390 const int sign = colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id]
60
5/6
✓ Branch 0 taken 372 times.
✓ Branch 1 taken 18 times.
✓ Branch 3 taken 372 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 342 times.
✓ Branch 6 taken 30 times.
 390 ? colwise_joint1_sparsity[col_id] ? +1 : -1
61 : 0; // specific case for CONTACT_3D
62
63 typedef typename Data::Matrix6x::ConstColXpr ColXprIn;
64
1/2
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
 390 const ColXprIn Jcol_in = data.J.col(col_id);
65
1/2
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
 390 const MotionRef<const ColXprIn> Jcol_motion_in(Jcol_in);
66
67 typedef typename Matrix6or3Like::ColXpr ColXprOut;
68
1/2
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
 390 ColXprOut Jcol_out = J.col(col_id);
69
70
2/3
✓ Branch 0 taken 318 times.
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 390 switch (constraint_model.type)
71 {
72 318 case CONTACT_3D: {
73
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 276 times.
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 318 switch (constraint_model.reference_frame)
74 {
75 ✗ case WORLD: {
76 ✗ Jcol_out.noalias() = Jcol_motion_in.linear() * Scalar(sign);
77 ✗ break;
78 }
79 276 case LOCAL: {
80
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 264 times.
 276 if (sign == 0)
81 {
82
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Motion Jcol_local1(oMc1.actInv(Jcol_motion_in)); // TODO: simplify computations
83
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Motion Jcol_local2(oMc2.actInv(Jcol_motion_in)); // TODO: simplify computations
84
7/14
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 12 times.
✗ Branch 20 not taken.
 12 Jcol_out.noalias() = Jcol_local1.linear() - c1Mc2.rotation() * Jcol_local2.linear();
85 }
86
2/2
✓ Branch 0 taken 252 times.
✓ Branch 1 taken 12 times.
 264 else if (sign == 1)
87 {
88
1/2
✓ Branch 1 taken 252 times.
✗ Branch 2 not taken.
 252 const Motion Jcol_local(oMc1.actInv(Jcol_motion_in));
89
3/6
✓ Branch 1 taken 252 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 252 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 252 times.
✗ Branch 8 not taken.
 252 Jcol_out.noalias() = Jcol_local.linear();
90 }
91 else // sign == -1
92 {
93
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Motion Jcol_local(oMc2.actInv(Jcol_motion_in)); // TODO: simplify computations
94
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 Jcol_out.noalias() =
95
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 24 -c1Mc2.rotation() * Jcol_local.linear(); // TODO: simplify computations
96 }
97 276 break;
98 }
99 42 case LOCAL_WORLD_ALIGNED: {
100
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 36 times.
 42 if (sign == 0)
101 {
102
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 Jcol_out.noalias() =
103
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 12 (oMc2.translation() - oMc1.translation()).cross(Jcol_motion_in.angular());
104 }
105 else
106 {
107
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 6 times.
 36 if (sign == 1)
108
4/8
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
 60 Jcol_out.noalias() =
109
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
 90 Jcol_motion_in.linear() - oMc1.translation().cross(Jcol_motion_in.angular());
110 else
111
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
 12 Jcol_out.noalias() =
112
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 18 -Jcol_motion_in.linear() + oMc2.translation().cross(Jcol_motion_in.angular());
113 }
114 42 break;
115 }
116 }
117 318 break;
118 }
119 72 case CONTACT_6D: {
120
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 MotionRef<ColXprOut> Jcol_motion_out(Jcol_out);
121
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
 72 assert(check_expression_if_real<Scalar>(sign != 0) && "sign should be equal to +1 or -1.");
122
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 switch (constraint_model.reference_frame)
123 {
124 ✗ case WORLD: {
125 ✗ Jcol_motion_out = Jcol_motion_in;
126 ✗ break;
127 }
128 36 case LOCAL: {
129
3/8
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 36 Jcol_motion_out = Scalar(sign) * oMc1.actInv(Jcol_motion_in);
130 36 break;
131 }
132 36 case LOCAL_WORLD_ALIGNED: {
133
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 Motion Jcol_local_world_aligned(Jcol_motion_in);
134
3/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
 36 Jcol_local_world_aligned.linear() -=
135
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 oMc1.translation().cross(Jcol_local_world_aligned.angular());
136
2/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 36 Jcol_motion_out = Scalar(sign) * Jcol_local_world_aligned;
137 36 break;
138 }
139 }
140 72 break;
141 }
142
143 ✗ default:
144 ✗ break;
145 }
146 }
147 31 }
148
149 template<
150 typename Scalar,
151 int Options,
152 template<typename, int> class JointCollectionTpl,
153 typename DynamicMatrixLike,
154 class ConstraintModelAllocator,
155 class ConstraintDataAllocator>
156 8 void getConstraintsJacobian(
157 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
158 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
159 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
160 constraint_models,
161 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> &
162 constraint_datas,
163 const Eigen::MatrixBase<DynamicMatrixLike> & J_)
164 {
165 typedef RigidConstraintModelTpl<Scalar, Options> ContraintModel;
166 typedef RigidConstraintDataTpl<Scalar, Options> ContraintData;
167
168 8 const Eigen::DenseIndex constraint_size = getTotalConstraintSize(constraint_models);
169
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.rows(), constraint_size);
170
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.cols(), model.nv);
171
172 8 DynamicMatrixLike & J = J_.const_cast_derived();
173 8 Eigen::DenseIndex row_id = 0;
174
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 8 times.
 24 for (size_t k = 0; k < constraint_models.size(); ++k)
175 {
176 16 const ContraintModel & cmodel = constraint_models[k];
177 16 ContraintData & cdata = constraint_datas[k];
178
179
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 getConstraintJacobian(model, data, cmodel, cdata, J.middleRows(row_id, cmodel.size()));
180
181 16 row_id += cmodel.size();
182 }
183 8 }
184
185 } // namespace pinocchio
186
187 #endif // ifndef __pinocchio_algorithm_contact_jacobian_hxx__
188