| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
: R_(Matrix3s::Identity()), |
||
18 |
box_(std::numeric_limits<Scalar>::infinity(), |
||
19 |
✓✗✓✗ |
3 |
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 |
: A_(other.get_A()), |
||
50 |
ub_(other.get_ub()), |
||
51 |
lb_(other.get_lb()), |
||
52 |
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 |
✓✗ | 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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
59 |
A_.row(0) << -W * R_.col(2).transpose(), R_.col(0).transpose(); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
59 |
A_.row(1) << -W * R_.col(2).transpose(), -R_.col(0).transpose(); |
76 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
59 |
A_.row(2) << -L * R_.col(2).transpose(), R_.col(1).transpose(); |
77 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
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 |
✓✗ | 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 |
| Generated by: GCOVR (Version 4.2) |