signal-base-py.cc 13.9 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.
//
// This file is part of dynamic-graph-python.
// dynamic-graph-python is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either
// version 3 of the License, or (at your option) any later version.
//
// dynamic-graph-python is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Lesser Public License for more details.  You should
// have received a copy of the GNU Lesser General Public License along
// with dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
15
16
17

#include <Python.h>
#include <iostream>
florent's avatar
florent committed
18
#include <sstream>
19
20

#include <dynamic-graph/signal-base.h>
21
#include <dynamic-graph/signal.h>
22
#include <dynamic-graph/signal-ptr.h>
23
#include <dynamic-graph/signal-caster.h>
24
#include <dynamic-graph/linear-algebra.h>
25
26
#include <dynamic-graph/pool.h>
#include <dynamic-graph/factory.h>
27
28
29

#include "convert-dg-to-py.hh"
#include "exception.hh"
30
#include "signal-wrapper.hh"
31

32
33
34
35
36
using dynamicgraph::SignalBase;

namespace dynamicgraph {
  namespace python {

37
    extern PyObject* dgpyError;
38
    using namespace convert;
39
40
41

    namespace signalBase {

42

43
44
45
46
47
      static void destroy (void* self);

      /**
	 \brief Create an instance of SignalBase
      */
Thomas Moulard's avatar
Thomas Moulard committed
48
      PyObject* create(PyObject* /*self*/, PyObject* args)
49
50
      {
	char *name = NULL;
florent's avatar
florent committed
51

52
53
	if (!PyArg_ParseTuple(args, "s", &name))
	  return NULL;
florent's avatar
florent committed
54

55
56
	SignalBase<int>* obj = NULL;
	obj = new SignalBase<int>(std::string(name));
florent's avatar
florent committed
57

58
59
60
	// Return the pointer
	return PyCObject_FromVoidPtr((void*)obj, destroy);
      }
florent's avatar
florent committed
61

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

        SignalWrapper_t* obj = new SignalWrapper_t(name, o);
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
        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));
            PyErr_SetString(dgpyError, msg.c_str());
            return NULL;
          }
        } else {
          try {
            obj = FactoryStorage::getInstance()->newEntity
              (std::string(className), std::string(instanceName));
          } CATCH_ALL_EXCEPTIONS();
        }
        return dynamic_cast<PythonSignalContainer*>(obj);
97
98
99
100
101
102
103
104
105
106
107
108
109
      }

#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)
      {
110
111
112
113
        PythonSignalContainer* psc = getPythonSignalContainer();
        if (psc == NULL)
          return NULL;

114
115
116
117
118
119
120
	char *name = NULL;
	char *type = NULL;
	PyObject* object = NULL;

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

121
        SignalBase<int>* obj = NULL;
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
        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) {
          PyErr_SetString(dgpyError, error.c_str());
          return NULL;
        }
140
141
142
        // Register signal into the python signal container
        psc->signalRegistration(*obj);

143
	// Return the pointer
144
	return PyCObject_FromVoidPtr((void*)obj, destroy);
145
146
      }

147
148
149
150
151
152
153
154
      /**
	 \brief Destroy an instance of InvertedPendulum
      */
      static void destroy (void* self)
      {
	SignalBase<int>* obj = (SignalBase<int>*)self;
	delete obj;
      }
florent's avatar
florent committed
155

Thomas Moulard's avatar
Thomas Moulard committed
156
      PyObject* getTime(PyObject* /*self*/, PyObject* args)
157
158
159
160
161
162
163
164
165
166
      {
	void* pointer = NULL;
	PyObject* object = NULL;
	if (!PyArg_ParseTuple(args,"O", &object))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* obj = (SignalBase<int>*)pointer;
florent's avatar
florent committed
167

168
169
170
	int time = obj->getTime();
	return Py_BuildValue("i", time);
      }
florent's avatar
florent committed
171

172
173
174
175
176
177
178
179
      PyObject* setTime(PyObject* /*self*/, PyObject* args)
      {
	void* pointer = NULL;
	PyObject* object = NULL;
	int time;
	if (!PyArg_ParseTuple(args,"Oi", &object, &time))
	  return NULL;
	if (!PyCObject_Check(object)) {
180
	  PyErr_SetString(dgpyError, "object should be a C object");
181
182
183
184
185
186
187
188
189
190
	  return NULL;
	}

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* obj = (SignalBase<int>*)pointer;

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

191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
      PyObject* display(PyObject* /*self*/, PyObject* args)
      {
	void* pointer = NULL;
	PyObject* object = NULL;
	if (!PyArg_ParseTuple(args,"O", &object))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* obj = (SignalBase<int>*)pointer;

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

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

211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
      PyObject* displayDependencies(PyObject* /*self*/, PyObject* args)
      {
	void* pointer = NULL;
	PyObject* object = NULL;
	int time;
	if (!PyArg_ParseTuple(args,"OI", &object,&time))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* obj = (SignalBase<int>*)pointer;

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

Thomas Moulard's avatar
Thomas Moulard committed
231
      PyObject* getValue(PyObject* /*self*/, PyObject* args)
florent's avatar
florent committed
232
233
234
235
236
237
238
239
240
241
242
      {
	void* pointer = NULL;
	PyObject* object = NULL;
	if (!PyArg_ParseTuple(args,"O", &object))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;

243
	try {
Nicolas Mansard's avatar
Nicolas Mansard committed
244
245
246
247
248
249
250
251
252
	  { // --- 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 ) {
253
254
255
	      return vectorToPython( sigvec->accessCopy() );
	    }

Nicolas Mansard's avatar
Nicolas Mansard committed
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
	    // 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;
	    }
	  }

	  { // --- 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 ) {
280
281
282
	      return matrixToPython( sigmat->accessCopy() );
	    }

Nicolas Mansard's avatar
Nicolas Mansard committed
283
284
285
286
287
288
289
290
291
292
293
	    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;
	    }
	  }
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
	  
	  { // --- 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() );
	    }
	    
	    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;
	    }
	  }
	  
322
323
324
325
326
327
	  Signal<double,int> * sigdouble
	    = dynamic_cast< Signal<double,int>* >( signal );
	  if( NULL!= sigdouble )
	    {
	      return Py_BuildValue("d", sigdouble->accessCopy() );
	    }
328
	} CATCH_ALL_EXCEPTIONS ();
329
	
330
	/* Non specific signal: use a generic way. */
florent's avatar
florent committed
331
	std::ostringstream value;
332
333
	try {
	  signal->get(value);
334
	} CATCH_ALL_EXCEPTIONS ();
335
	
336
337
	std::string valueString = value.str();
	return Py_BuildValue("s", valueString.c_str());
florent's avatar
florent committed
338
      }
339
      
Thomas Moulard's avatar
Thomas Moulard committed
340
      PyObject* getName(PyObject* /*self*/, PyObject* args)
341
342
343
344
345
346
347
348
349
350
351
352
353
354
      {
	void* pointer = NULL;
	PyObject* object = NULL;
	if (!PyArg_ParseTuple(args,"O", &object))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;

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

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

olivier stasse's avatar
olivier stasse committed
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
      PyObject* getClassName(PyObject* /*self*/, PyObject* args)
      {
	void* pointer = NULL;
	PyObject* object = NULL;
	if (!PyArg_ParseTuple(args,"O", &object))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;

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

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

Thomas Moulard's avatar
Thomas Moulard committed
380
      PyObject* setValue(PyObject* /*self*/, PyObject* args)
florent's avatar
florent committed
381
382
383
384
385
386
387
388
389
390
391
392
      {
	void * pointer = NULL;
	PyObject* object = NULL;
	char* valueString = NULL;

	if (!PyArg_ParseTuple(args,"Os", &object, &valueString))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;
393
394
395
396
397
398
	std::ostringstream os;
	os << valueString;
	std::istringstream value(os.str());

	try {
	  signal->set(value);
399
	} CATCH_ALL_EXCEPTIONS ();
florent's avatar
florent committed
400
401
	return Py_BuildValue("");
      }
402

Thomas Moulard's avatar
Thomas Moulard committed
403
      PyObject* recompute(PyObject* /*self*/, PyObject* args)
404
405
406
407
408
409
410
411
412
413
414
      {
	void * pointer = NULL;
	PyObject* object = NULL;
	unsigned int time;
	if (!PyArg_ParseTuple(args,"OI", &object, &time))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;
415
416
	try {
	  signal->recompute(time);
417
	} CATCH_ALL_EXCEPTIONS ();
418
419
	return Py_BuildValue("");
      }
420
421
422
423
424
425
426
427
428
429
430
431
432
433

      PyObject* unplug(PyObject* /*self*/, PyObject* args)
      {
	void * pointer = NULL;
	PyObject* object = NULL;
	if (!PyArg_ParseTuple(args,"O", &object))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;
	try {
	  signal->unplug();
434
	} CATCH_ALL_EXCEPTIONS ();
435
436
	return Py_BuildValue("");
      }
Florent Lamiraux's avatar
Florent Lamiraux committed
437
438
439
440
441
442
443
444
445
446
447
448
449
450

      PyObject* isPlugged (PyObject*, PyObject* args)
      {
	void * pointer = NULL;
	PyObject* object = NULL;
	if (!PyArg_ParseTuple(args,"O", &object))
	  return NULL;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;
	bool plugged = false;
	try {
451
	  plugged = signal->isPlugged();
Florent Lamiraux's avatar
Florent Lamiraux committed
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
	} 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;
	if (!PyCObject_Check(object))
	  return NULL;

	pointer = PyCObject_AsVoidPtr(object);
	SignalBase<int>* signal = (SignalBase<int>*)pointer;
	SignalBase<int>* otherSignal = 0;
	try {
470
	  bool plugged = signal->isPlugged ();
Florent Lamiraux's avatar
Florent Lamiraux committed
471
472
473
474
475
476
477
478
479
480
481
482
483
484
	  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.
	return PyCObject_FromVoidPtr((void*)otherSignal, NULL);
      }
    } // namespace signalBase
  } // namespace python
} // namespace dynamicgraph