GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-3d.hxx
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 118 126 93.7%
Branches: 182 367 49.6%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 namespace crocoddyl {
11
12 template <typename Scalar>
13 1181 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
14 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
15 const Vector3s& xref, const pinocchio::ReferenceFrame type,
16 const std::size_t nu, const Vector2s& gains)
17
2/4
✓ Branch 4 taken 1181 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1181 times.
✗ Branch 8 not taken.
 1181 : Base(state, type, 3, nu), xref_(xref), gains_(gains) {
18 1181 id_ = id;
19 1181 }
20
21 template <typename Scalar>
22 5 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
23 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
24 const Vector3s& xref, const pinocchio::ReferenceFrame type,
25 const Vector2s& gains)
26
2/4
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 : Base(state, type, 3), xref_(xref), gains_(gains) {
27 5 id_ = id;
28 5 }
29
30 #pragma GCC diagnostic push // TODO: Remove once the deprecated FrameXX has
31 // been removed in a future release
32 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
33
34 template <typename Scalar>
35 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
36 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
37 const Vector3s& xref, const std::size_t nu, const Vector2s& gains)
38 : Base(state, pinocchio::ReferenceFrame::LOCAL, 3, nu),
39 xref_(xref),
40 gains_(gains) {
41 id_ = id;
42 std::cerr << "Deprecated: Use constructor that passes the type of contact, "
43 "this assumes is pinocchio::LOCAL."
44 << std::endl;
45 }
46
47 template <typename Scalar>
48 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
49 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
50 const Vector3s& xref, const Vector2s& gains)
51 : Base(state, pinocchio::ReferenceFrame::LOCAL, 3),
52 xref_(xref),
53 gains_(gains) {
54 id_ = id;
55 std::cerr << "Deprecated: Use constructor that passes the type of contact, "
56 "this assumes is pinocchio::LOCAL."
57 << std::endl;
58 }
59
60 #pragma GCC diagnostic pop
61
62 template <typename Scalar>
63 2376 ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}
64
65 template <typename Scalar>
66 80635 void ContactModel3DTpl<Scalar>::calc(
67 const std::shared_ptr<ContactDataAbstract>& data,
68 const Eigen::Ref<const VectorXs>&) {
69 80635 Data* d = static_cast<Data*>(data.get());
70
1/2
✓ Branch 4 taken 80635 times.
✗ Branch 5 not taken.
 80635 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
71 id_);
72
1/2
✓ Branch 3 taken 80635 times.
✗ Branch 4 not taken.
 80635 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
73 80635 id_, pinocchio::LOCAL, d->fJf);
74
2/4
✓ Branch 3 taken 80635 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 80635 times.
✗ Branch 7 not taken.
 80635 d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
75 80635 *d->pinocchio, id_);
76
1/2
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
 80635 d->a0_local =
77 pinocchio::getFrameClassicalAcceleration(
78 80635 *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
79
2/4
✓ Branch 2 taken 80635 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 80635 times.
✗ Branch 6 not taken.
 161270 .linear();
80
81
2/4
✓ Branch 2 taken 80635 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 80635 times.
✗ Branch 6 not taken.
 80635 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
82
3/4
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 80571 times.
✓ Branch 4 taken 64 times.
 80635 if (gains_[0] != 0.) {
83
3/6
✓ Branch 2 taken 80571 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 80571 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 80571 times.
✗ Branch 9 not taken.
 80571 d->dp = d->pinocchio->oMf[id_].translation() - xref_;
84
4/8
✓ Branch 1 taken 80571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80571 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80571 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 80571 times.
✗ Branch 11 not taken.
 80571 d->dp_local.noalias() = oRf.transpose() * d->dp;
85
3/6
✓ Branch 1 taken 80571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80571 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80571 times.
✗ Branch 8 not taken.
 80571 d->a0_local += gains_[0] * d->dp_local;
86 }
87
3/4
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 80571 times.
✓ Branch 4 taken 64 times.
 80635 if (gains_[1] != 0.) {
88
4/8
✓ Branch 1 taken 80571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80571 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80571 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 80571 times.
✗ Branch 11 not taken.
 80571 d->a0_local += gains_[1] * d->v.linear();
89 }
90
2/3
✓ Branch 0 taken 46129 times.
✓ Branch 1 taken 34506 times.
✗ Branch 2 not taken.
 80635 switch (type_) {
91 46129 case pinocchio::ReferenceFrame::LOCAL:
92
2/4
✓ Branch 1 taken 46129 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46129 times.
✗ Branch 5 not taken.
 46129 d->Jc = d->fJf.template topRows<3>();
93
1/2
✓ Branch 1 taken 46129 times.
✗ Branch 2 not taken.
 46129 d->a0 = d->a0_local;
94 46129 break;
95 34506 case pinocchio::ReferenceFrame::WORLD:
96 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
97
4/8
✓ Branch 1 taken 34506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34506 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34506 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 34506 times.
✗ Branch 11 not taken.
 34506 d->Jc.noalias() = oRf * d->fJf.template topRows<3>();
98
3/6
✓ Branch 1 taken 34506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34506 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34506 times.
✗ Branch 8 not taken.
 34506 d->a0.noalias() = oRf * d->a0_local;
99 34506 break;
100 }
101 80635 }
102
103 template <typename Scalar>
104 12665 void ContactModel3DTpl<Scalar>::calcDiff(
105 const std::shared_ptr<ContactDataAbstract>& data,
106 const Eigen::Ref<const VectorXs>&) {
107 12665 Data* d = static_cast<Data*>(data.get());
108 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
109 12665 const pinocchio::JointIndex joint =
110 12665 state_->get_pinocchio()->frames[d->frame].parentJoint;
111 #else
112 const pinocchio::JointIndex joint =
113 state_->get_pinocchio()->frames[d->frame].parent;
114 #endif
115
1/2
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
 12665 pinocchio::getJointAccelerationDerivatives(
116 12665 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
117 12665 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
118 12665 const std::size_t nv = state_->get_nv();
119
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 pinocchio::skew(d->v.linear(), d->vv_skew);
120
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 pinocchio::skew(d->v.angular(), d->vw_skew);
121
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
122
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
123
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
124
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
125
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
126
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vw_skew * d->fXjdv_dq.template topRows<3>();
127
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() -=
128
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
129
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
130
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv).noalias() +=
131
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vw_skew * d->fJf.template topRows<3>();
132
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv).noalias() -=
133
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vv_skew * d->fJf.template bottomRows<3>();
134
2/4
✓ Branch 2 taken 12665 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12665 times.
✗ Branch 6 not taken.
 12665 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
135
136
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12665 times.
✗ Branch 4 not taken.
 12665 if (gains_[0] != 0.) {
137
1/2
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
 12665 pinocchio::skew(d->dp_local, d->dp_skew);
138
4/8
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12665 times.
✗ Branch 11 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
139
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
140
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
141
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 25330 gains_[0] * d->fJf.template topRows<3>();
142 }
143
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12665 times.
✗ Branch 4 not taken.
 12665 if (gains_[1] != 0.) {
144
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
145
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 gains_[1] * d->fXjdv_dq.template topRows<3>();
146
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv).noalias() +=
147
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 25330 gains_[1] * d->fJf.template topRows<3>();
148 }
149
2/3
✓ Branch 0 taken 7591 times.
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
 12665 switch (type_) {
150 7591 case pinocchio::ReferenceFrame::LOCAL:
151
1/2
✓ Branch 1 taken 7591 times.
✗ Branch 2 not taken.
 7591 d->da0_dx = d->da0_local_dx;
152 7591 break;
153 5074 case pinocchio::ReferenceFrame::WORLD:
154 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
155 // Recalculate the constrained accelerations after imposing contact
156 // constraints. This is necessary for the forward-dynamics case.
157
1/2
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
 5074 d->a0_local = pinocchio::getFrameClassicalAcceleration(
158 5074 *state_->get_pinocchio().get(), *d->pinocchio, id_,
159 pinocchio::LOCAL)
160
2/4
✓ Branch 2 taken 5074 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5074 times.
✗ Branch 6 not taken.
 10148 .linear();
161
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5074 times.
✗ Branch 4 not taken.
 5074 if (gains_[0] != 0.) {
162
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->a0_local += gains_[0] * d->dp_local;
163 }
164
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5074 times.
✗ Branch 4 not taken.
 5074 if (gains_[1] != 0.) {
165
4/8
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5074 times.
✗ Branch 11 not taken.
 5074 d->a0_local += gains_[1] * d->v.linear();
166 }
167
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->a0.noalias() = oRf * d->a0_local;
168
169
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
 5074 pinocchio::skew(d->a0.template head<3>(), d->a0_skew);
170
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->a0_world_skew.noalias() = d->a0_skew * oRf;
171
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->da0_dx.noalias() = oRf * d->da0_local_dx;
172
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->da0_dx.leftCols(nv).noalias() -=
173
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
 5074 d->a0_world_skew * d->fJf.template bottomRows<3>();
174 5074 break;
175 }
176 12665 }
177
178 template <typename Scalar>
179 73535 void ContactModel3DTpl<Scalar>::updateForce(
180 const std::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
181
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 73535 times.
 73535 if (force.size() != 3) {
182 ✗ throw_pretty(
183 "Invalid argument: " << "lambda has wrong dimension (it should be 3)");
184 }
185 73535 Data* d = static_cast<Data*>(data.get());
186
1/2
✓ Branch 3 taken 73535 times.
✗ Branch 4 not taken.
 73535 data->f.linear() = force;
187
1/2
✓ Branch 3 taken 73535 times.
✗ Branch 4 not taken.
 73535 data->f.angular().setZero();
188
2/3
✓ Branch 0 taken 42043 times.
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
 73535 switch (type_) {
189 42043 case pinocchio::ReferenceFrame::LOCAL:
190
2/4
✓ Branch 2 taken 42043 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 42043 times.
✗ Branch 7 not taken.
 42043 data->fext = d->jMf.act(data->f);
191
1/2
✓ Branch 2 taken 42043 times.
✗ Branch 3 not taken.
 42043 data->dtau_dq.setZero();
192 42043 break;
193 31492 case pinocchio::ReferenceFrame::WORLD:
194 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
195
2/4
✓ Branch 2 taken 31492 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31492 times.
✗ Branch 6 not taken.
 31492 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
196
5/10
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31492 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31492 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 31492 times.
✗ Branch 14 not taken.
 31492 d->f_local.linear().noalias() = oRf.transpose() * force;
197
2/4
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
 31492 d->f_local.angular().setZero();
198
2/4
✓ Branch 2 taken 31492 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31492 times.
✗ Branch 7 not taken.
 31492 data->fext = data->jMf.act(d->f_local);
199
2/4
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
 31492 pinocchio::skew(d->f_local.linear(), d->f_skew);
200
4/8
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31492 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31492 times.
✗ Branch 11 not taken.
 31492 d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
201
4/8
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 31492 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 31492 times.
✗ Branch 12 not taken.
 31492 data->dtau_dq.noalias() =
202
2/4
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
 31492 -d->fJf.template topRows<3>().transpose() * d->fJf_df;
203 31492 break;
204 }
205 73535 }
206
207 template <typename Scalar>
208 std::shared_ptr<ContactDataAbstractTpl<Scalar> >
209 92589 ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
210 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
211
1/2
✓ Branch 2 taken 92589 times.
✗ Branch 3 not taken.
 92589 data);
212 }
213
214 template <typename Scalar>
215 15 void ContactModel3DTpl<Scalar>::print(std::ostream& os) const {
216 15 os << "ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
217 30 << ", type=" << type_ << "}";
218 15 }
219
220 template <typename Scalar>
221 const typename MathBaseTpl<Scalar>::Vector3s&
222 ✗ ContactModel3DTpl<Scalar>::get_reference() const {
223 ✗ return xref_;
224 }
225
226 template <typename Scalar>
227 const typename MathBaseTpl<Scalar>::Vector2s&
228 ✗ ContactModel3DTpl<Scalar>::get_gains() const {
229 ✗ return gains_;
230 }
231
232 template <typename Scalar>
233 ✗ void ContactModel3DTpl<Scalar>::set_reference(const Vector3s& reference) {
234 ✗ xref_ = reference;
235 }
236
237 } // namespace crocoddyl
238