30 #ifndef EXOTICA_CORE_CONVERSIONS_H_
31 #define EXOTICA_CORE_CONVERSIONS_H_
37 #include <kdl/frames.hpp>
38 #include <kdl/jacobian.hpp>
40 #include <Eigen/Dense>
41 #pragma GCC diagnostic push
42 #pragma GCC diagnostic ignored "-Wfloat-conversion"
43 #include <unsupported/Eigen/CXX11/Tensor>
44 #pragma GCC diagnostic pop
57 Eigen::VectorXd
VectorTransform(
double px = 0.0,
double py = 0.0,
double pz = 0.0,
double qx = 0.0,
double qy = 0.0,
double qz = 0.0,
double qw = 1.0);
61 using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
62 template <
typename Scalar,
int rank,
typename sizeType>
65 return Eigen::Map<const MatrixType<Scalar>>(tensor.data(), rows, cols);
68 template <
typename Scalar,
typename... Dims>
71 constexpr
int rank =
sizeof...(Dims);
72 return Eigen::TensorMap<Eigen::Tensor<const Scalar, rank>>(matrix.data(), {dims...});
95 static constexpr
int types[] = {4, 3, 3, 3, 3, 9};
96 return types[
static_cast<int>(type)];
101 if (rotation_type ==
"Quaternion")
105 else if (rotation_type ==
"RPY")
109 else if (rotation_type ==
"ZYX")
113 else if (rotation_type ==
"ZYZ")
117 else if (rotation_type ==
"AngleAxis")
121 else if (rotation_type ==
"Matrix")
127 ThrowPretty(
"Unsupported rotation type '" << rotation_type <<
"'");
135 Eigen::MatrixXd
GetFrame(
const KDL::Frame& val);
143 q.segment<4>(3) = Eigen::Quaterniond(q.segment<4>(3)).normalized().coeffs();
148 q.segment<4>(3) = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0).normalized().coeffs();
151 typedef Eigen::Array<KDL::Frame, Eigen::Dynamic, 1>
ArrayFrame;
152 typedef Eigen::Array<KDL::Twist, Eigen::Dynamic, 1>
ArrayTwist;
154 typedef Eigen::Array<Eigen::MatrixXd, Eigen::Dynamic, 1>
Hessian;
155 typedef Eigen::Array<Eigen::Array<Eigen::MatrixXd, Eigen::Dynamic, 1>, Eigen::Dynamic, 1>
ArrayHessian;
171 return type ==
"exotica::Initializer";
176 return type.substr(0, 11) ==
"std::vector";
181 return type ==
"std::vector<exotica::Initializer>";
184 template <
class Key,
class Val>
185 [[deprecated(
"Replaced by GetKeysFromMap and GetValuesFromMap")]] std::vector<Val>
MapToVec(
const std::map<Key, Val>& map) {
186 std::vector<Val> ret;
189 ret.push_back(it.second);
194 template <
class Key,
class Val>
197 std::vector<Key> ret;
200 ret.push_back(it.first);
205 template <
class Key,
class Val>
208 std::vector<Val> ret;
211 ret.push_back(it.second);
216 template <
class Key,
class Val>
217 void AppendMap(std::map<Key, Val>& orig,
const std::map<Key, Val>& extra)
219 orig.insert(extra.begin(), extra.end());
223 void AppendVector(std::vector<Val>& orig,
const std::vector<Val>& extra)
225 orig.insert(orig.end(), extra.begin(), extra.end());
228 inline std::string
Trim(
const std::string& s)
230 auto wsfront = std::find_if_not(
s.begin(),
s.end(), [](
int c) { return std::isspace(c); });
231 return std::string(wsfront, std::find_if_not(
s.rbegin(), std::string::const_reverse_iterator(wsfront), [](
int c) { return std::isspace(c); }).base());
234 template <
typename T>
237 throw std::runtime_error(
"conversion not implemented!");
243 return std::stof(val);
249 return std::stod(val);
255 return std::stoi(val);
258 template <
typename T, const
int S>
259 inline Eigen::Matrix<T, S, 1>
ParseVector(
const std::string value)
261 Eigen::Matrix<T, S, 1> ret;
262 std::string temp_entry;
265 std::istringstream text_parser(value);
267 while (text_parser >> temp_entry)
269 ret.conservativeResize(++i);
272 ret[i - 1] = ToNumber<T>(temp_entry);
274 catch (
const std::invalid_argument& )
276 ret[i - 1] = std::numeric_limits<T>::quiet_NaN();
280 if (S != Eigen::Dynamic && S != i)
282 ThrowPretty(
"Wrong vector size! Requested: " + std::to_string(S) +
", Provided: " + std::to_string(i));
290 std::istringstream text_parser(value);
298 std::istringstream text_parser(value);
301 if ((text_parser.fail() || text_parser.bad()))
311 std::istringstream text_parser(value);
314 if ((text_parser.fail() || text_parser.bad()))
321 inline std::vector<std::string>
ParseList(
const std::string& value,
char token =
',')
323 std::stringstream ss(value);
325 std::vector<std::string> ret;
326 while (std::getline(ss, item, token))
328 ret.push_back(
Trim(item));
330 if (ret.size() == 0)
WARNING_NAMED(
"Parser",
"Empty vector!")
336 std::stringstream ss(value);
338 std::vector<int> ret;
339 while (std::getline(ss, item,
' '))
342 std::istringstream text_parser(item);
344 if ((text_parser.fail() || text_parser.bad()))
350 if (ret.size() == 0)
WARNING_NAMED(
"Parser",
"Empty vector!")
356 std::stringstream ss(value);
358 std::vector<bool> ret;
359 while (std::getline(ss, item,
' '))
362 std::istringstream text_parser(item);
364 if ((text_parser.fail() || text_parser.bad()))
375 #endif // EXOTICA_CORE_CONVERSIONS_H_