GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/cop-support.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 53 69 76.8%
Branches: 34 76 44.7%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, University of Edinburgh, University of Oxford
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include <math.h>
10
11 #include <iostream>
12
13 namespace crocoddyl {
14
15 template <typename Scalar>
16 3 CoPSupportTpl<Scalar>::CoPSupportTpl()
17
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 : R_(Matrix3s::Identity()),
18
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 box_(std::numeric_limits<Scalar>::infinity(),
19 6 std::numeric_limits<Scalar>::infinity()) {
20 3 A_.setZero();
21 3 ub_.setZero();
22 3 lb_.setZero();
23
24 // Update the inequality matrix and bounds
25 3 update();
26 3 }
27
28 template <typename Scalar>
29 56 CoPSupportTpl<Scalar>::CoPSupportTpl(const Matrix3s& R, const Vector2s& box)
30 56 : R_(R), box_(box) {
31 56 A_.setZero();
32 56 ub_.setZero();
33 56 lb_.setZero();
34
35 // Update the inequality matrix and bounds
36 56 update();
37 56 }
38
39 template <typename Scalar>
40 CoPSupportTpl<Scalar>::CoPSupportTpl(const WrenchConeTpl<Scalar>& other)
41 : A_(other.get_A()),
42 ub_(other.get_ub()),
43 lb_(other.get_lb()),
44 R_(other.get_R()),
45 box_(other.get_box()) {}
46
47 template <typename Scalar>
48 39 CoPSupportTpl<Scalar>::CoPSupportTpl(const CoPSupportTpl<Scalar>& other)
49 39 : A_(other.get_A()),
50 39 ub_(other.get_ub()),
51 39 lb_(other.get_lb()),
52 39 R_(other.get_R()),
53 39 box_(other.get_box()) {}
54
55 template <typename Scalar>
56 98 CoPSupportTpl<Scalar>::~CoPSupportTpl() {}
57
58 template <typename Scalar>
59 59 void CoPSupportTpl<Scalar>::update() {
60 // Initialize the matrix and bounds
61 59 A_.setZero();
62 59 ub_.setZero();
63 59 lb_.setOnes();
64
1/2
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
 59 lb_ *= -std::numeric_limits<Scalar>::infinity();
65
66 // CoP information
67 // This matrix is defined as
68 // [0 0 -W 1 0 0;
69 // 0 0 -W -1 0 0;
70 // 0 0 -L 0 1 0;
71 // 0 0 -L 0 -1 0]
72 59 const Scalar L = box_(0) / Scalar(2.);
73 59 const Scalar W = box_(1) / Scalar(2.);
74
7/14
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 59 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 59 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 59 times.
✗ Branch 21 not taken.
 59 A_.row(0) << -W * R_.col(2).transpose(), R_.col(0).transpose();
75
8/16
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 59 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 59 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 59 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 59 times.
✗ Branch 24 not taken.
 59 A_.row(1) << -W * R_.col(2).transpose(), -R_.col(0).transpose();
76
7/14
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 59 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 59 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 59 times.
✗ Branch 21 not taken.
 59 A_.row(2) << -L * R_.col(2).transpose(), R_.col(1).transpose();
77
8/16
✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 59 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 59 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 59 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 59 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 59 times.
✗ Branch 24 not taken.
 59 A_.row(3) << -L * R_.col(2).transpose(), -R_.col(1).transpose();
78 59 }
79
80 template <typename Scalar>
81 3537 const typename MathBaseTpl<Scalar>::Matrix46s& CoPSupportTpl<Scalar>::get_A()
82 const {
83 3537 return A_;
84 }
85
86 template <typename Scalar>
87 79 const typename MathBaseTpl<Scalar>::Vector4s& CoPSupportTpl<Scalar>::get_ub()
88 const {
89 79 return ub_;
90 }
91
92 template <typename Scalar>
93 69 const typename MathBaseTpl<Scalar>::Vector4s& CoPSupportTpl<Scalar>::get_lb()
94 const {
95 69 return lb_;
96 }
97
98 template <typename Scalar>
99 48 const typename MathBaseTpl<Scalar>::Vector2s& CoPSupportTpl<Scalar>::get_box()
100 const {
101 48 return box_;
102 }
103
104 template <typename Scalar>
105 49 const typename MathBaseTpl<Scalar>::Matrix3s& CoPSupportTpl<Scalar>::get_R()
106 const {
107 49 return R_;
108 }
109
110 template <typename Scalar>
111 ✗ void CoPSupportTpl<Scalar>::set_R(const Matrix3s& R) {
112 ✗ R_ = R;
113 }
114
115 template <typename Scalar>
116 ✗ void CoPSupportTpl<Scalar>::set_box(const Vector2s& box) {
117 ✗ box_ = box;
118 ✗ if (box_(0) < Scalar(0.)) {
119 ✗ box_(0) = std::numeric_limits<Scalar>::infinity();
120 ✗ std::cerr << "Warning: box(0) has to be a positive value, set to inf float"
121 ✗ << std::endl;
122 }
123 ✗ if (box_(1) < Scalar(0.)) {
124 ✗ box_(1) = std::numeric_limits<Scalar>::infinity();
125 ✗ std::cerr << "Warning: box(0) has to be a positive value, set to inf float"
126 ✗ << std::endl;
127 }
128 }
129
130 template <typename Scalar>
131 3 CoPSupportTpl<Scalar>& CoPSupportTpl<Scalar>::operator=(
132 const CoPSupportTpl<Scalar>& other) {
133
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (this != &other) {
134 3 A_ = other.get_A();
135 3 ub_ = other.get_ub();
136 3 lb_ = other.get_lb();
137 3 R_ = other.get_R();
138 3 box_ = other.get_box();
139 }
140 3 return *this;
141 }
142
143 template <typename Scalar>
144 ✗ std::ostream& operator<<(std::ostream& os, const CoPSupportTpl<Scalar>& X) {
145 ✗ os << " R: " << X.get_R() << std::endl;
146 ✗ os << " box: " << X.get_box().transpose() << std::endl;
147 ✗ return os;
148 }
149
150 } // namespace crocoddyl
151