Unverified Commit 902c0273 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #172 from machines-in-motion/topic/mnaveau/fix_python_bindings_bugs

[operators][fir_filter] fix some bug on the python bindings side
parents 42ea48c2 57b6fc2d
Pipeline #12184 passed with stage
in 61 minutes
......@@ -4,3 +4,4 @@ include/sot-core/import-default-paths.h
unitTesting/test-paths.h
*~
_build*
*__pycache__*
\ No newline at end of file
......@@ -11,10 +11,10 @@ from .operator import (
HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo,
Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
PoseQuatToMatrixHomo, MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta,
MatrixToHomo, MatrixToQuaternion, MatrixToRPY, MatrixToUTheta,
MatrixHomoToSE3Vector, SE3VectorToMatrixHomo, MatrixTranspose,
Multiply_double_vector, Multiply_matrix_vector,
PoseQuaternionToMatrixHomo, MatrixHomoToPoseRollPitchYaw,
MatrixHomoToPoseUTheta, MatrixToHomo, MatrixToQuaternion, MatrixToRPY,
MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo,
MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector,
Multiply_matrixTwist_vector, Multiply_matrixHomo_vector,
Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo,
Multiply_of_matrixrotation, Multiply_of_matrixtwist,
......
......@@ -4,7 +4,7 @@ from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple
from dynamic_graph.sot.core.meta_task_6d import toFlags # kept for backward compatibility
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.sot import Task
from dynamic_graph.sot.core.task import Task
from numpy import identity, matrix, zeros
......
......@@ -3,7 +3,7 @@ from dynamic_graph.sot.core.feature_visual_point import FeatureVisualPoint
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
from dynamic_graph.sot.core.sot import Task
from dynamic_graph.sot.core.task import Task
from dynamic_graph.sot.core.visual_point_projecter import VisualPointProjecter
......
......@@ -23,7 +23,7 @@ using dynamicgraph::Vector;
\
template <> \
const std::string sotClassType<sotSigType, sotCoefType>::CLASS_NAME = \
std::string(className) + "_" + #sotSigType + "," + #sotCoefType + "_"; \
std::string(className) + "_" + #sotSigType + "_" + #sotCoefType; \
\
template <> \
const std::string &sotClassType<sotSigType, sotCoefType>::getClassName(void) \
......
......@@ -99,9 +99,12 @@ BOOST_PYTHON_MODULE(wrap)
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */
exposeUnaryOp < HomogeneousMatrixToVector> ();
exposeUnaryOp < SkewSymToVector> ();
exposeUnaryOp < PoseUThetaToMatrixHomo> ();
exposeUnaryOp < MatrixHomoToPoseUTheta> ();
exposeUnaryOp < MatrixHomoToSE3Vector> ();
exposeUnaryOp < SE3VectorToMatrixHomo> ();
exposeUnaryOp < PoseQuaternionToMatrixHomo> ();
exposeUnaryOp < MatrixHomoToPoseQuaternion> ();
exposeUnaryOp < MatrixHomoToPoseRollPitchYaw> ();
exposeUnaryOp < PoseRollPitchYawToMatrixHomo> ();
......
......@@ -57,42 +57,13 @@ REGISTER_UNARY_OP(InverserQuaternion, Inverse_of_unitquat);
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */
REGISTER_UNARY_OP(HomogeneousMatrixToVector, MatrixHomoToPoseUTheta);
struct HomogeneousMatrixToSE3Vector
: public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
res.resize(12);
res.head<3>() = M.translation();
res.segment(3, 3) = M.linear().row(0);
res.segment(6, 3) = M.linear().row(1);
res.segment(9, 3) = M.linear().row(2);
}
};
REGISTER_UNARY_OP(HomogeneousMatrixToSE3Vector, MatrixHomoToSE3Vector);
struct SE3VectorToHomogeneousMatrix
: public UnaryOpHeader<dg::Vector, MatrixHomogeneous> {
void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
Mres.translation() = vect.head<3>();
Mres.linear().row(0) = vect.segment(3, 3);
Mres.linear().row(1) = vect.segment(6, 3);
Mres.linear().row(2) = vect.segment(9, 3);
}
};
REGISTER_UNARY_OP(SE3VectorToHomogeneousMatrix, SE3VectorToMatrixHomo);
REGISTER_UNARY_OP(MatrixHomoToPoseUTheta, MatrixHomoToPoseUTheta);
REGISTER_UNARY_OP(MatrixHomoToSE3Vector, MatrixHomoToSE3Vector);
REGISTER_UNARY_OP(SE3VectorToMatrixHomo, SE3VectorToMatrixHomo);
REGISTER_UNARY_OP(SkewSymToVector, SkewSymToVector);
REGISTER_UNARY_OP(PoseUThetaToMatrixHomo, PoseUThetaToMatrixHomo);
REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion, MatrixHomoToPoseQuaternion);
struct PoseQuaternionToMatrixHomo
: public UnaryOpHeader<Vector, MatrixHomogeneous> {
void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
Mres.translation() = vect.head<3>();
Mres.linear() = VectorQuaternion(vect.tail<4>()).toRotationMatrix();
}
};
REGISTER_UNARY_OP(PoseQuaternionToMatrixHomo, PoseQuatToMatrixHomo);
REGISTER_UNARY_OP(PoseQuaternionToMatrixHomo, PoseQuaternionToMatrixHomo);
REGISTER_UNARY_OP(MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseRollPitchYaw);
REGISTER_UNARY_OP(PoseRollPitchYawToMatrixHomo, PoseRollPitchYawToMatrixHomo);
REGISTER_UNARY_OP(PoseRollPitchYawToPoseUTheta, PoseRollPitchYawToPoseUTheta);
......
......@@ -278,7 +278,7 @@ struct InverserQuaternion
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */
struct HomogeneousMatrixToVector
struct MatrixHomoToPoseUTheta
: public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
res.resize(6);
......@@ -310,6 +310,35 @@ struct PoseUThetaToMatrixHomo
}
};
struct SE3VectorToMatrixHomo
: public UnaryOpHeader<dg::Vector, MatrixHomogeneous> {
void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
Mres.translation() = vect.head<3>();
Mres.linear().row(0) = vect.segment(3, 3);
Mres.linear().row(1) = vect.segment(6, 3);
Mres.linear().row(2) = vect.segment(9, 3);
}
};
struct MatrixHomoToSE3Vector
: public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
res.resize(12);
res.head<3>() = M.translation();
res.segment(3, 3) = M.linear().row(0);
res.segment(6, 3) = M.linear().row(1);
res.segment(9, 3) = M.linear().row(2);
}
};
struct PoseQuaternionToMatrixHomo
: public UnaryOpHeader<Vector, MatrixHomogeneous> {
void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
Mres.translation() = vect.head<3>();
Mres.linear() = VectorQuaternion(vect.tail<4>()).toRotationMatrix();
}
};
struct MatrixHomoToPoseQuaternion
: public UnaryOpHeader<MatrixHomogeneous, Vector> {
inline void operator()(const MatrixHomogeneous &M, Vector &res) {
......
......@@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(matrixHomo_poseQuaternion) {
test_impl<MatrixHomoToPoseQuaternion, PoseQuaternionToMatrixHomo>();
}
BOOST_AUTO_TEST_CASE(matrixHomo_se3Vector) {
test_impl<HomogeneousMatrixToSE3Vector, SE3VectorToHomogeneousMatrix>();
test_impl<MatrixHomoToSE3Vector, SE3VectorToMatrixHomo>();
}
BOOST_AUTO_TEST_SUITE_END()
#!/usr/bin/env python
import unittest
class PythonImportTest(unittest.TestCase):
def test_math_small_entities(self):
try:
import dynamic_graph.sot.core.math_small_entities
except ImportError as ie:
self.fail(str(ie))
def test_feature_position_relative(self):
try:
import dynamic_graph.sot.core.feature_position_relative
except ImportError as ie:
self.fail(str(ie))
def test_feature_position(self):
try:
import dynamic_graph.sot.core.feature_position
except ImportError as ie:
self.fail(str(ie))
def test_matrix_util(self):
try:
import dynamic_graph.sot.core.matrix_util
except ImportError as ie:
self.fail(str(ie))
def test_meta_task_6d(self):
try:
import dynamic_graph.sot.core.meta_task_6d
except ImportError as ie:
self.fail(str(ie))
def test_meta_task_posture(self):
try:
import dynamic_graph.sot.core.meta_task_posture
except ImportError as ie:
self.fail(str(ie))
def test_meta_task_visual_point(self):
try:
import dynamic_graph.sot.core.meta_task_visual_point
except ImportError as ie:
self.fail(str(ie))
def test_meta_tasks_kine_relative(self):
try:
import dynamic_graph.sot.core.meta_tasks_kine_relative
except ImportError as ie:
self.fail(str(ie))
def test_meta_tasks_kine(self):
try:
import dynamic_graph.sot.core.meta_tasks_kine
except ImportError as ie:
self.fail(str(ie))
def test_meta_tasks(self):
try:
import dynamic_graph.sot.core.meta_tasks
except ImportError as ie:
self.fail(str(ie))
def test_attime(self):
try:
import dynamic_graph.sot.core.utils.attime
except ImportError as ie:
self.fail(str(ie))
def test_history(self):
try:
import dynamic_graph.sot.core.utils.history
except ImportError as ie:
self.fail(str(ie))
@unittest.skip("Fail on 'import dynamic_graph.matlab'")
def test_thread_interruptible_loop(self):
try:
import dynamic_graph.sot.core.utils.thread_interruptible_loop
except ImportError as ie:
self.fail(str(ie))
def test_viewer_helper(self):
try:
import dynamic_graph.sot.core.utils.viewer_helper
except ImportError as ie:
self.fail(str(ie))
def test_viewer_loger(self):
try:
import dynamic_graph.sot.core.utils.viewer_loger
except ImportError as ie:
self.fail(str(ie))
if __name__ == '__main__':
unittest.main()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment