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