second-order-cone.hpp
Go to the documentation of this file.
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 
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 
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.;
35  }
36 
38  : m_Q(Q),
39  m_QPo(_dim, _dim),
40  m_direction(direction.normalized()),
41  m_Pd(_dim, _dim),
42  m_Po(_dim, _dim) {
43  assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
44  assert((Q - Q.transpose()).isMuchSmallerThan(Q));
46  }
47 
58  const VectorD& direction) {
59  assert(mu > 0 && "The friction coefficient must be non-negative");
60  MatrixD Q(MatrixD::Zero());
61  Q.diagonal().fill(1. / mu);
62 
63  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 
77  Scalar lhsValue(const VectorD& point) const {
78  // const VectorD x_Po(m_Po * point);
79  return (m_QPo * point).norm();
80  }
81 
83  Scalar rhsValue(const VectorD& point) const { return m_direction.dot(point); }
84 
86  bool check(const VectorD& point) const { return check(point, 1.); }
87 
88  bool check(const VectorD& point, const Scalar factor) const {
89  return lhsValue(point) <= factor * rhsValue(point);
90  }
91 
93  const VectorD& direction() const { return m_direction; }
95  assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
96  m_direction = direction.normalized();
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 
109  const MatrixD& Q() const { return m_Q; }
110  void setQ(const MatrixD& Q) {
111  assert((Q - Q.transpose()).isMuchSmallerThan(Q));
112  m_Q = Q;
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  inline void computeProjectors() {
129  m_Pd = m_direction * m_direction.transpose();
130  m_Po = MatrixD::Identity() - m_Pd;
131  m_QPo.noalias() = m_Q * m_Po;
132  }
133 
138 
141 
146 
147  private:
148  // Serialization of the class
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 
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__
multicontact_api::geometry::SecondOrderCone::isApprox
bool isApprox(const SecondOrderCone< S2, dim, O2 > &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: second-order-cone.hpp:101
multicontact_api::geometry::SecondOrderCone::check
bool check(const VectorD &point) const
Definition: second-order-cone.hpp:86
multicontact_api::geometry::SecondOrderCone::direction
const VectorD & direction() const
Definition: second-order-cone.hpp:93
multicontact_api::geometry::SecondOrderCone::disp
void disp(std::ostream &os) const
Definition: second-order-cone.hpp:116
multicontact_api::geometry::SecondOrderCone::lhsValue
Scalar lhsValue(const VectorD &point) const
Definition: second-order-cone.hpp:77
multicontact_api::geometry::SecondOrderCone::dim
@ dim
Definition: second-order-cone.hpp:21
eigen-matrix.hpp
archive.hpp
multicontact_api::geometry::SecondOrderCone::SecondOrderCone
SecondOrderCone()
Definition: second-order-cone.hpp:27
multicontact_api::geometry::SecondOrderCone::m_Po
MatrixD m_Po
Projector orthogonal to d.
Definition: second-order-cone.hpp:145
multicontact_api::geometry::SecondOrderCone::rhsValue
Scalar rhsValue(const VectorD &point) const
Definition: second-order-cone.hpp:83
multicontact_api
Definition: ellipsoid.hpp:12
multicontact_api::geometry::SecondOrderCone::m_Q
MatrixD m_Q
Cholesky decomposition matrix reprensenting the conic norm.
Definition: second-order-cone.hpp:135
multicontact_api::geometry::SecondOrderCone::check
bool check(const VectorD &point, const Scalar factor) const
Definition: second-order-cone.hpp:88
multicontact_api::geometry::SecondOrderCone::Scalar
_Scalar Scalar
Definition: second-order-cone.hpp:22
multicontact_api::geometry::SecondOrderCone::m_Pd
MatrixD m_Pd
Projector along the direction of d.
Definition: second-order-cone.hpp:143
multicontact_api::geometry::SecondOrderCone::SecondOrderCone
SecondOrderCone(const MatrixD &Q, const VectorD &direction)
Definition: second-order-cone.hpp:37
multicontact_api::geometry::SecondOrderCone::operator<<
friend std::ostream & operator<<(std::ostream &os, const SecondOrderCone &C)
Definition: second-order-cone.hpp:122
multicontact_api::geometry::SecondOrderCone::RegularCone
static SecondOrderCone RegularCone(const Scalar mu, const VectorD &direction)
Build a regular cone from a given friction coefficient and a direction.
Definition: second-order-cone.hpp:57
multicontact_api::geometry::SecondOrderCone::Options
@ Options
Definition: second-order-cone.hpp:21
boost::serialization::save
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16
multicontact_api::geometry::SecondOrderCone::VectorD
Eigen::Matrix< Scalar, dim, 1, Options > VectorD
Definition: second-order-cone.hpp:24
multicontact_api::geometry::SecondOrderCone::operator==
bool operator==(const SecondOrderCone< S2, dim, O2 > &other) const
Definition: second-order-cone.hpp:67
fwd.hpp
multicontact_api::geometry::SecondOrderCone::computeProjectors
void computeProjectors()
Definition: second-order-cone.hpp:128
boost::serialization::load
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
multicontact_api::geometry::SecondOrderCone::DenseIndex
Eigen::DenseIndex DenseIndex
Definition: second-order-cone.hpp:25
multicontact_api::geometry::SecondOrderCone::access
friend class boost::serialization::access
Definition: second-order-cone.hpp:149
multicontact_api::geometry::SecondOrderCone::Q
const MatrixD & Q() const
Definition: second-order-cone.hpp:109
multicontact_api::geometry::SecondOrderCone::setDirection
void setDirection(const VectorD &direction)
Definition: second-order-cone.hpp:94
multicontact_api::geometry::SecondOrderCone::setQ
void setQ(const MatrixD &Q)
Definition: second-order-cone.hpp:110
multicontact_api::geometry::SecondOrderCone::operator!=
bool operator!=(const SecondOrderCone< S2, dim, O2 > &other) const
Definition: second-order-cone.hpp:72
multicontact_api::geometry::SecondOrderCone
Definition: fwd.hpp:29
multicontact_api::geometry::SecondOrderCone::m_QPo
MatrixD m_QPo
Cholesky decomposition projected on the orthogonal of m_direction.
Definition: second-order-cone.hpp:137
multicontact_api::geometry::SecondOrderCone::m_direction
VectorD m_direction
Direction of the cone.
Definition: second-order-cone.hpp:140
multicontact_api::geometry::SecondOrderCone::MatrixD
Eigen::Matrix< Scalar, dim, dim, Options > MatrixD
Definition: second-order-cone.hpp:23