hpp-pinocchio  4.14.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
util.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017-2018 CNRS
3 // Author: Joseph Mirabel
4 //
5 //
6 
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30 
31 #ifndef HPP_PINOCCHIO_UTIL_HH
32 #define HPP_PINOCCHIO_UTIL_HH
33 
34 #include <hpp/pinocchio/config.hh>
35 #include <hpp/pinocchio/fwd.hh>
36 #include <hpp/util/indent.hh>
37 
38 namespace hpp {
75 
77 template <typename T, int Option>
79  static std::ostream& run(std::ostream& os, const T& pp);
80 };
81 
83 enum {
85 
88  PrettyOutput = 2
89 };
90 
92 HPP_PINOCCHIO_DLLAPI long& getpythonformat(std::ostream& o);
93 
94 template <bool OneLine, bool PythonStyle, bool Vector>
95 struct HPP_PINOCCHIO_DLLAPI eigen_format {
96  static const Eigen::IOFormat run();
97 };
98 
99 // Default implementation
100 template <bool OneLine, bool PythonStyle, bool Vector>
101 const Eigen::IOFormat eigen_format<OneLine, PythonStyle, Vector>::run() {
102  static const Eigen::IOFormat fmt(
103  (PythonStyle ? Eigen::FullPrecision : Eigen::StreamPrecision), 0,
104  ", ", // Coeff separator
105  (PythonStyle // Row separator
106  ? (OneLine ? ", " : ",\n")
107  : (OneLine ? "; " : "\n")),
108  (PythonStyle ? "(" : ""), // row prefix
109  (PythonStyle ? ",)" : ""), // row suffix
110  (PythonStyle && !Vector ? "( " : ""), // mat prefix
111  (PythonStyle && !Vector ? ", )" : "") // mat suffix
112  );
113  return fmt;
114 }
115 
116 template <typename T, int Option>
117 struct PrettyPrint {
118  const T& value;
119  inline explicit PrettyPrint(const T& t) : value(t) {}
120 };
121 
122 template <typename T, int Option>
123 std::ostream& operator<<(std::ostream& os, const PrettyPrint<T, Option> pp) {
124  return prettyPrint<T, Option>::run(os, pp.value);
125 }
126 
128 template <typename Derived, int Option>
129 struct HPP_PINOCCHIO_DLLAPI prettyPrintEigen {
130  static inline std::ostream& run(std::ostream& os, const Derived& M) {
131  enum {
132  Condensed = ((Option & OutputFormatBits) == OneLineOutput) ||
133  ((Option & OutputFormatBits) == CondensedOutput)
134  };
135  static const Eigen::IOFormat mfmt_py =
136  eigen_format<Condensed, true, false>::run();
137  static const Eigen::IOFormat vfmt_py =
138  eigen_format<Condensed, true, true>::run();
139  static const Eigen::IOFormat mfmt_raw =
140  eigen_format<Condensed, false, false>::run();
141  static const Eigen::IOFormat vfmt_raw =
142  eigen_format<Condensed, false, true>::run();
143  bool use_py_fmt = (getpythonformat(os) != 0);
144  const Eigen::IOFormat& fmt =
145  (Derived::IsVectorAtCompileTime ? (use_py_fmt ? vfmt_py : vfmt_raw)
146  : (use_py_fmt ? mfmt_py : mfmt_raw));
147  bool transpose = (Derived::ColsAtCompileTime == 1);
148 
149  if (transpose)
150  return os << M.transpose().format(fmt);
151  else
152  return os << M.format(fmt);
153  }
154 };
156 
158 template <typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows,
159  int _MaxCols, int Option>
160 struct HPP_PINOCCHIO_DLLAPI prettyPrint<
161  Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, Option>
162  : prettyPrintEigen<
163  Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>,
164  Option> {};
165 
167 template <typename OtherDerived, int Size, int Option>
169  prettyPrint<Eigen::VectorBlock<OtherDerived, Size>, Option>
170  : prettyPrintEigen<Eigen::VectorBlock<OtherDerived, Size>, Option> {};
171 
173 template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel,
174  int Option>
176  prettyPrint<Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>, Option>
177  : prettyPrintEigen<Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>,
178  Option> {};
179 
181 template <typename _PlainObjectType, int _Options, typename _StrideType,
182  int Option>
184  prettyPrint<Eigen::Ref<_PlainObjectType, _Options, _StrideType>, Option>
185  : prettyPrintEigen<Eigen::Ref<_PlainObjectType, _Options, _StrideType>,
186  Option> {};
187 
189 template <typename _Scalar, int _Options, int Option>
191  prettyPrint<Eigen::Quaternion<_Scalar, _Options>, Option> {
192  typedef Eigen::Quaternion<_Scalar, _Options> Derived;
193  typedef typename Eigen::internal::traits<Derived>::Coefficients Coefficients;
194  static inline std::ostream& run(std::ostream& os, const Derived& M) {
195  return prettyPrint<Coefficients, Option>::run(os, M.coeffs());
196  }
197 };
199 
200 // Set python formatting of vector and matrices
201 HPP_PINOCCHIO_DLLAPI std::ostream& setpyformat(std::ostream& o);
202 
203 // Unset python formatting of vector and matrices
204 HPP_PINOCCHIO_DLLAPI std::ostream& unsetpyformat(std::ostream& o);
205 
207 template <typename T>
208 inline PrettyPrint<T, PrettyOutput> pretty_print(const T& t) {
209  return PrettyPrint<T, PrettyOutput>(t);
210 }
212 template <typename T>
213 inline PrettyPrint<T, CondensedOutput> condensed(const T& t) {
214  return PrettyPrint<T, CondensedOutput>(t);
215 }
217 template <typename T>
218 inline PrettyPrint<T, OneLineOutput> one_line(const T& t) {
219  return PrettyPrint<T, OneLineOutput>(t);
220 }
221 
223 } // namespace hpp
224 #endif // HPP_PINOCCHIO_UTIL_HH
#define HPP_PINOCCHIO_DLLAPI
Definition: config.hh:64
PrettyPrint< T, PrettyOutput > pretty_print(const T &t)
Pretty printing.
Definition: util.hh:208
PrettyPrint< T, CondensedOutput > condensed(const T &t)
Condensed printing.
Definition: util.hh:213
HPP_PINOCCHIO_DLLAPI std::ostream & unsetpyformat(std::ostream &o)
PrettyPrint< T, OneLineOutput > one_line(const T &t)
Print on one line.
Definition: util.hh:218
HPP_PINOCCHIO_DLLAPI std::ostream & setpyformat(std::ostream &o)
@ PrettyOutput
Definition: util.hh:88
@ CondensedOutput
Definition: util.hh:87
@ OutputFormatBits
Definition: util.hh:84
@ OneLineOutput
Definition: util.hh:86
std::ostream & operator<<(std::ostream &os, const hpp::pinocchio::Device &device)
Definition: device.hh:368
Utility functions.
Definition: body.hh:39
This function must be specialized for the type you want to print.
Definition: util.hh:78
static std::ostream & run(std::ostream &os, const T &pp)