signal-base-py.cc 13 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.
2
3
4

#include <Python.h>
#include <iostream>
florent's avatar
florent committed
5
#include <sstream>
6
7

#include <dynamic-graph/signal-base.h>
8
#include <dynamic-graph/signal.h>
9
#include <dynamic-graph/signal-ptr.h>
10
#include <dynamic-graph/signal-caster.h>
11
#include <dynamic-graph/linear-algebra.h>
12
13
#include <dynamic-graph/pool.h>
#include <dynamic-graph/factory.h>
14

15
16
17
18
#include "dynamic-graph/python/dynamic-graph-py.hh"
#include "dynamic-graph/python/convert-dg-to-py.hh"
#include "dynamic-graph/python/exception.hh"
#include "dynamic-graph/python/signal-wrapper.hh"
19

20
21
22
using dynamicgraph::SignalBase;

namespace dynamicgraph {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
23
24
25
26
27
28
namespace python {

using namespace convert;

namespace signalBase {

29
static void destroy(PyObject* self);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
30
31
32
33
34
35
36
37
38
39
40
41
42

/**
   \brief Create an instance of SignalBase
*/
PyObject* create(PyObject* /*self*/, PyObject* args) {
  char* name = NULL;

  if (!PyArg_ParseTuple(args, "s", &name)) return NULL;

  SignalBase<int>* obj = NULL;
  obj = new SignalBase<int>(std::string(name));

  // Return the pointer
43
  return PyCapsule_New((void*)obj, "dynamic_graph.Signal", destroy);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
}

template <class T>
SignalWrapper<T, int>* createSignalWrapperTpl(const char* name, PyObject* o, std::string& error) {
  typedef SignalWrapper<T, int> SignalWrapper_t;
  if (!SignalWrapper_t::checkCallable(o, error)) {
    return NULL;
  }

  SignalWrapper_t* obj = new SignalWrapper_t(name, o);
  return obj;
}

PythonSignalContainer* getPythonSignalContainer() {
  const std::string instanceName = "python_signals";
  const std::string className = "PythonSignalContainer";
  Entity* obj;
  if (PoolStorage::getInstance()->existEntity(instanceName, obj)) {
    if (obj->getClassName() != className) {
      std::string msg("Found an object named " + std::string(instanceName) +
                      ",\n"
                      "but this object is of type " +
                      std::string(obj->getClassName()) + " and not " + std::string(className));
Guilhem Saurel's avatar
Guilhem Saurel committed
67
      PyErr_SetString(DGPYERROR, msg.c_str());
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
      return NULL;
    }
  } else {
    try {
      obj = FactoryStorage::getInstance()->newEntity(std::string(className), std::string(instanceName));
    }
    CATCH_ALL_EXCEPTIONS();
  }
  return dynamic_cast<PythonSignalContainer*>(obj);
}

#define SIGNAL_WRAPPER_TYPE(IF, Enum, Type)                               \
  IF(command::Value::typeName(command::Value::Enum).compare(type) == 0) { \
    obj = createSignalWrapperTpl<Type>(name, object, error);              \
  }

/**
   \brief Create an instance of SignalWrapper
*/
PyObject* createSignalWrapper(PyObject* /*self*/, PyObject* args) {
  PythonSignalContainer* psc = getPythonSignalContainer();
  if (psc == NULL) return NULL;

  char* name = NULL;
  char* type = NULL;
  PyObject* object = NULL;

  if (!PyArg_ParseTuple(args, "ssO", &name, &type, &object)) return NULL;

  SignalBase<int>* obj = NULL;
  std::string error;
  SIGNAL_WRAPPER_TYPE(if, BOOL, bool)
  // SIGNAL_WRAPPER_TYPE(else if, UNSIGNED ,bool)
  SIGNAL_WRAPPER_TYPE(else if, INT, int)
  SIGNAL_WRAPPER_TYPE(else if, FLOAT, float)
  SIGNAL_WRAPPER_TYPE(else if, DOUBLE, double)
  // SIGNAL_WRAPPER_TYPE(else if, STRING   ,bool)
  SIGNAL_WRAPPER_TYPE(else if, VECTOR, Vector)
  // SIGNAL_WRAPPER_TYPE(else if, MATRIX   ,bool)
  // SIGNAL_WRAPPER_TYPE(else if, MATRIX4D ,bool)
  else {
    error = "Type not understood";
  }

  if (obj == NULL) {
Guilhem Saurel's avatar
Guilhem Saurel committed
113
    PyErr_SetString(DGPYERROR, error.c_str());
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
114
115
116
117
118
119
    return NULL;
  }
  // Register signal into the python signal container
  psc->signalRegistration(*obj);

  // Return the pointer
120
  return PyCapsule_New((void*)obj, "dynamic_graph.SignalWrapper", destroy);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
121
122
123
124
125
}

/**
   \brief Destroy an instance of InvertedPendulum
*/
126
static void destroy(PyObject* self) {
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
127
128
129
130
131
132
133
134
  SignalBase<int>* obj = (SignalBase<int>*)self;
  delete obj;
}

PyObject* getTime(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
135
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
136

137
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
138
139
140
141
142
143
144
145
146
147
148
  SignalBase<int>* obj = (SignalBase<int>*)pointer;

  int time = obj->getTime();
  return Py_BuildValue("i", time);
}

PyObject* setTime(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  int time;
  if (!PyArg_ParseTuple(args, "Oi", &object, &time)) return NULL;
149
  if (!PyCapsule_CheckExact(object)) {
Guilhem Saurel's avatar
Guilhem Saurel committed
150
    PyErr_SetString(DGPYERROR, "object should be a C object");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
151
152
153
    return NULL;
  }

154
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
155
156
157
158
159
160
161
162
163
164
  SignalBase<int>* obj = (SignalBase<int>*)pointer;

  obj->setTime(time);
  return Py_BuildValue("");
}

PyObject* display(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
165
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
166

167
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
  SignalBase<int>* obj = (SignalBase<int>*)pointer;

  std::ostringstream oss;
  try {
    obj->display(oss);
  }
  CATCH_ALL_EXCEPTIONS();

  return Py_BuildValue("s", oss.str().c_str());
}

PyObject* displayDependencies(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  int time;
  if (!PyArg_ParseTuple(args, "OI", &object, &time)) return NULL;
184
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
185

186
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
187
188
189
190
191
192
193
194
195
196
197
198
199
200
  SignalBase<int>* obj = (SignalBase<int>*)pointer;

  std::ostringstream oss;
  try {
    obj->displayDependencies(oss, time);
  }
  CATCH_ALL_EXCEPTIONS();
  return Py_BuildValue("s", oss.str().c_str());
}

PyObject* getValue(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
201
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
202

203
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
204
205
206
207
208
209
210
211
212
213
214
215
  SignalBase<int>* signal = (SignalBase<int>*)pointer;

  try {
    {  // --- VECTOR SIGNALS -----------------
      // Two cases: the signal embeds directly a vector, or embeds
      // an object deriving from vector.In the first case,
      // the signal is directly cast into sig<vector>.
      // In the second case, the derived object can be access as a vector
      // using the signal-ptr<vector> type.
      Signal<dynamicgraph::Vector, int>* sigvec = dynamic_cast<Signal<dynamicgraph::Vector, int>*>(signal);
      if (NULL != sigvec) {
        return vectorToPython(sigvec->accessCopy());
216
      }
florent's avatar
florent committed
217

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
218
219
220
221
222
223
224
225
226
      // Extraction of object derinving from vector: plug signal into
      // a vector signal and get the value from the signal-ptr instead
      // of the original vector.
      SignalPtr<dynamicgraph::Vector, int> sigptr(NULL, "vector-caster");
      try {
        sigptr.plug(signal);
        return vectorToPython(sigptr.accessCopy());
      } catch (dynamicgraph::ExceptionSignal& ex) {
        if (ex.getCode() != dynamicgraph::ExceptionSignal::PLUG_IMPOSSIBLE) throw;
227
      }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
228
229
230
231
232
233
234
235
236
237
238
    }

    {  // --- MATRIX SIGNALS --------------------
      // Two cases: the signal embeds directly a matrix, or embeds
      // an object deriving from matrix.In the first case,
      // the signal is directly cast into sig<matrix>.
      // In the second case, the derived object can be access as a matrix
      // using the signal-ptr<matrix> type.
      Signal<dynamicgraph::Matrix, int>* sigmat = dynamic_cast<Signal<dynamicgraph::Matrix, int>*>(signal);
      if (NULL != sigmat) {
        return matrixToPython(sigmat->accessCopy());
239
240
      }

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
241
242
243
244
245
246
      SignalPtr<dynamicgraph::Matrix, int> sigptr(NULL, "matrix-caster");
      try {
        sigptr.plug(signal);
        return matrixToPython(sigptr.accessCopy());
      } catch (dynamicgraph::ExceptionSignal& ex) {
        if (ex.getCode() != dynamicgraph::ExceptionSignal::PLUG_IMPOSSIBLE) throw;
247
      }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
248
249
250
251
252
253
254
255
256
257
258
259
260
261
    }

    {  // --- HOMOGENEOUS MATRIX SIGNALS --------------------
      // Two cases: the signal embeds directly a matrix, or embeds
      // an object deriving from matrix.In the first case,
      // the signal is directly cast into sig<matrix>.
      // In the second case, the derived object can be access as a matrix
      // using the signal-ptr<matrix> type.

      // TODO: See if matrix homogeneous can be properly put in linear-algebra.h
      typedef Eigen::Transform<double, 3, Eigen::Affine> MatrixHomogeneous;
      Signal<MatrixHomogeneous, int>* sigmat = dynamic_cast<Signal<MatrixHomogeneous, int>*>(signal);
      if (NULL != sigmat) {
        return matrixToPython(sigmat->accessCopy().matrix());
Florent Lamiraux's avatar
Florent Lamiraux committed
262
263
      }

Guilhem Saurel's avatar
format    
Guilhem Saurel committed
264
265
266
267
268
269
      SignalPtr<Eigen::Transform<double, 3, Eigen::Affine>, int> sigptr(NULL, "matrix-caster");
      try {
        sigptr.plug(signal);
        return matrixToPython(sigptr.accessCopy().matrix());
      } catch (dynamicgraph::ExceptionSignal& ex) {
        if (ex.getCode() != dynamicgraph::ExceptionSignal::PLUG_IMPOSSIBLE) throw;
Florent Lamiraux's avatar
Florent Lamiraux committed
270
      }
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
    }

    Signal<double, int>* sigdouble = dynamic_cast<Signal<double, int>*>(signal);
    if (NULL != sigdouble) {
      return Py_BuildValue("d", sigdouble->accessCopy());
    }
  }
  CATCH_ALL_EXCEPTIONS();

  /* Non specific signal: use a generic way. */
  std::ostringstream value;
  try {
    signal->get(value);
  }
  CATCH_ALL_EXCEPTIONS();

  std::string valueString = value.str();
  return Py_BuildValue("s", valueString.c_str());
}

PyObject* getName(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
295
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
296

297
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
  SignalBase<int>* signal = (SignalBase<int>*)pointer;

  std::string name;
  try {
    name = signal->getName();
  }
  CATCH_ALL_EXCEPTIONS();

  return Py_BuildValue("s", name.c_str());
}

PyObject* getClassName(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
313
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
314

315
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
  SignalBase<int>* signal = (SignalBase<int>*)pointer;

  std::string name;
  try {
    signal->getClassName(name);
  }
  CATCH_ALL_EXCEPTIONS();

  return Py_BuildValue("s", name.c_str());
}

PyObject* setValue(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  char* valueString = NULL;

  if (!PyArg_ParseTuple(args, "Os", &object, &valueString)) return NULL;
333
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
334

335
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
  SignalBase<int>* signal = (SignalBase<int>*)pointer;
  std::ostringstream os;
  os << valueString;
  std::istringstream value(os.str());

  try {
    signal->set(value);
  }
  CATCH_ALL_EXCEPTIONS();
  return Py_BuildValue("");
}

PyObject* recompute(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  unsigned int time;
  if (!PyArg_ParseTuple(args, "OI", &object, &time)) return NULL;
353
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
354

355
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
356
357
358
359
360
361
362
363
364
365
366
367
  SignalBase<int>* signal = (SignalBase<int>*)pointer;
  try {
    signal->recompute(time);
  }
  CATCH_ALL_EXCEPTIONS();
  return Py_BuildValue("");
}

PyObject* unplug(PyObject* /*self*/, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
368
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
369

370
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
371
372
373
374
375
376
377
378
379
380
381
382
  SignalBase<int>* signal = (SignalBase<int>*)pointer;
  try {
    signal->unplug();
  }
  CATCH_ALL_EXCEPTIONS();
  return Py_BuildValue("");
}

PyObject* isPlugged(PyObject*, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
383
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
384

385
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
  SignalBase<int>* signal = (SignalBase<int>*)pointer;
  bool plugged = false;
  try {
    plugged = signal->isPlugged();
  }
  CATCH_ALL_EXCEPTIONS();
  if (plugged)
    return PyBool_FromLong(1);
  else
    return PyBool_FromLong(0);
}

PyObject* getPlugged(PyObject*, PyObject* args) {
  void* pointer = NULL;
  PyObject* object = NULL;
  if (!PyArg_ParseTuple(args, "O", &object)) return NULL;
402
  if (!PyCapsule_CheckExact(object)) return NULL;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
403

404
  pointer = PyCapsule_GetPointer(object, "dynamic_graph.Signal");
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
405
406
407
408
409
410
411
412
413
414
415
416
417
  SignalBase<int>* signal = (SignalBase<int>*)pointer;
  SignalBase<int>* otherSignal = 0;
  try {
    bool plugged = signal->isPlugged();
    otherSignal = signal->getPluged();
    if (!plugged || otherSignal == 0) {
      std::string msg = std::string("Signal ") + signal->getName() + std::string(" is not plugged.");
      throw std::runtime_error(msg);
    }
  }
  CATCH_ALL_EXCEPTIONS();
  // Return the pointer to the signal without destructor since the signal
  // is not owned by the calling object.
418
  return PyCapsule_New((void*)otherSignal, "dynamic_graph.Signal", NULL);
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
419
420
421
422
}
}  // namespace signalBase
}  // namespace python
}  // namespace dynamicgraph