4 #ifndef __multicontact_api_geometry_second_order_cone_hpp__ 5 #define __multicontact_api_geometry_second_order_cone_hpp__ 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
23 typedef Eigen::Matrix<Scalar, dim, dim, Options>
MatrixD;
24 typedef Eigen::Matrix<Scalar, dim, 1, Options>
VectorD;
28 :
m_Q(MatrixD::Identity()),
43 assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
44 assert((Q - Q.transpose()).isMuchSmallerThan(Q));
59 assert(mu > 0 &&
"The friction coefficient must be non-negative");
60 MatrixD
Q(MatrixD::Zero());
61 Q.diagonal().fill(1. / mu);
66 template <
typename S2,
int O2>
71 template <
typename S2,
int O2>
73 return !(*
this == other);
79 return (
m_QPo * point).norm();
86 bool check(
const VectorD& point)
const {
return check(point, 1.); }
88 bool check(
const VectorD& point,
const Scalar factor)
const {
95 assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
100 template <
typename S2,
int O2>
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);
109 const MatrixD&
Q()
const {
return m_Q; }
111 assert((Q - Q.transpose()).isMuchSmallerThan(Q));
116 void disp(std::ostream& os)
const {
119 <<
"direction: " <<
m_direction.transpose() << std::endl;
151 template <
class Archive>
152 void save(Archive& ar,
const unsigned int )
const {
153 ar& boost::serialization::make_nvp(
"quadratic_term", m_Q);
154 ar& boost::serialization::make_nvp(
"direction", m_direction);
157 template <
class Archive>
158 void load(Archive& ar,
const unsigned int ) {
159 ar >> boost::serialization::make_nvp(
"quadratic_term", m_Q);
160 ar >> boost::serialization::make_nvp(
"direction", m_direction);
165 BOOST_SERIALIZATION_SPLIT_MEMBER()
171 #endif // ifndef __multicontact_api_geometry_second_order_cone_hpp__
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16