GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/math/constraint-bound.cpp Lines: 0 45 0.0 %
Date: 2024-02-02 08:47:34 Branches: 0 64 0.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2017 CNRS
3
//
4
// This file is part of tsid
5
// tsid is free software: you can redistribute it
6
// and/or modify it under the terms of the GNU Lesser General Public
7
// License as published by the Free Software Foundation, either version
8
// 3 of the License, or (at your option) any later version.
9
// tsid is distributed in the hope that it will be
10
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12
// General Lesser Public License for more details. You should have
13
// received a copy of the GNU Lesser General Public License along with
14
// tsid If not, see
15
// <http://www.gnu.org/licenses/>.
16
//
17
18
#include <tsid/math/constraint-bound.hpp>
19
20
using namespace tsid::math;
21
22
ConstraintBound::ConstraintBound(const std::string& name)
23
    : ConstraintBase(name) {}
24
25
ConstraintBound::ConstraintBound(const std::string& name,
26
                                 const unsigned int size)
27
    : ConstraintBase(name, Matrix::Identity(size, size)),
28
      m_lb(Vector::Zero(size)),
29
      m_ub(Vector::Zero(size)) {}
30
31
ConstraintBound::ConstraintBound(const std::string& name, ConstRefVector lb,
32
                                 ConstRefVector ub)
33
    : ConstraintBase(name, Matrix::Identity(lb.size(), lb.size())),
34
      m_lb(lb),
35
      m_ub(ub) {
36
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
37
      lb.size() == ub.size(),
38
      "The size of the lower and upper bound vectors needs to be match!");
39
}
40
41
unsigned int ConstraintBound::rows() const {
42
  assert(m_lb.rows() == m_ub.rows());
43
  return (unsigned int)m_lb.rows();
44
}
45
46
unsigned int ConstraintBound::cols() const {
47
  assert(m_lb.rows() == m_ub.rows());
48
  return (unsigned int)m_lb.rows();
49
}
50
51
void ConstraintBound::resize(const unsigned int r, const unsigned int c) {
52
  PINOCCHIO_CHECK_INPUT_ARGUMENT(r == c, "r and c need to be equal!");
53
  m_A.setIdentity(r, c);
54
  m_lb.setZero(r);
55
  m_ub.setZero(r);
56
}
57
58
bool ConstraintBound::isEquality() const { return false; }
59
bool ConstraintBound::isInequality() const { return false; }
60
bool ConstraintBound::isBound() const { return true; }
61
62
const Vector& ConstraintBound::vector() const {
63
  assert(false);
64
  return m_lb;
65
}
66
const Vector& ConstraintBound::lowerBound() const { return m_lb; }
67
const Vector& ConstraintBound::upperBound() const { return m_ub; }
68
69
Vector& ConstraintBound::vector() {
70
  assert(false);
71
  return m_lb;
72
}
73
Vector& ConstraintBound::lowerBound() { return m_lb; }
74
Vector& ConstraintBound::upperBound() { return m_ub; }
75
76
bool ConstraintBound::setVector(ConstRefVector) {
77
  assert(false);
78
  return false;
79
}
80
bool ConstraintBound::setLowerBound(ConstRefVector lb) {
81
  m_lb = lb;
82
  return true;
83
}
84
bool ConstraintBound::setUpperBound(ConstRefVector ub) {
85
  m_ub = ub;
86
  return true;
87
}
88
89
bool ConstraintBound::checkConstraint(ConstRefVector x, double tol) const {
90
  return (x.array() <= m_ub.array() + tol).all() &&
91
         (x.array() >= m_lb.array() - tol).all();
92
}