GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-friction-cone.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 74 0.0%
Branches: 0 132 0.0%
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 ✗ 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 ✗ : Base(state, fref.get_nf() + 1, nu, fwddyn ? true : false,
21 fwddyn ? true : false, true),
22 ✗ fwddyn_(fwddyn),
23 ✗ update_jacobians_(true),
24 ✗ id_(id),
25 ✗ fref_(fref) {
26 ✗ 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 }
33
34 template <typename Scalar>
35 ✗ ResidualModelContactFrictionConeTpl<Scalar>::
36 ResidualModelContactFrictionConeTpl(std::shared_ptr<StateMultibody> state,
37 const pinocchio::FrameIndex id,
38 const FrictionCone& fref)
39 ✗ : Base(state, fref.get_nf() + 1),
40 ✗ fwddyn_(true),
41 ✗ update_jacobians_(true),
42 ✗ id_(id),
43 ✗ fref_(fref) {
44 ✗ 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 }
51
52 template <typename Scalar>
53 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::calc(
54 const std::shared_ptr<ResidualDataAbstract>& data,
55 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
56 ✗ 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 ✗ data->r.noalias() = fref_.get_A() * d->contact->f.linear();
61 }
62
63 template <typename Scalar>
64 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::calc(
65 const std::shared_ptr<ResidualDataAbstract>& data,
66 const Eigen::Ref<const VectorXs>&) {
67 ✗ data->r.setZero();
68 }
69
70 template <typename Scalar>
71 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
72 const std::shared_ptr<ResidualDataAbstract>& data,
73 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
74 ✗ if (fwddyn_ || update_jacobians_) {
75 ✗ updateJacobians(data);
76 }
77 }
78
79 template <typename Scalar>
80 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::calcDiff(
81 const std::shared_ptr<ResidualDataAbstract>& data,
82 const Eigen::Ref<const VectorXs>&) {
83 ✗ data->Rx.setZero();
84 }
85
86 template <typename Scalar>
87 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
88 ✗ ResidualModelContactFrictionConeTpl<Scalar>::createData(
89 DataCollectorAbstract* const data) {
90 ✗ std::shared_ptr<ResidualDataAbstract> d =
91 ✗ std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data);
92 ✗ if (!fwddyn_) {
93 ✗ updateJacobians(d);
94 }
95 ✗ return d;
96 }
97
98 template <typename Scalar>
99 ✗ void ResidualModelContactFrictionConeTpl<Scalar>::updateJacobians(
100 const std::shared_ptr<ResidualDataAbstract>& data) {
101 ✗ Data* d = static_cast<Data*>(data.get());
102
103 ✗ const MatrixXs& df_dx = d->contact->df_dx;
104 ✗ const MatrixXs& df_du = d->contact->df_du;
105 ✗ const MatrixX3s& A = fref_.get_A();
106 ✗ switch (d->contact_type) {
107 ✗ case Contact2D: {
108 // Valid for xz plane
109 ✗ data->Rx.noalias() = A.col(0) * df_dx.row(0) + A.col(2) * df_dx.row(1);
110 ✗ data->Ru.noalias() = A.col(0) * df_du.row(0) + A.col(2) * df_du.row(1);
111 ✗ break;
112 }
113 ✗ case Contact3D:
114 ✗ data->Rx.noalias() = A * df_dx;
115 ✗ data->Ru.noalias() = A * df_du;
116 ✗ break;
117 ✗ case Contact6D:
118 ✗ data->Rx.noalias() = A * df_dx.template topRows<3>();
119 ✗ data->Ru.noalias() = A * df_du.template topRows<3>();
120 ✗ break;
121 ✗ default:
122 ✗ break;
123 }
124 ✗ update_jacobians_ = false;
125 }
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 ✗ pinocchio::FrameIndex ResidualModelContactFrictionConeTpl<Scalar>::get_id()
156 const {
157 ✗ 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