GCC Code Coverage Report

Directory: ./
File: include/multicontact-api/geometry/second-order-cone.hpp
Date: 2025-04-05 01:06:26
Exec Total Coverage
Lines: 28 66 42.4%
Branches: 20 80 25.0%
Line Branch Exec Source
1 // Copyright (c) 2015-2018, CNRS
2 // Authors: Justin Carpentier <jcarpent@laas.fr>
3
4 #ifndef __multicontact_api_geometry_second_order_cone_hpp__
5 #define __multicontact_api_geometry_second_order_cone_hpp__
6
7 #include <Eigen/Dense>
8 #include <iostream>
9
10 #include "multicontact-api/geometry/fwd.hpp"
11 #include "multicontact-api/serialization/archive.hpp"
12 #include "multicontact-api/serialization/eigen-matrix.hpp"
13
14 namespace multicontact_api {
15 namespace geometry {
16
17 template <typename _Scalar, int _dim, int _Options>
18 struct SecondOrderCone : public serialization::Serializable<
19 SecondOrderCone<_Scalar, _dim, _Options> > {
20 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21 enum { dim = _dim, Options = _Options };
22 typedef _Scalar Scalar;
23 typedef Eigen::Matrix<Scalar, dim, dim, Options> MatrixD;
24 typedef Eigen::Matrix<Scalar, dim, 1, Options> VectorD;
25 typedef Eigen::DenseIndex DenseIndex;
26
27 ✗ SecondOrderCone()
28 ✗ : m_Q(MatrixD::Identity()),
29 ✗ m_QPo(_dim, _dim),
30 ✗ m_direction(VectorD::Zero()),
31 ✗ m_Pd(_dim, _dim),
32 ✗ m_Po(_dim, _dim) {
33 ✗ m_direction[_dim - 1] = 1.;
34 ✗ computeProjectors();
35 }
36
37 3 SecondOrderCone(const MatrixD& Q, const VectorD& direction)
38 3 : m_Q(Q),
39
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 3 m_QPo(_dim, _dim),
40 3 m_direction(direction.normalized()),
41
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 3 m_Pd(_dim, _dim),
42
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 3 m_Po(_dim, _dim) {
43
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 3 assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
44
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
 3 assert((Q - Q.transpose()).isMuchSmallerThan(Q));
45 3 computeProjectors();
46 3 }
47
48 ///
49 /// \brief Build a regular cone from a given friction coefficient and a
50 /// direction.
51 ///
52 /// \param mu Friction coefficient.
53 /// \param direction Direction of the cone.
54 ///
55 /// \returns A second order cone.
56 ///
57 1 static SecondOrderCone RegularCone(const Scalar mu,
58 const VectorD& direction) {
59
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 assert(mu > 0 && "The friction coefficient must be non-negative");
60
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 MatrixD Q(MatrixD::Zero());
61
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Q.diagonal().fill(1. / mu);
62
63
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return SecondOrderCone(Q, direction);
64 }
65
66 template <typename S2, int O2>
67 ✗ bool operator==(const SecondOrderCone<S2, dim, O2>& other) const {
68 ✗ return m_Q == other.m_Q && m_direction == other.m_direction;
69 }
70
71 template <typename S2, int O2>
72 ✗ bool operator!=(const SecondOrderCone<S2, dim, O2>& other) const {
73 ✗ return !(*this == other);
74 }
75
76 /// \returns the value of lhs of the conic inequality
77 3 Scalar lhsValue(const VectorD& point) const {
78 // const VectorD x_Po(m_Po * point);
79
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 return (m_QPo * point).norm();
80 }
81
82 /// \returns the value of rhs of the conic inequality
83 3 Scalar rhsValue(const VectorD& point) const { return m_direction.dot(point); }
84
85 /// \returns true if the point is inside the cone
86 3 bool check(const VectorD& point) const { return check(point, 1.); }
87
88 3 bool check(const VectorD& point, const Scalar factor) const {
89 3 return lhsValue(point) <= factor * rhsValue(point);
90 }
91
92 /// \returns the direction of the cone.
93 2 const VectorD& direction() const { return m_direction; }
94 ✗ void setDirection(const VectorD& direction) {
95 ✗ assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
96 ✗ m_direction = direction.normalized();
97 ✗ computeProjectors();
98 }
99
100 template <typename S2, int O2>
101 ✗ bool isApprox(
102 const SecondOrderCone<S2, dim, O2>& other,
103 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
104 ✗ return m_direction.isApprox(other.m_direction, prec) &&
105 ✗ m_Q.isApprox(other.m_Q, prec);
106 }
107
108 /// \returns the quadratic term of the lhs norm.
109 2 const MatrixD& Q() const { return m_Q; }
110 ✗ void setQ(const MatrixD& Q) {
111 ✗ assert((Q - Q.transpose()).isMuchSmallerThan(Q));
112 ✗ m_Q = Q;
113 ✗ computeProjectors();
114 }
115
116 ✗ void disp(std::ostream& os) const {
117 ✗ os << "Q:\n"
118 ✗ << m_Q << std::endl
119 ✗ << "direction: " << m_direction.transpose() << std::endl;
120 }
121
122 ✗ friend std::ostream& operator<<(std::ostream& os, const SecondOrderCone& C) {
123 ✗ C.disp(os);
124 ✗ return os;
125 }
126
127 protected:
128 3 inline void computeProjectors() {
129
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 3 m_Pd = m_direction * m_direction.transpose();
130
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 3 m_Po = MatrixD::Identity() - m_Pd;
131
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 3 m_QPo.noalias() = m_Q * m_Po;
132 3 }
133
134 /// \brief Cholesky decomposition matrix reprensenting the conic norm
135 MatrixD m_Q;
136 /// \brief Cholesky decomposition projected on the orthogonal of m_direction
137 MatrixD m_QPo;
138
139 /// \brief Direction of the cone
140 VectorD m_direction;
141
142 /// \brief Projector along the direction of d
143 MatrixD m_Pd;
144 /// \brief Projector orthogonal to d
145 MatrixD m_Po;
146
147 private:
148 // Serialization of the class
149 friend class boost::serialization::access;
150
151 template <class Archive>
152 ✗ void save(Archive& ar, const unsigned int /*version*/) const {
153 ✗ ar& boost::serialization::make_nvp("quadratic_term", m_Q);
154 ✗ ar& boost::serialization::make_nvp("direction", m_direction);
155 }
156
157 template <class Archive>
158 ✗ void load(Archive& ar, const unsigned int /*version*/) {
159 ✗ ar >> boost::serialization::make_nvp("quadratic_term", m_Q);
160 ✗ ar >> boost::serialization::make_nvp("direction", m_direction);
161
162 ✗ computeProjectors();
163 }
164
165 ✗ BOOST_SERIALIZATION_SPLIT_MEMBER()
166 };
167
168 } // namespace geometry
169 } // namespace multicontact_api
170
171 #endif // ifndef __multicontact_api_geometry_second_order_cone_hpp__
172