GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-friction-cone.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 58 81 71.6%
Branches: 52 132 39.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 #include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 881 ResidualModelContactFrictionConeTpl<Scalar>::
16 ResidualModelContactFrictionConeTpl(std::shared_ptr<StateMultibody> state,
17 const pinocchio::FrameIndex id,
18 const FrictionCone& fref,
19 const std::size_t nu, const bool fwddyn)
20 881 : Base(state, fref.get_nf() + 1, nu, fwddyn ? true : false,
21 fwddyn ? true : false, true),
22 881 fwddyn_(fwddyn),
23 881 update_jacobians_(true),
24 881 id_(id),
25
6/8
✓ Branch 0 taken 581 times.
✓ Branch 1 taken 300 times.
✓ Branch 2 taken 581 times.
✓ Branch 3 taken 300 times.
✓ Branch 6 taken 881 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 881 times.
✗ Branch 11 not taken.
 1762 fref_(fref) {
26
2/4
✓ Branch 2 taken 881 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 881 times.
 881 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
27 id) {
28 ✗ throw_pretty(
29 "Invalid argument: "
30 << "the frame index is wrong (it does not exist in the robot)");
31 }
32 881 }
33
34 template <typename Scalar>
35 1 ResidualModelContactFrictionConeTpl<Scalar>::
36 ResidualModelContactFrictionConeTpl(std::shared_ptr<StateMultibody> state,
37 const pinocchio::FrameIndex id,
38 const FrictionCone& fref)
39 2 : Base(state, fref.get_nf() + 1),
40 1 fwddyn_(true),
41 1 update_jacobians_(true),
42 1 id_(id),
43
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 fref_(fref) {
44
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
45 id) {
46 ✗ throw_pretty(
47 "Invalid argument: "
48 << "the frame index is wrong (it does not exist in the robot)");
49 }
50 1 }
51
52 template <typename Scalar>
53 60012 void ResidualModelContactFrictionConeTpl<Scalar>::calc(
54 const std::shared_ptr<ResidualDataAbstract>& data,
55 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
56 60012 Data* d = static_cast<Data*>(data.get());
57
58 // Compute the residual of the friction cone. Note that we need to transform
59 // the force to the contact frame
60
4/8
✓ Branch 3 taken 60012 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 60012 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 60012 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60012 times.
✗ Branch 14 not taken.
 60012 data->r.noalias() = fref_.get_A() * d->contact->f.linear();
61 60012 }
62
63 template <typename Scalar>
64 12150 void ResidualModelContactFrictionConeTpl<Scalar>::calc(
65 const std::shared_ptr<ResidualDataAbstract>& data,
66 const Eigen::Ref<const VectorXs>&) {
67 12150 data->r.setZero();
68 12150 }
69
70 template <typename Scalar>
71 8684 void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
72 const std::shared_ptr<ResidualDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
74
3/4
✓ Branch 0 taken 3762 times.
✓ Branch 1 taken 4922 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3762 times.
 8684 if (fwddyn_ || update_jacobians_) {
75 4922 updateJacobians(data);
76 }
77 8684 }
78
79 template <typename Scalar>
80 349 void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
81 const std::shared_ptr<ResidualDataAbstract>& data,
82 const Eigen::Ref<const VectorXs>&) {
83 349 data->Rx.setZero();
84 349 }
85
86 template <typename Scalar>
87 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
88 72435 ResidualModelContactFrictionConeTpl<Scalar>::createData(
89 DataCollectorAbstract* const data) {
90
1/2
✓ Branch 1 taken 72435 times.
✗ Branch 2 not taken.
 144870 std::shared_ptr<ResidualDataAbstract> d =
91 72435 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data);
92
2/2
✓ Branch 0 taken 30942 times.
✓ Branch 1 taken 41493 times.
 72435 if (!fwddyn_) {
93
1/2
✓ Branch 1 taken 30942 times.
✗ Branch 2 not taken.
 30942 updateJacobians(d);
94 }
95 72435 return d;
96 }
97
98 template <typename Scalar>
99 35864 void ResidualModelContactFrictionConeTpl<Scalar>::updateJacobians(
100 const std::shared_ptr<ResidualDataAbstract>& data) {
101 35864 Data* d = static_cast<Data*>(data.get());
102
103 35864 const MatrixXs& df_dx = d->contact->df_dx;
104 35864 const MatrixXs& df_du = d->contact->df_du;
105 35864 const MatrixX3s& A = fref_.get_A();
106
3/4
✓ Branch 0 taken 627 times.
✓ Branch 1 taken 27315 times.
✓ Branch 2 taken 7922 times.
✗ Branch 3 not taken.
 35864 switch (d->contact_type) {
107 627 case Contact2D: {
108 // Valid for xz plane
109
8/16
✓ Branch 2 taken 627 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 627 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 627 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 627 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 627 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 627 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 627 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 627 times.
✗ Branch 25 not taken.
 627 data->Rx.noalias() = A.col(0) * df_dx.row(0) + A.col(2) * df_dx.row(1);
110
8/16
✓ Branch 2 taken 627 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 627 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 627 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 627 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 627 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 627 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 627 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 627 times.
✗ Branch 25 not taken.
 627 data->Ru.noalias() = A.col(0) * df_du.row(0) + A.col(2) * df_du.row(1);
111 627 break;
112 }
113 27315 case Contact3D:
114
2/4
✓ Branch 3 taken 27315 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27315 times.
✗ Branch 7 not taken.
 27315 data->Rx.noalias() = A * df_dx;
115
2/4
✓ Branch 3 taken 27315 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27315 times.
✗ Branch 7 not taken.
 27315 data->Ru.noalias() = A * df_du;
116 27315 break;
117 7922 case Contact6D:
118
3/6
✓ Branch 2 taken 7922 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7922 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7922 times.
✗ Branch 10 not taken.
 7922 data->Rx.noalias() = A * df_dx.template topRows<3>();
119
3/6
✓ Branch 2 taken 7922 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7922 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7922 times.
✗ Branch 10 not taken.
 7922 data->Ru.noalias() = A * df_du.template topRows<3>();
120 7922 break;
121 ✗ default:
122 ✗ break;
123 }
124 35864 update_jacobians_ = false;
125 35864 }
126
127 template <typename Scalar>
128 template <typename NewScalar>
129 ResidualModelContactFrictionConeTpl<NewScalar>
130 ✗ ResidualModelContactFrictionConeTpl<Scalar>::cast() const {
131 typedef ResidualModelContactFrictionConeTpl<NewScalar> ReturnType;
132 typedef StateMultibodyTpl<NewScalar> StateType;
133 ✗ ReturnType ret(
134 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
135 ✗ id_, fref_.template cast<NewScalar>(), nu_, fwddyn_);
136 ✗ return ret;
137 }
138
139 template <typename Scalar>
140 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::print(
141 std::ostream& os) const {
142 ✗ std::shared_ptr<StateMultibody> s =
143 ✗ std::static_pointer_cast<StateMultibody>(state_);
144 os << "ResidualModelContactFrictionCone {frame="
145 ✗ << s->get_pinocchio()->frames[id_].name << ", mu=" << fref_.get_mu()
146 ✗ << "}";
147 }
148
149 template <typename Scalar>
150 ✗ bool ResidualModelContactFrictionConeTpl<Scalar>::is_fwddyn() const {
151 ✗ return fwddyn_;
152 }
153
154 template <typename Scalar>
155 72435 pinocchio::FrameIndex ResidualModelContactFrictionConeTpl<Scalar>::get_id()
156 const {
157 72435 return id_;
158 }
159
160 template <typename Scalar>
161 const FrictionConeTpl<Scalar>&
162 ✗ ResidualModelContactFrictionConeTpl<Scalar>::get_reference() const {
163 ✗ return fref_;
164 }
165
166 template <typename Scalar>
167 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::set_id(
168 const pinocchio::FrameIndex id) {
169 ✗ id_ = id;
170 }
171
172 template <typename Scalar>
173 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::set_reference(
174 const FrictionCone& reference) {
175 ✗ fref_ = reference;
176 ✗ update_jacobians_ = true;
177 }
178
179 } // namespace crocoddyl
180