GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-wrench-cone.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 47 69 68.1%
Branches: 25 104 24.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
10
11 namespace crocoddyl {
12
13 template <typename Scalar>
14 175 ResidualModelContactWrenchConeTpl<Scalar>::ResidualModelContactWrenchConeTpl(
15 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
16 const WrenchCone& fref, const std::size_t nu, const bool fwddyn)
17 175 : Base(state, fref.get_nf() + 13, nu, fwddyn ? true : false,
18 fwddyn ? true : false, true),
19 175 fwddyn_(fwddyn),
20 175 update_jacobians_(true),
21 175 id_(id),
22
6/8
✓ Branch 0 taken 125 times.
✓ Branch 1 taken 50 times.
✓ Branch 2 taken 125 times.
✓ Branch 3 taken 50 times.
✓ Branch 6 taken 175 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 175 times.
✗ Branch 11 not taken.
 350 fref_(fref) {
23
2/4
✓ Branch 2 taken 175 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 175 times.
 175 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
24 id) {
25 ✗ throw_pretty(
26 "Invalid argument: "
27 << "the frame index is wrong (it does not exist in the robot)");
28 }
29 175 }
30
31 template <typename Scalar>
32 1 ResidualModelContactWrenchConeTpl<Scalar>::ResidualModelContactWrenchConeTpl(
33 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
34 const WrenchCone& fref)
35 2 : Base(state, fref.get_nf() + 13),
36 1 fwddyn_(true),
37 1 update_jacobians_(true),
38 1 id_(id),
39
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 fref_(fref) {
40
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) <=
41 id) {
42 ✗ throw_pretty(
43 "Invalid argument: "
44 << "the frame index is wrong (it does not exist in the robot)");
45 }
46 1 }
47
48 template <typename Scalar>
49 17585 void ResidualModelContactWrenchConeTpl<Scalar>::calc(
50 const std::shared_ptr<ResidualDataAbstract>& data,
51 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
52 17585 Data* d = static_cast<Data*>(data.get());
53
54 // Compute the residual of the wrench cone. Note that we need to transform the
55 // wrench to the contact frame
56
2/4
✓ Branch 6 taken 17585 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 17585 times.
✗ Branch 10 not taken.
 17585 data->r.noalias() = fref_.get_A() * d->contact->f.toVector();
57 17585 }
58
59 template <typename Scalar>
60 5872 void ResidualModelContactWrenchConeTpl<Scalar>::calc(
61 const std::shared_ptr<ResidualDataAbstract>& data,
62 const Eigen::Ref<const VectorXs>&) {
63 5872 data->r.setZero();
64 5872 }
65
66 template <typename Scalar>
67 1393 void ResidualModelContactWrenchConeTpl<Scalar>::calcDiff(
68 const std::shared_ptr<ResidualDataAbstract>& data,
69 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
70
3/4
✓ Branch 0 taken 627 times.
✓ Branch 1 taken 766 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 627 times.
 1393 if (fwddyn_ || update_jacobians_) {
71 766 updateJacobians(data);
72 }
73 1393 }
74
75 template <typename Scalar>
76 91 void ResidualModelContactWrenchConeTpl<Scalar>::calcDiff(
77 const std::shared_ptr<ResidualDataAbstract>& data,
78 const Eigen::Ref<const VectorXs>&) {
79 91 data->Rx.setZero();
80 91 }
81
82 template <typename Scalar>
83 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
84 19521 ResidualModelContactWrenchConeTpl<Scalar>::createData(
85 DataCollectorAbstract* const data) {
86
1/2
✓ Branch 1 taken 19521 times.
✗ Branch 2 not taken.
 39042 std::shared_ptr<ResidualDataAbstract> d =
87 19521 std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data);
88
2/2
✓ Branch 0 taken 7156 times.
✓ Branch 1 taken 12365 times.
 19521 if (!fwddyn_) {
89
1/2
✓ Branch 1 taken 7156 times.
✗ Branch 2 not taken.
 7156 updateJacobians(d);
90 }
91 19521 return d;
92 }
93
94 template <typename Scalar>
95 7922 void ResidualModelContactWrenchConeTpl<Scalar>::updateJacobians(
96 const std::shared_ptr<ResidualDataAbstract>& data) {
97 7922 Data* d = static_cast<Data*>(data.get());
98
99 7922 const MatrixXs& df_dx = d->contact->df_dx;
100 7922 const MatrixXs& df_du = d->contact->df_du;
101 7922 const MatrixX6s& A = fref_.get_A();
102
2/4
✓ Branch 3 taken 7922 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7922 times.
✗ Branch 7 not taken.
 7922 data->Rx.noalias() = A * df_dx;
103
2/4
✓ Branch 3 taken 7922 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7922 times.
✗ Branch 7 not taken.
 7922 data->Ru.noalias() = A * df_du;
104 7922 update_jacobians_ = false;
105 7922 }
106
107 template <typename Scalar>
108 template <typename NewScalar>
109 ResidualModelContactWrenchConeTpl<NewScalar>
110 ✗ ResidualModelContactWrenchConeTpl<Scalar>::cast() const {
111 typedef ResidualModelContactWrenchConeTpl<NewScalar> ReturnType;
112 typedef StateMultibodyTpl<NewScalar> StateType;
113 ✗ ReturnType ret(
114 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
115 ✗ id_, fref_.template cast<NewScalar>(), nu_, fwddyn_);
116 ✗ return ret;
117 }
118
119 template <typename Scalar>
120 ✗ void ResidualModelContactWrenchConeTpl<Scalar>::print(std::ostream& os) const {
121 ✗ std::shared_ptr<StateMultibody> s =
122 ✗ std::static_pointer_cast<StateMultibody>(state_);
123 ✗ const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
124 "]");
125 os << "ResidualModelContactWrenchCone {frame="
126 ✗ << s->get_pinocchio()->frames[id_].name << ", mu=" << fref_.get_mu()
127 ✗ << ", box=" << fref_.get_box().transpose().format(fmt) << "}";
128 }
129
130 template <typename Scalar>
131 ✗ bool ResidualModelContactWrenchConeTpl<Scalar>::is_fwddyn() const {
132 ✗ return fwddyn_;
133 }
134
135 template <typename Scalar>
136 19521 pinocchio::FrameIndex ResidualModelContactWrenchConeTpl<Scalar>::get_id()
137 const {
138 19521 return id_;
139 }
140
141 template <typename Scalar>
142 const WrenchConeTpl<Scalar>&
143 ✗ ResidualModelContactWrenchConeTpl<Scalar>::get_reference() const {
144 ✗ return fref_;
145 }
146
147 template <typename Scalar>
148 ✗ void ResidualModelContactWrenchConeTpl<Scalar>::set_id(
149 const pinocchio::FrameIndex id) {
150 ✗ id_ = id;
151 }
152
153 template <typename Scalar>
154 ✗ void ResidualModelContactWrenchConeTpl<Scalar>::set_reference(
155 const WrenchCone& reference) {
156 ✗ fref_ = reference;
157 ✗ update_jacobians_ = true;
158 }
159
160 } // namespace crocoddyl
161