GCC Code Coverage Report

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