GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-2d.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 84 111 75.7%
Branches: 151 360 41.9%
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 133 ContactModel2DTpl<Scalar>::ContactModel2DTpl(
14 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
15 const Vector2s& xref, const std::size_t nu, const Vector2s& gains)
16 : Base(state, pinocchio::ReferenceFrame::LOCAL, 2, nu),
17 133 xref_(xref),
18
3/6
✓ Branch 2 taken 133 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 133 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 133 times.
✗ Branch 10 not taken.
 133 gains_(gains) {
19 133 id_ = id;
20 133 }
21
22 template <typename Scalar>
23 ✗ ContactModel2DTpl<Scalar>::ContactModel2DTpl(
24 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
25 const Vector2s& xref, const Vector2s& gains)
26 : Base(state, pinocchio::ReferenceFrame::LOCAL, 2),
27 ✗ xref_(xref),
28 ✗ gains_(gains) {
29 ✗ id_ = id;
30 }
31
32 template <typename Scalar>
33 5694 void ContactModel2DTpl<Scalar>::calc(
34 const std::shared_ptr<ContactDataAbstract>& data,
35 const Eigen::Ref<const VectorXs>&) {
36 5694 Data* d = static_cast<Data*>(data.get());
37 5694 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
38 id_);
39 5694 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
40 5694 id_, pinocchio::LOCAL, d->fJf);
41
1/2
✓ Branch 4 taken 5694 times.
✗ Branch 5 not taken.
 5694 d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
42 5694 *d->pinocchio, id_);
43
1/2
✓ Branch 4 taken 5694 times.
✗ Branch 5 not taken.
 5694 d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(),
44 5694 *d->pinocchio, id_);
45
46
2/4
✓ Branch 2 taken 5694 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5694 times.
✗ Branch 6 not taken.
 5694 d->Jc.row(0) = d->fJf.row(0);
47
2/4
✓ Branch 2 taken 5694 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5694 times.
✗ Branch 6 not taken.
 5694 d->Jc.row(1) = d->fJf.row(2);
48
49
1/2
✓ Branch 2 taken 5694 times.
✗ Branch 3 not taken.
 5694 d->vw = d->v.angular();
50
1/2
✓ Branch 2 taken 5694 times.
✗ Branch 3 not taken.
 5694 d->vv = d->v.linear();
51
52
6/12
✓ Branch 2 taken 5694 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5694 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5694 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5694 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5694 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5694 times.
✗ Branch 18 not taken.
 5694 d->a0[0] = d->a.linear()[0] + d->vw[1] * d->vv[2] - d->vw[2] * d->vv[1];
53
6/12
✓ Branch 2 taken 5694 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5694 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5694 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 5694 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5694 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 5694 times.
✗ Branch 18 not taken.
 5694 d->a0[1] = d->a.linear()[2] + d->vw[0] * d->vv[1] - d->vw[1] * d->vv[0];
54
55
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5694 times.
 5694 if (gains_[0] != 0.) {
56 ✗ d->a0[0] +=
57 ✗ gains_[0] * (d->pinocchio->oMf[id_].translation()[0] - xref_[0]);
58 ✗ d->a0[1] +=
59 ✗ gains_[0] * (d->pinocchio->oMf[id_].translation()[2] - xref_[1]);
60 }
61
2/2
✓ Branch 1 taken 5690 times.
✓ Branch 2 taken 4 times.
 5694 if (gains_[1] != 0.) {
62 5690 d->a0[0] += gains_[1] * d->vv[0];
63 5690 d->a0[1] += gains_[1] * d->vv[2];
64 }
65 5694 }
66
67 template <typename Scalar>
68 1266 void ContactModel2DTpl<Scalar>::calcDiff(
69 const std::shared_ptr<ContactDataAbstract>& data,
70 const Eigen::Ref<const VectorXs>&) {
71 1266 Data* d = static_cast<Data*>(data.get());
72 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
73 1266 const pinocchio::JointIndex joint =
74 1266 state_->get_pinocchio()->frames[d->frame].parentJoint;
75 #else
76 const pinocchio::JointIndex joint =
77 state_->get_pinocchio()->frames[d->frame].parent;
78 #endif
79 1266 pinocchio::getJointAccelerationDerivatives(
80 1266 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
81 1266 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
82 1266 const std::size_t nv = state_->get_nv();
83 1266 pinocchio::skew(d->vv, d->vv_skew);
84 1266 pinocchio::skew(d->vw, d->vw_skew);
85
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
86
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
87
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
88
89
3/6
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1266 times.
✗ Branch 9 not taken.
 1266 d->da0_dx.leftCols(nv).row(0) = d->fXjda_dq.row(0);
90
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.leftCols(nv).row(0).noalias() +=
91
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vw_skew.row(0) * d->fXjdv_dq.template topRows<3>();
92
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.leftCols(nv).row(0).noalias() -=
93
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vv_skew.row(0) * d->fXjdv_dq.template bottomRows<3>();
94
95
3/6
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1266 times.
✗ Branch 9 not taken.
 1266 d->da0_dx.leftCols(nv).row(1) = d->fXjda_dq.row(2);
96
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.leftCols(nv).row(1).noalias() +=
97
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vw_skew.row(2) * d->fXjdv_dq.template topRows<3>();
98
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.leftCols(nv).row(1).noalias() -=
99
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vv_skew.row(2) * d->fXjdv_dq.template bottomRows<3>();
100
101
3/6
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1266 times.
✗ Branch 9 not taken.
 1266 d->da0_dx.rightCols(nv).row(0) = d->fXjda_dv.row(0);
102
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.rightCols(nv).row(0).noalias() +=
103
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vw_skew.row(0) * d->fJf.template topRows<3>();
104
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.rightCols(nv).row(0).noalias() -=
105
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vv_skew.row(0) * d->fJf.template bottomRows<3>();
106
107
3/6
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1266 times.
✗ Branch 9 not taken.
 1266 d->da0_dx.rightCols(nv).row(1) = d->fXjda_dv.row(2);
108
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.rightCols(nv).row(1).noalias() +=
109
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vw_skew.row(2) * d->fJf.template topRows<3>();
110
4/8
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
 1266 d->da0_dx.rightCols(nv).row(1).noalias() -=
111
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 d->vv_skew.row(2) * d->fJf.template bottomRows<3>();
112
113
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1266 times.
 1266 if (gains_[0] != 0.) {
114 ✗ const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
115 ✗ d->oRf(0, 0) = oRf(0, 0);
116 ✗ d->oRf(1, 0) = oRf(2, 0);
117 ✗ d->oRf(0, 1) = oRf(0, 2);
118 ✗ d->oRf(1, 1) = oRf(2, 2);
119 ✗ d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->oRf * d->Jc;
120 }
121
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 if (gains_[1] != 0.) {
122
5/10
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1266 times.
✗ Branch 14 not taken.
 1266 d->da0_dx.leftCols(nv).row(0).noalias() +=
123
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 gains_[1] * d->fXj.row(0) * d->v_partial_dq;
124
5/10
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1266 times.
✗ Branch 14 not taken.
 1266 d->da0_dx.leftCols(nv).row(1).noalias() +=
125
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 gains_[1] * d->fXj.row(2) * d->v_partial_dq;
126
5/10
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1266 times.
✗ Branch 14 not taken.
 1266 d->da0_dx.rightCols(nv).row(0).noalias() +=
127
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 1266 gains_[1] * d->fXj.row(0) * d->a_partial_da;
128
5/10
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1266 times.
✗ Branch 14 not taken.
 1266 d->da0_dx.rightCols(nv).row(1).noalias() +=
129
2/4
✓ Branch 2 taken 1266 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1266 times.
✗ Branch 6 not taken.
 2532 gains_[1] * d->fXj.row(2) * d->a_partial_da;
130 }
131 1266 }
132
133 template <typename Scalar>
134 4835 void ContactModel2DTpl<Scalar>::updateForce(
135 const std::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
136
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4835 times.
 4835 if (force.size() != 2) {
137 ✗ throw_pretty(
138 "Invalid argument: " << "lambda has wrong dimension (it should be 2)");
139 }
140 4835 Data* d = static_cast<Data*>(data.get());
141
2/4
✓ Branch 1 taken 4835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4835 times.
✗ Branch 5 not taken.
 4835 const Eigen::Ref<const Matrix3s> R = d->jMf.rotation();
142
9/18
✓ Branch 1 taken 4835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4835 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4835 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4835 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4835 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4835 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4835 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 4835 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4835 times.
✗ Branch 27 not taken.
 4835 data->f.linear() = R.col(0) * force[0] + R.col(2) * force[1];
143
2/4
✓ Branch 2 taken 4835 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4835 times.
✗ Branch 6 not taken.
 4835 data->f.angular().setZero();
144
9/18
✓ Branch 1 taken 4835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4835 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4835 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4835 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4835 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4835 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4835 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 4835 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 4835 times.
✗ Branch 27 not taken.
 4835 data->fext.linear() = R.col(0) * force[0] + R.col(2) * force[1];
145
5/10
✓ Branch 1 taken 4835 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4835 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4835 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 4835 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 4835 times.
✗ Branch 16 not taken.
 4835 data->fext.angular() = d->jMf.translation().cross(data->fext.linear());
146 4835 }
147
148 template <typename Scalar>
149 std::shared_ptr<ContactDataAbstractTpl<Scalar> >
150 6905 ContactModel2DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
151 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
152
1/2
✓ Branch 2 taken 6905 times.
✗ Branch 3 not taken.
 6905 data);
153 }
154
155 template <typename Scalar>
156 template <typename NewScalar>
157 ✗ ContactModel2DTpl<NewScalar> ContactModel2DTpl<Scalar>::cast() const {
158 typedef ContactModel2DTpl<NewScalar> ReturnType;
159 typedef StateMultibodyTpl<NewScalar> StateType;
160 ✗ ReturnType ret(
161 ✗ std::make_shared<StateType>(state_->template cast<NewScalar>()), id_,
162 ✗ xref_.template cast<NewScalar>(), nu_, gains_.template cast<NewScalar>());
163 ✗ return ret;
164 }
165
166 template <typename Scalar>
167 5 void ContactModel2DTpl<Scalar>::print(std::ostream& os) const {
168 5 os << "ContactModel2D {frame=" << state_->get_pinocchio()->frames[id_].name
169 10 << "}";
170 5 }
171
172 template <typename Scalar>
173 const typename MathBaseTpl<Scalar>::Vector2s&
174 ✗ ContactModel2DTpl<Scalar>::get_reference() const {
175 ✗ return xref_;
176 }
177
178 template <typename Scalar>
179 const typename MathBaseTpl<Scalar>::Vector2s&
180 ✗ ContactModel2DTpl<Scalar>::get_gains() const {
181 ✗ return gains_;
182 }
183
184 template <typename Scalar>
185 ✗ void ContactModel2DTpl<Scalar>::set_reference(const Vector2s& reference) {
186 ✗ xref_ = reference;
187 }
188
189 } // namespace crocoddyl
190