From 4a48fe4739fd57d3d578a25661f6b3443000a2c9 Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Fri, 29 Jan 2016 23:02:45 +0100
Subject: [PATCH] [wip/py-dynamic-graph3] Patch for inputing Eigen::Transform
as Matrix4d
---
py-dynamic-graph3/Makefile | 3 +
py-dynamic-graph3/distinfo | 1 +
py-dynamic-graph3/patches/patch-aa | 127 +++++++++++++++++++++++++++++
3 files changed, 131 insertions(+)
create mode 100644 py-dynamic-graph3/patches/patch-aa
diff --git a/py-dynamic-graph3/Makefile b/py-dynamic-graph3/Makefile
index aca5a7b9..f83b9c0c 100644
--- a/py-dynamic-graph3/Makefile
+++ b/py-dynamic-graph3/Makefile
@@ -4,6 +4,8 @@
DISTNAME= dynamic-graph-python-${VERSION}
VERSION= 3.0.0
+PKGREVISION= 1
+
PKGNAME= ${PKGTAG.python-}dynamic-graph-${VERSION}
MASTER_SITES= ${MASTER_SITE_OPENROBOTS:=dynamic-graph-python/}
MASTER_REPOSITORY= ${MASTER_REPOSITORY_GITHUB}/proyan/dynamic-graph-python
@@ -29,6 +31,7 @@ CMAKE_ARGS+= -DCMAKE_INSTALL_LIBDIR=lib
# fixed here
CMAKE_ARGS+= -DCXX_DISABLE_WERROR=yes
+PATCH_STRIP?= -p1
DEPEND_ABI.python+= python<3
include ../../wip/dynamic-graph3/depend.mk
diff --git a/py-dynamic-graph3/distinfo b/py-dynamic-graph3/distinfo
index 2cdf132a..e096e9ba 100644
--- a/py-dynamic-graph3/distinfo
+++ b/py-dynamic-graph3/distinfo
@@ -1,3 +1,4 @@
SHA1 (dynamic-graph-python-3.0.0.tar.gz) = 8c847cca3273d60b9d12c5fb883f3ff8aba25117
RMD160 (dynamic-graph-python-3.0.0.tar.gz) = 64c15fd48b1cf96d60e3e9b7f4267ecbb9a878d3
Size (dynamic-graph-python-3.0.0.tar.gz) = 778414 bytes
+SHA1 (patch-aa) = 3f3f7224e931fd652ac8861b20a2eae39695b8af
diff --git a/py-dynamic-graph3/patches/patch-aa b/py-dynamic-graph3/patches/patch-aa
new file mode 100644
index 00000000..c1343f6d
--- /dev/null
+++ b/py-dynamic-graph3/patches/patch-aa
@@ -0,0 +1,127 @@
+Patch for inputing Eigen::Transform as Matrix4d
+
+diff --git a/src/convert-dg-to-py.cc b/src/convert-dg-to-py.cc
+index 0c9cc61..446dba0 100644
+--- a/src/convert-dg-to-py.cc
++++ b/src/convert-dg-to-py.cc
+@@ -49,6 +49,24 @@ namespace dynamicgraph {
+ "a floating point number.");
+ }
+ }
++ void fillMatrixRow(Eigen::Matrix4d& m, unsigned iRow, PyObject* tuple)
++ {
++ if (PyTuple_Size(tuple) != (int)m.cols()) {
++ throw ExceptionPython(ExceptionPython::MATRIX_PARSING,
++ "lines of matrix have different sizes.");
++ }
++ for (int iCol=0; iCol < m.cols(); iCol++) {
++ PyObject* pyDouble = PyTuple_GetItem(tuple, iCol);
++ if (PyFloat_Check(pyDouble))
++ m(iRow, iCol) = PyFloat_AsDouble(pyDouble);
++ else if(PyInt_Check(pyDouble))
++ m(iRow, iCol) = (int)PyInt_AS_LONG(pyDouble)+0.0;
++ else
++ throw ExceptionPython(ExceptionPython::MATRIX_PARSING,
++ "element of matrix should be "
++ "a floating point number.");
++ }
++ }
+
+ command::Value pythonToValue(PyObject* pyObject,
+ const command::Value::Type& valueType)
+@@ -62,6 +80,7 @@ namespace dynamicgraph {
+ std::string svalue;
+ Vector v;
+ Matrix m;
++ Eigen::Matrix4d m4;
+ Py_ssize_t nCols;
+ Py_ssize_t size;
+ PyObject* row;
+@@ -175,6 +194,36 @@ namespace dynamicgraph {
+ }
+ return Value(m);
+ break;
++ case (Value::MATRIX4D) :
++ // Check that argument is a tuple
++ if (!PyTuple_Check(pyObject)) {
++ throw ExceptionPython(ExceptionPython::VALUE_PARSING,
++ "matrix4d");
++ }
++ nRows = PyTuple_Size(pyObject);
++ if (nRows == 0) {
++ return Value(Eigen::Matrix4d());
++ }
++ row = PyTuple_GetItem(pyObject, 0);
++ if (!PyTuple_Check(row)) {
++ throw ExceptionPython(ExceptionPython::MATRIX_PARSING,
++ "matrix4d");
++ }
++ nCols = PyTuple_Size(row);
++
++ m4.resize(nRows, nCols);
++ fillMatrixRow(m4, 0, row);
++
++ for (Py_ssize_t iRow=1; iRow<nRows; iRow++) {
++ row = PyTuple_GetItem(pyObject, iRow);
++ if (!PyTuple_Check(row)) {
++ throw ExceptionPython(ExceptionPython::MATRIX_PARSING,
++ "matrix");
++ }
++ fillMatrixRow(m4, static_cast<unsigned> (iRow), row);
++ }
++ return Value(m4);
++ break;
+ default:
+ std::cerr << "Only int, double and string are supported."
+ << std::endl;
+@@ -206,6 +255,20 @@ namespace dynamicgraph {
+ return tuple;
+ }
+
++ 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;
++ }
++
+ PyObject* valueToPython(const command::Value& value)
+ {
+ using command::Value;
+@@ -217,6 +280,7 @@ namespace dynamicgraph {
+ std::string stringValue;
+ Vector vectorValue;
+ Matrix matrixValue;
++ Eigen::Matrix4d matrix4dValue;
+ switch(value.type()) {
+ case (Value::BOOL) :
+ boolValue = value.value();
+@@ -245,6 +309,9 @@ namespace dynamicgraph {
+ case (Value::MATRIX) :
+ matrixValue = value.value();
+ return matrixToPython(matrixValue);
++ case (Value::MATRIX4D) :
++ matrix4dValue = value.value();
++ return matrix4dToPython(matrix4dValue);
+ default:
+ return Py_BuildValue("");
+ }
+diff --git a/src/convert-dg-to-py.hh b/src/convert-dg-to-py.hh
+index 62d6fc2..13f114f 100644
+--- a/src/convert-dg-to-py.hh
++++ b/src/convert-dg-to-py.hh
+@@ -26,6 +26,7 @@ namespace dynamicgraph {
+ const command::Value::Type& valueType);
+ PyObject* vectorToPython(const Vector& vector);
+ PyObject* matrixToPython(const ::dynamicgraph::Matrix& matrix);
++ PyObject* matrix4dToPython(const Eigen::Matrix4d& matrix);
+ PyObject* valueToPython(const ::dynamicgraph::command::Value& value);
+
+ } // namespace dynamicgraph
--
GitLab