matrix_parser.cpp
Go to the documentation of this file.
2 
4 {
7 
8 bool MatrixParser::parseMatrixData(Eigen::MatrixXd &M,
9  const std::string param_name,
10  const ros::NodeHandle &n)
11 {
12  std::vector<double> vals;
13  int rows, cols;
14  if (n.hasParam(param_name.c_str()))
15  {
16  if (n.hasParam((param_name + std::string("/data").c_str())))
17  {
18  n.getParam((param_name + std::string("/data")).c_str(), vals);
19 
20  if (n.hasParam((param_name + std::string("/rows").c_str())) &&
21  n.hasParam((param_name + std::string("/cols").c_str())))
22  {
23  n.getParam((param_name + std::string("/rows")).c_str(), rows);
24  n.getParam((param_name + std::string("/cols")).c_str(), cols);
25 
26  initializeEigenMatrix(M, vals, rows, cols);
27  }
28  else
29  {
30  ROS_WARN_STREAM("MatrixParser: Assuming square matrix for "
31  << param_name
32  << ". You can set dimensions by setting the parameter "
33  "/rows and /cols");
34  initializeEigenMatrix(M, vals);
35  }
36  }
37  else
38  {
39  ROS_ERROR_STREAM("MatrixParser: Matrix definition "
40  << param_name << " has no data values (" << param_name
41  << "/data)! Shutting down...");
42  return false;
43  }
44  }
45  else
46  {
47  ROS_WARN_STREAM("MatrixParser: Configuration name " << param_name
48  << " does not exist");
49  return false;
50  }
51 
52  return true;
53 } // namespace generic_control_toolbox
54 
55 void MatrixParser::initializeEigenMatrix(Eigen::MatrixXd &M,
56  const std::vector<double> &vals)
57 {
58  double size_f, frac_part, discard;
59  int size;
60 
61  size_f = std::sqrt(vals.size());
62  frac_part = std::modf(size_f, &discard);
63 
64  size = (int)size_f;
65  if (frac_part != 0.0)
66  {
67  std::stringstream errMsg;
68  errMsg << "MatrixParser: Tried to initialize a square matrix with a "
69  "non-square (got: "
70  << vals.size() << ") number of values";
71  throw std::logic_error(errMsg.str().c_str());
72  }
73 
74  ROS_DEBUG("MatrixParser: filling matrix");
75  initializeEigenMatrix(M, vals, size, size);
76 }
77 
78 void MatrixParser::initializeEigenMatrix(Eigen::MatrixXd &M,
79  const std::vector<double> &vals,
80  int rows, int cols)
81 {
82  if (rows <= 0 || cols <= 0)
83  {
84  std::stringstream errMsg;
85  errMsg << "MatrixParser: Tried to initialize a matrix with dims " << rows
86  << "x" << cols;
87  throw std::logic_error(errMsg.str().c_str());
88  }
89 
90  ROS_DEBUG("MatrixParser: filling matrix");
91 
92  M = Eigen::MatrixXd(rows, cols);
93 
94  int idx = 0;
95  for (unsigned int i = 0; i < rows; i++)
96  {
97  for (unsigned int j = 0; j < cols; j++)
98  {
99  M(i, j) = vals[idx++];
100  }
101  }
102 }
103 
104 Eigen::Matrix3d MatrixParser::computeSkewSymmetric(const Eigen::Vector3d &v)
105 {
106  Eigen::Matrix3d S;
107 
108  S << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
109 
110  return S;
111 }
112 } // namespace generic_control_toolbox
static Eigen::Matrix3d computeSkewSymmetric(const Eigen::Vector3d &v)
#define ROS_DEBUG(...)
static void initializeEigenMatrix(Eigen::MatrixXd &M, const std::vector< double > &vals)
bool getParam(const std::string &key, std::string &s) const
static bool parseMatrixData(Eigen::MatrixXd &M, const std::string param_name, const ros::NodeHandle &n)
#define ROS_WARN_STREAM(args)
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)


generic_control_toolbox
Author(s): diogo
autogenerated on Mon Feb 28 2022 22:24:37