GCC Code Coverage Report

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