convert-dg-to-py.cc 8.99 KB
Newer Older
1
2
3
4
5
6
7
8
9
// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.

#include <iostream>
#include <sstream>

#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-caster.h>

10
11
#include "dynamic-graph/python/convert-dg-to-py.hh"

12
13
namespace dynamicgraph {

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
14
15
16
17
using ::dynamicgraph::SignalBase;

namespace python {
namespace convert {
18

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
19
20
21
22
23
24
25
26
void fillMatrixRow(Matrix& m, unsigned iRow, PyObject* sequence) {
  if (PySequence_Size(sequence) != (int)m.cols()) {
    throw ExceptionPython(ExceptionPython::MATRIX_PARSING, "lines of matrix have different sizes.");
  }
  for (int iCol = 0; iCol < m.cols(); iCol++) {
    PyObject* pyDouble = PySequence_GetItem(sequence, iCol);
    if (PyFloat_Check(pyDouble))
      m(iRow, iCol) = PyFloat_AsDouble(pyDouble);
27
28
    else if (PyLong_Check(pyDouble))
      m(iRow, iCol) = (int)PyLong_AsLong(pyDouble) + 0.0;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
29
30
31
32
33
34
35
36
37
38
39
40
41
42
    else
      throw ExceptionPython(ExceptionPython::MATRIX_PARSING,
                            "element of matrix should be "
                            "a floating point number.");
  }
}
void fillMatrixRow(Eigen::Matrix4d& m, unsigned iRow, PyObject* sequence) {
  if (PySequence_Size(sequence) != (int)m.cols()) {
    throw ExceptionPython(ExceptionPython::MATRIX_PARSING, "lines of matrix have different sizes.");
  }
  for (int iCol = 0; iCol < m.cols(); iCol++) {
    PyObject* pyDouble = PySequence_GetItem(sequence, iCol);
    if (PyFloat_Check(pyDouble))
      m(iRow, iCol) = PyFloat_AsDouble(pyDouble);
43
44
    else if (PyLong_Check(pyDouble))
      m(iRow, iCol) = (int)PyLong_AsLong(pyDouble) + 0.0;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
45
46
47
48
49
50
    else
      throw ExceptionPython(ExceptionPython::MATRIX_PARSING,
                            "element of matrix should be "
                            "a floating point number.");
  }
}
51

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
command::Value pythonToValue(PyObject* pyObject, const command::Value::Type& valueType) {
  using command::Value;
  bool bvalue;
  unsigned uvalue;
  int ivalue;
  float fvalue;
  double dvalue;
  std::string svalue;
  Vector v;
  Matrix m;
  Eigen::Matrix4d m4;
  Py_ssize_t nCols;
  Py_ssize_t size;
  PyObject* row;
  Py_ssize_t nRows;
67

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
68
69
70
71
  switch (valueType) {
    case (Value::BOOL):
      if (!PyBool_Check(pyObject)) {
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "bool");
72
      }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
73
74
75
76
      bvalue = PyObject_IsTrue(pyObject);
      return Value(bvalue);
      break;
    case (Value::UNSIGNED):
77
      if (!PyLong_Check(pyObject)) {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
78
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "unsigned int");
79
      }
80
      uvalue = (unsigned int)PyLong_AsUnsignedLongMask(pyObject);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
81
82
83
      return Value(uvalue);
      break;
    case (Value::INT):
84
      if (!PyLong_Check(pyObject)) {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
85
86
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "int");
      }
87
      ivalue = (int)PyLong_AsLong(pyObject);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
88
89
90
91
92
93
      return Value(ivalue);
      break;
    case (Value::FLOAT):
      if (PyFloat_Check(pyObject)) {
        fvalue = (float)PyFloat_AsDouble(pyObject);
        return Value(fvalue);
94
95
      } else if (PyLong_Check(pyObject)) {
        fvalue = (float)PyLong_AsLong(pyObject);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
96
97
98
99
100
101
102
103
104
        return Value(fvalue);
      } else {
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "float");
      }
      break;
    case (Value::DOUBLE):
      if (PyFloat_Check(pyObject)) {
        dvalue = PyFloat_AsDouble(pyObject);
        return Value(dvalue);
105
106
      } else if (PyLong_Check(pyObject)) {
        dvalue = (double)PyLong_AsLong(pyObject);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
107
108
109
110
111
112
        return Value(dvalue);
      } else {
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "double");
      }
      break;
    case (Value::STRING):
113
      if (!PyUnicode_Check(pyObject)) {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
114
115
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "string");
      }
116
      svalue = PyUnicode_AS_DATA(pyObject);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
117
118
119
120
121
122
123
124
125
126
127
128
129
      return Value(svalue);
      break;
    case (Value::VECTOR):
      // Check that argument is a tuple
      if (!PySequence_Check(pyObject)) {
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "vector");
      }
      size = PySequence_Size(pyObject);
      v.resize(size);
      for (Py_ssize_t i = 0; i < size; i++) {
        PyObject* pyDouble = PySequence_GetItem(pyObject, i);
        if (PyFloat_Check(pyDouble))
          v(i) = PyFloat_AsDouble(pyDouble);
130
131
        else if (PyLong_Check(pyDouble))
          v(i) = (int)PyLong_AsLong(pyDouble) + 0.0;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
        else
          throw ExceptionPython(ExceptionPython::VECTOR_PARSING,
                                "element of vector should be a floating "
                                "point number.");
      }
      return Value(v);
      break;
    case (Value::MATRIX):
      // Check that argument is a tuple
      if (!PySequence_Check(pyObject)) {
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "matrix");
      }
      nRows = PySequence_Size(pyObject);
      if (nRows == 0) {
        return Value(Matrix());
      }
      row = PySequence_GetItem(pyObject, 0);
      if (!PySequence_Check(row)) {
        throw ExceptionPython(ExceptionPython::MATRIX_PARSING, "matrix");
      }
      nCols = PySequence_Size(row);
153

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
154
155
      m.resize((unsigned int)nRows, (unsigned int)nCols);
      fillMatrixRow(m, 0, row);
156

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
      for (Py_ssize_t iRow = 1; iRow < nRows; iRow++) {
        row = PySequence_GetItem(pyObject, iRow);
        if (!PySequence_Check(row)) {
          throw ExceptionPython(ExceptionPython::MATRIX_PARSING, "matrix");
        }
        fillMatrixRow(m, static_cast<unsigned>(iRow), row);
      }
      return Value(m);
      break;
    case (Value::MATRIX4D):
      // Check that argument is a tuple
      if (!PySequence_Check(pyObject)) {
        throw ExceptionPython(ExceptionPython::VALUE_PARSING, "matrix4d");
      }
      nRows = PySequence_Size(pyObject);
      if (nRows == 0) {
        return Value(Eigen::Matrix4d());
      }
      row = PySequence_GetItem(pyObject, 0);
      if (!PySequence_Check(row)) {
        throw ExceptionPython(ExceptionPython::MATRIX_PARSING, "matrix4d");
      }
      nCols = PySequence_Size(row);
180

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
181
182
      m4.resize(nRows, nCols);
      fillMatrixRow(m4, 0, row);
183

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
184
185
186
187
188
189
      for (Py_ssize_t iRow = 1; iRow < nRows; iRow++) {
        row = PySequence_GetItem(pyObject, iRow);
        if (!PySequence_Check(row)) {
          throw ExceptionPython(ExceptionPython::MATRIX_PARSING, "matrix");
        }
        fillMatrixRow(m4, static_cast<unsigned>(iRow), row);
190
      }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
191
192
193
194
195
196
197
      return Value(m4);
      break;
    default:
      std::cerr << "Only int, double and string are supported." << std::endl;
  }
  return Value();
}
198

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
199
200
201
202
203
204
205
206
PyObject* vectorToPython(const Vector& vector) {
  PyObject* tuple = PyTuple_New(vector.size());
  for (int index = 0; index < vector.size(); index++) {
    PyObject* pyDouble = PyFloat_FromDouble(vector(index));
    PyTuple_SET_ITEM(tuple, index, pyDouble);
  }
  return tuple;
}
207

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
208
209
210
211
212
213
214
215
216
217
218
219
PyObject* matrixToPython(const Matrix& matrix) {
  PyObject* tuple = PyTuple_New(matrix.rows());
  for (int iRow = 0; iRow < matrix.rows(); iRow++) {
    PyObject* row = PyTuple_New(matrix.cols());
    for (int iCol = 0; iCol < matrix.cols(); iCol++) {
      PyObject* pyDouble = PyFloat_FromDouble(matrix(iRow, iCol));
      PyTuple_SET_ITEM(row, iCol, pyDouble);
    }
    PyTuple_SET_ITEM(tuple, iRow, row);
  }
  return tuple;
}
220

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
221
222
223
224
225
226
227
228
229
230
231
232
PyObject* matrix4dToPython(const Eigen::Matrix4d& matrix) {
  PyObject* tuple = PyTuple_New(matrix.rows());
  for (int iRow = 0; iRow < matrix.rows(); iRow++) {
    PyObject* row = PyTuple_New(matrix.cols());
    for (int iCol = 0; iCol < matrix.cols(); iCol++) {
      PyObject* pyDouble = PyFloat_FromDouble(matrix(iRow, iCol));
      PyTuple_SET_ITEM(row, iCol, pyDouble);
    }
    PyTuple_SET_ITEM(tuple, iRow, row);
  }
  return tuple;
}
233

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
PyObject* valueToPython(const command::Value& value) {
  using command::Value;
  bool boolValue;
  unsigned unsignedValue;
  int intValue;
  float floatValue;
  double doubleValue;
  std::string stringValue;
  Vector vectorValue;
  Matrix matrixValue;
  Eigen::Matrix4d matrix4dValue;
  switch (value.type()) {
    case (Value::BOOL):
      boolValue = value.value();
      if (boolValue) {
        return PyBool_FromLong(1);
250
      }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
      return PyBool_FromLong(0);
    case (Value::UNSIGNED):
      unsignedValue = value.value();
      return Py_BuildValue("I", unsignedValue);
    case (Value::INT):
      intValue = value.value();
      return Py_BuildValue("i", intValue);
    case (Value::FLOAT):
      floatValue = value.value();
      return Py_BuildValue("f", floatValue);
    case (Value::DOUBLE):
      doubleValue = value.value();
      return Py_BuildValue("d", doubleValue);
    case (Value::STRING):
      stringValue = (std::string)value.value();
      return Py_BuildValue("s", stringValue.c_str());
    case (Value::VECTOR):
      vectorValue = value.value();
      return vectorToPython(vectorValue);
    case (Value::MATRIX):
      matrixValue = value.value();
      return matrixToPython(matrixValue);
    case (Value::MATRIX4D):
      matrix4dValue = value.value();
      return matrix4dToPython(matrix4dValue);
    default:
      return Py_BuildValue("");
  }
  return Py_BuildValue("");
}
281

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
282
283
284
}  // namespace convert
}  // namespace python
}  // namespace dynamicgraph