GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-friction-cone.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 60 76 78.9%
Branches: 49 118 41.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, 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(boost::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
1/2
✗ Branch 3 not taken.
✓ Branch 4 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(boost::shared_ptr<StateMultibody> state,
37 const pinocchio::FrameIndex id,
38 const FrictionCone& fref)
39 1 : 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
1/2
✗ Branch 3 not taken.
✓ Branch 4 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 1768 ResidualModelContactFrictionConeTpl<
54 1768 Scalar>::~ResidualModelContactFrictionConeTpl() {}
55
56 template <typename Scalar>
57 60012 void ResidualModelContactFrictionConeTpl<Scalar>::calc(
58 const boost::shared_ptr<ResidualDataAbstract>& data,
59 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
60 60012 Data* d = static_cast<Data*>(data.get());
61
62 // Compute the residual of the friction cone. Note that we need to transform
63 // the force to the contact frame
64
3/6
✓ Branch 4 taken 60012 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 60012 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 60012 times.
✗ Branch 12 not taken.
 60012 data->r.noalias() = fref_.get_A() * d->contact->f.linear();
65 60012 }
66
67 template <typename Scalar>
68 12150 void ResidualModelContactFrictionConeTpl<Scalar>::calc(
69 const boost::shared_ptr<ResidualDataAbstract>& data,
70 const Eigen::Ref<const VectorXs>&) {
71 12150 data->r.setZero();
72 12150 }
73
74 template <typename Scalar>
75 8684 void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
76 const boost::shared_ptr<ResidualDataAbstract>& data,
77 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
78
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_) {
79 4922 updateJacobians(data);
80 }
81 8684 }
82
83 template <typename Scalar>
84 349 void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
85 const boost::shared_ptr<ResidualDataAbstract>& data,
86 const Eigen::Ref<const VectorXs>&) {
87 349 data->Rx.setZero();
88 349 }
89
90 template <typename Scalar>
91 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
92 72435 ResidualModelContactFrictionConeTpl<Scalar>::createData(
93 DataCollectorAbstract* const data) {
94
1/2
✓ Branch 1 taken 72435 times.
✗ Branch 2 not taken.
 144870 boost::shared_ptr<ResidualDataAbstract> d = boost::allocate_shared<Data>(
95 72435 Eigen::aligned_allocator<Data>(), this, data);
96
2/2
✓ Branch 0 taken 30942 times.
✓ Branch 1 taken 41493 times.
 72435 if (!fwddyn_) {
97
1/2
✓ Branch 1 taken 30942 times.
✗ Branch 2 not taken.
 30942 updateJacobians(d);
98 }
99 72435 return d;
100 }
101
102 template <typename Scalar>
103 35864 void ResidualModelContactFrictionConeTpl<Scalar>::updateJacobians(
104 const boost::shared_ptr<ResidualDataAbstract>& data) {
105 35864 Data* d = static_cast<Data*>(data.get());
106
107 35864 const MatrixXs& df_dx = d->contact->df_dx;
108 35864 const MatrixXs& df_du = d->contact->df_du;
109 35864 const MatrixX3s& A = fref_.get_A();
110
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) {
111 627 case Contact2D: {
112 // Valid for xz plane
113
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);
114
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);
115 627 break;
116 }
117 27315 case Contact3D:
118
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;
119
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;
120 27315 break;
121 7922 case Contact6D:
122
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>();
123
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>();
124 7922 break;
125 ✗ default:
126 ✗ break;
127 }
128 35864 update_jacobians_ = false;
129 35864 }
130
131 template <typename Scalar>
132 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::print(
133 std::ostream& os) const {
134 ✗ boost::shared_ptr<StateMultibody> s =
135 ✗ boost::static_pointer_cast<StateMultibody>(state_);
136 os << "ResidualModelContactFrictionCone {frame="
137 ✗ << s->get_pinocchio()->frames[id_].name << ", mu=" << fref_.get_mu()
138 ✗ << "}";
139 }
140
141 template <typename Scalar>
142 bool ResidualModelContactFrictionConeTpl<Scalar>::is_fwddyn() const {
143 return fwddyn_;
144 }
145
146 template <typename Scalar>
147 72435 pinocchio::FrameIndex ResidualModelContactFrictionConeTpl<Scalar>::get_id()
148 const {
149 72435 return id_;
150 }
151
152 template <typename Scalar>
153 const FrictionConeTpl<Scalar>&
154 ✗ ResidualModelContactFrictionConeTpl<Scalar>::get_reference() const {
155 ✗ return fref_;
156 }
157
158 template <typename Scalar>
159 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::set_id(
160 const pinocchio::FrameIndex id) {
161 ✗ id_ = id;
162 }
163
164 template <typename Scalar>
165 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::set_reference(
166 const FrictionCone& reference) {
167 ✗ fref_ = reference;
168 ✗ update_jacobians_ = true;
169 }
170
171 } // namespace crocoddyl
172