GCC Code Coverage Report

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