From dbe38c9f5c36e5c4d9c6c1788b2790ea558b88be Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Fri, 20 Jul 2018 00:35:30 -0700 Subject: [PATCH 001/136] Added pybind11 KDL binding --- python_orocos_kdl/CMakeLists.txt | 53 ++-- python_orocos_kdl/PyKDL/PyKDL.cpp | 37 +++ python_orocos_kdl/PyKDL/PyKDL.h | 30 ++ python_orocos_kdl/PyKDL/dynamics.cpp | 86 +++++ python_orocos_kdl/PyKDL/frames.cpp | 341 ++++++++++++++++++++ python_orocos_kdl/PyKDL/frames.sip | 1 - python_orocos_kdl/PyKDL/framevel.cpp | 243 ++++++++++++++ python_orocos_kdl/PyKDL/framevel.sip | 1 + python_orocos_kdl/PyKDL/kinfam.cpp | 457 +++++++++++++++++++++++++++ 9 files changed, 1229 insertions(+), 20 deletions(-) create mode 100644 python_orocos_kdl/PyKDL/PyKDL.cpp create mode 100644 python_orocos_kdl/PyKDL/PyKDL.h create mode 100644 python_orocos_kdl/PyKDL/dynamics.cpp create mode 100644 python_orocos_kdl/PyKDL/frames.cpp create mode 100644 python_orocos_kdl/PyKDL/framevel.cpp create mode 100644 python_orocos_kdl/PyKDL/kinfam.cpp diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 211b81c06..d5af15734 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -9,25 +9,40 @@ find_package(orocos_kdl REQUIRED) include_directories(${orocos_kdl_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) -if(DEFINED ENV{ROS_PYTHON_VERSION}) - SET(PYTHON_VERSION $ENV{ROS_PYTHON_VERSION} CACHE STRING "Python Version") -else() - SET(PYTHON_VERSION 2 CACHE STRING "Python Version") -endif() -find_package(PythonInterp ${PYTHON_VERSION} REQUIRED) -find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED) -execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) -list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -find_package(SIP REQUIRED) -include(SIPMacros) - -include_directories(${SIP_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS}) - -file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.sip") -set(SIP_INCLUDES ${SIP_FILES}) -set(SIP_EXTRA_OPTIONS "-o") -set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) -add_sip_python_module(PyKDL PyKDL/PyKDL.sip ${orocos_kdl_LIBRARIES}) +option(BUILD_PYKDL_PYBIND11 OFF) +if (BUILD_PYKDL_PYBIND11) + find_package(pybind11 REQUIRED) + pybind11_add_module(PyKDL + PyKDL/PyKDL.h + PyKDL/PyKDL.cpp + PyKDL/frames.cpp + PyKDL/kinfam.cpp + PyKDL/framevel.cpp + PyKDL/dynamics.cpp) + target_link_libraries(PyKDL PRIVATE ${orocos_kdl_LIBRARIES}) + +else (BUILD_PYKDL_PYBIND11) + if(DEFINED ENV{ROS_PYTHON_VERSION}) + SET(PYTHON_VERSION $ENV{ROS_PYTHON_VERSION} CACHE STRING "Python Version") + else() + SET(PYTHON_VERSION 2 CACHE STRING "Python Version") + endif() + find_package(PythonInterp ${PYTHON_VERSION} REQUIRED) + find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED) + execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) + list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) + find_package(SIP REQUIRED) + include(SIPMacros) + + include_directories(${SIP_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS}) + + file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.sip") + set(SIP_INCLUDES ${SIP_FILES}) + set(SIP_EXTRA_OPTIONS "-o") + set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) + add_sip_python_module(PyKDL PyKDL/PyKDL.sip ${orocos_kdl_LIBRARIES}) +endif(BUILD_PYKDL_PYBIND11) + install(FILES package.xml DESTINATION share/python_orocos_kdl) diff --git a/python_orocos_kdl/PyKDL/PyKDL.cpp b/python_orocos_kdl/PyKDL/PyKDL.cpp new file mode 100644 index 000000000..79a8f2319 --- /dev/null +++ b/python_orocos_kdl/PyKDL/PyKDL.cpp @@ -0,0 +1,37 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits +//Author: Zihan Chen +//Maintainer: Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library 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 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include "PyKDL.h" + +namespace py = pybind11; + +PYBIND11_MODULE(PyKDL, m) +{ + m.doc() = "Orocos KDL Python wrapper"; // optional module docstring + m.attr("__version__") = KDL_VERSION_STRING; + init_frames(m); + init_kinfam(m); + init_framevel(m); + init_dynamics(m); +} diff --git a/python_orocos_kdl/PyKDL/PyKDL.h b/python_orocos_kdl/PyKDL/PyKDL.h new file mode 100644 index 000000000..7fdcc6f2b --- /dev/null +++ b/python_orocos_kdl/PyKDL/PyKDL.h @@ -0,0 +1,30 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits +//Author: Zihan Chen +//Maintainer: Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library 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 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include + +void init_frames(pybind11::module &m); +void init_kinfam(pybind11::module &m); +void init_framevel(pybind11::module &m); +void init_dynamics(pybind11::module &m); diff --git a/python_orocos_kdl/PyKDL/dynamics.cpp b/python_orocos_kdl/PyKDL/dynamics.cpp new file mode 100644 index 000000000..095e04a68 --- /dev/null +++ b/python_orocos_kdl/PyKDL/dynamics.cpp @@ -0,0 +1,86 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits +//Author: Zihan Chen +//Maintainer: Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library 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 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_dynamics(pybind11::module &m) +{ + // -------------------- + // JntSpaceInertiaMatrix + // -------------------- + py::class_(m, "JntSpaceInertiaMatrix") + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def("resize", &JntSpaceInertiaMatrix::resize) + .def("rows", &JntSpaceInertiaMatrix::rows) + .def("columns", &JntSpaceInertiaMatrix::columns) + .def("__getitem__", [](const JntSpaceInertiaMatrix &jm, std::tuple idx) { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > jm.rows() || j < 0 || j > jm.columns()) { + throw py::index_error("Inertia index out of range"); + } + return jm((unsigned int)i, (unsigned int)j); + }) + .def("__repr__", [](const JntSpaceInertiaMatrix &jm) { + std::ostringstream oss; + for (size_t r = 0; r < jm.rows(); r++) { + for (size_t c = 0; c < jm.columns(); c++) { + oss << std::setw(KDL_FRAME_WIDTH) << jm(r, c); + } + oss << std::endl; + } + return oss.str(); + }) + .def(py::self == py::self) + ; + + m.def("Add", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, JntSpaceInertiaMatrix&)) &KDL::Add); + m.def("Subtract", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&,JntSpaceInertiaMatrix&)) &KDL::Subtract); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Multiply); + m.def("Divide", (void (*)(const JntSpaceInertiaMatrix&, const double&, JntSpaceInertiaMatrix&)) &KDL::Divide); + m.def("Multiply", (void (*)(const JntSpaceInertiaMatrix&, const JntArray&, JntArray&)) &KDL::Multiply); + m.def("SetToZero", (void (*)(JntSpaceInertiaMatrix&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // ChainDynParam + // -------------------- + py::class_(m, "ChainDynParam") + .def(py::init()) + .def("JntToCoriolis", &ChainDynParam::JntToCoriolis) + .def("JntToMass", &ChainDynParam::JntToMass) + .def("JntToGravity", &ChainDynParam::JntToGravity) + ; +} diff --git a/python_orocos_kdl/PyKDL/frames.cpp b/python_orocos_kdl/PyKDL/frames.cpp new file mode 100644 index 000000000..3c6350847 --- /dev/null +++ b/python_orocos_kdl/PyKDL/frames.cpp @@ -0,0 +1,341 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits +//Author: Zihan Chen +//Maintainer: Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library 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 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + +void init_frames(py::module &m) +{ + // -------------------- + // Vector + // -------------------- + py::class_(m, "Vector") + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def("x", (void (Vector::*)(double)) &Vector::x, "Set x") + .def("y", (void (Vector::*)(double)) &Vector::y, "Set y") + .def("z", (void (Vector::*)(double)) &Vector::z, "Set z") + .def("x", (double (Vector::*)(void) const) &Vector::x, "Get x") + .def("y", (double (Vector::*)(void) const) &Vector::y, "Get y") + .def("z", (double (Vector::*)(void) const) &Vector::z, "Get z") + .def("__getitem__", [](const Vector &v, int i) { + if (i < 0 || i > 2) throw py::index_error("Vector index out of range"); + return v(i); + }) + .def("__setitem__", [](Vector &v, int i, double value) { + if (i < 0 || i > 2) throw py::index_error("Vector index out of range"); + v(i) = value; + }) + .def("__repr__", [](const Vector &v) { + std::ostringstream oss; + oss<(m, "Rotation") + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def("__getitem__", [](const Rotation &r, int i, int j) { + if (i < 0 || i > 2 || j < 0 || j > 2) throw py::index_error("Vector index out of range"); + return r(i, j); + }) + .def("__repr__", [](const Rotation &r) { + std::ostringstream oss; + oss<(m, "Frame") + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def(py::init<>()) + .def_readwrite("M", &Frame::M) + .def_readwrite("p", &Frame::p) + .def("__getitem__", [](const Frame &frm, std::tuple idx) { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) { + throw py::index_error("Frame index out of range"); + } + return frm((unsigned int)i, (unsigned int)j); + }) + .def("__setitem__", [](Frame &frm, std::tuple idx, double value) { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) { + throw py::index_error("Frame index out of range"); + } + if (j == 3) { + frm.p(i) = value; + } else { + frm.M(i, j) = value; + } + }) + .def("__repr__", [](const Frame &frm) { + std::ostringstream oss; + oss << frm; + return oss.str(); + }) + .def("DH_Craig1989", &Frame::DH_Craig1989) + .def("DH", &Frame::DH) + .def("Inverse", (Frame (Frame::*)() const) &Frame::Inverse) + .def("Inverse", (Vector (Frame::*)(const Vector&) const) &Frame::Inverse) + .def("Inverse", (Wrench (Frame::*)(const Wrench&) const) &Frame::Inverse) + .def("Inverse", (Twist (Frame::*)(const Twist&) const) &Frame::Inverse) + .def_static("Identity", &Frame::Identity) + .def("Integrate", &Frame::Integrate) + .def(py::self * Vector()) + .def(py::self * Wrench()) + .def(py::self * Twist()) + .def(py::self * py::self) + .def(py::self == py::self) + .def(py::self != py::self) + ; + + m.def("Equal", (bool (*)(const Frame&, const Frame&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Twist + // -------------------- + py::class_(m, "Twist") + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def_readwrite("vel", &Twist::vel) + .def_readwrite("rot", &Twist::rot) + .def("__getitem__", [](const Twist &t, int i) { + if (i < 0 || i > 5) { + throw py::index_error("Twist index out of range"); + } + return t(i); + }) + .def("__setitem__", [](Twist &t, int i, double value) { + if (i < 0 || i > 5) { + throw py::index_error("Twist index out of range"); + } + t(i) = value; + }) + .def("__repr__", [](const Twist &t) { + std::ostringstream oss; + oss << t; + return oss.str(); + }) + .def_static("Zero", &Twist::Zero) + .def("ReverseSign", &Twist::ReverseSign) + .def("RefPoint", &Twist::RefPoint) + .def(py::self -= py::self) + .def(py::self += py::self) + .def(py::self * double()) + .def(double() * py::self) + .def(py::self / double()) + .def(py::self + py::self) + .def(py::self - py::self) + .def(py::self == py::self) + .def(py::self != py::self) + .def("__neg__", [](const Twist &a) { + return operator-(a); + }, py::is_operator()) + ; + + m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot); + m.def("dot", (double (*)(const Wrench&, const Twist&)) &KDL::dot); + m.def("SetToZero", (void (*)(Twist&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const Twist&, const Twist&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Wrench + // -------------------- + py::class_(m, "Wrench") + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def_readwrite("force", &Wrench::force) + .def_readwrite("torque", &Wrench::torque) + .def("__getitem__", [](const Wrench &t, int i) { + if (i < 0 || i > 5) { + throw py::index_error("Wrench index out of range"); + } + return t(i); + }) + .def("__setitem__", [](Wrench &t, int i, double value) { + if (i < 0 || i > 5) { + throw py::index_error("Wrench index out of range"); + } + t(i) = value; + }) + .def("__repr__", [](const Wrench &t) { + std::ostringstream oss; + oss << t; + return oss.str(); + }) + .def_static("Zero", &Wrench::Zero) + .def("ReverseSign", &Wrench::ReverseSign) + .def("RefPoint", &Wrench::RefPoint) + .def(py::self -= py::self) + .def(py::self += py::self) + .def(py::self * double()) + .def(double() * py::self) + .def(py::self / double()) + .def(py::self + py::self) + .def(py::self - py::self) + .def(py::self == py::self) + .def(py::self != py::self) + .def("__neg__", [](const Wrench &w) { + return operator-(w); + }, py::is_operator()) + ; + + m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Global + // -------------------- + m.def("diff", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Vector (*)(const Rotation&, const Rotation&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Twist (*)(const Frame&, const Frame&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("diff", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt") = 1); + m.def("addDelta", (Vector (*)(const Vector&, const Vector&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Rotation (*)(const Rotation&, const Vector&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Frame (*)(const Frame&, const Twist&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Twist (*)(const Twist&, const Twist&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); + m.def("addDelta", (Wrench (*)(const Wrench&, const Wrench&, double dt)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt") = 1); +} diff --git a/python_orocos_kdl/PyKDL/frames.sip b/python_orocos_kdl/PyKDL/frames.sip index 59f27ceef..84302a674 100644 --- a/python_orocos_kdl/PyKDL/frames.sip +++ b/python_orocos_kdl/PyKDL/frames.sip @@ -258,7 +258,6 @@ public: Twist operator * (const Twist& arg) const /Numeric,Factory/; static Frame Identity() /Factory/; - void Integrate(const Twist& t_this,double frequency); %PickleCode diff --git a/python_orocos_kdl/PyKDL/framevel.cpp b/python_orocos_kdl/PyKDL/framevel.cpp new file mode 100644 index 000000000..9a4a46224 --- /dev/null +++ b/python_orocos_kdl/PyKDL/framevel.cpp @@ -0,0 +1,243 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits +//Author: Zihan Chen +//Maintainer: Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library 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 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + + +void init_framevel(pybind11::module &m) +{ + // -------------------- + // doubleVel + // -------------------- + py::class_ >(m, "doubleVel") + .def_readwrite("t", &doubleVel::t) + .def_readwrite("grad", &doubleVel::grad) + .def(py::init<>()) + ; + m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("addDelta", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("Equal", (bool (*)(const doubleVel&, const doubleVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + py::class_(m, "VectorVel") + .def_readwrite("p", &VectorVel::p) + .def_readwrite("v", &VectorVel::v) + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def("value", &VectorVel::value) + .def("deriv", &VectorVel::deriv) + .def_static("Zero", &VectorVel::Zero) + .def("ReverseSign", &VectorVel::ReverseSign) + .def("Norm", &VectorVel::Norm) + .def(py::self += py::self) + .def(py::self -= py::self) + .def(py::self + py::self) + .def(py::self - py::self) + .def(Vector() + py::self) + .def(Vector() - py::self) + .def(py::self + Vector()) + .def(py::self - Vector()) + + .def(py::self * py::self) + .def(py::self * Vector()) + .def(Vector() * py::self) + .def(double() * py::self) + .def(py::self * double()) + .def(doubleVel() * py::self) + .def(py::self * doubleVel()) + .def(Rotation() * py::self) + + .def(py::self / double()) + .def(py::self / doubleVel()) + .def("__neg__", [](const VectorVel &a) { + return operator-(a); + }, py::is_operator()) + ; + + m.def("Equal", (bool (*)(const VectorVel&, const VectorVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Vector&, const VectorVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const VectorVel&, const Vector&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + m.def("dot", (doubleVel (*)(const VectorVel&, const VectorVel&)) &KDL::dot); + m.def("dot", (doubleVel (*)(const VectorVel&, const Vector&)) &KDL::dot); + m.def("dot", (doubleVel (*)(const Vector&, const VectorVel&)) &KDL::dot); + + + // -------------------- + // RotationVel + // -------------------- + py::class_(m, "RotationVel") + .def_readwrite("R", &RotationVel::R) + .def_readwrite("w", &RotationVel::w) + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def("value", &RotationVel::value) + .def("deriv", &RotationVel::deriv) + .def("UnitX", &RotationVel::UnitX) + .def("UnitY", &RotationVel::UnitY) + .def("UnitZ", &RotationVel::UnitZ) + .def_static("Identity", &RotationVel::Identity) + .def("Inverse", (RotationVel (RotationVel::*)(void) const) &RotationVel::Inverse) + .def("Inverse", (VectorVel (RotationVel::*)(const VectorVel&) const) &RotationVel::Inverse) + .def("Inverse", (VectorVel (RotationVel::*)(const Vector&) const) &RotationVel::Inverse) + .def("DoRotX", &RotationVel::DoRotX) + .def("DoRotY", &RotationVel::DoRotY) + .def("DoRotZ", &RotationVel::DoRotZ) + .def_static("RotX", &RotationVel::RotX) + .def_static("RotY", &RotationVel::RotY) + .def_static("RotZ", &RotationVel::RotZ) + .def_static("Rot", &RotationVel::Rot) + .def_static("Rot2", &RotationVel::Rot2) + + .def("Inverse", (TwistVel (RotationVel::*)(const TwistVel&) const) &RotationVel::Inverse) + .def("Inverse", (TwistVel (RotationVel::*)(const Twist&) const) &RotationVel::Inverse) + + .def(py::self * VectorVel()) + .def(py::self * Vector()) + .def(py::self * TwistVel()) + .def(py::self * Twist()) + .def(py::self * py::self) + .def(Rotation() * py::self) + .def(py::self * Rotation()) + ; + + m.def("Equal", (bool (*)(const RotationVel&, const RotationVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Rotation&, const RotationVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const RotationVel&, const Rotation&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // FrameVel + // -------------------- + py::class_(m, "FrameVel") + .def_readwrite("M", &FrameVel::M) + .def_readwrite("p", &FrameVel::p) + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def("value", &FrameVel::value) + .def("deriv", &FrameVel::deriv) + .def_static("Identity", &FrameVel::Identity) + .def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse) + .def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse) + .def("Inverse", (VectorVel (FrameVel::*)(const Vector&) const) &FrameVel::Inverse) + .def(py::self * VectorVel()) + .def(py::self * Vector()) + .def("GetFrame", &FrameVel::GetFrame) + .def("GetTwist", &FrameVel::GetTwist) + .def("Inverse", (TwistVel (FrameVel::*)(const TwistVel&) const) &FrameVel::Inverse) + .def("Inverse", (TwistVel (FrameVel::*)(const Twist&) const) &FrameVel::Inverse) + .def(py::self * TwistVel()) + .def(py::self * Twist()) + .def(py::self * py::self) + .def(Frame() * py::self) + .def(py::self * Frame()) + ; + + m.def("Equal", (bool (*)(const FrameVel&, const FrameVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Frame&, const FrameVel&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const FrameVel&, const Frame&, double)) &KDL::Equal, + py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); + + + // -------------------- + // TwistVel + // -------------------- + py::class_(m, "TwistVel") + .def_readwrite("vel", &TwistVel::vel) + .def_readwrite("rot", &TwistVel::rot) + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def("value", &TwistVel::value) + .def("deriv", &TwistVel::deriv) + .def_static("Zero", &TwistVel::Zero) + .def("ReverseSign", &TwistVel::ReverseSign) + .def("RefPoint", &TwistVel::RefPoint) + .def("GetTwist", &TwistVel::GetTwist) + .def("GetTwistDot", &TwistVel::GetTwistDot) + + .def(py::self -= py::self) + .def(py::self += py::self) + .def(py::self * double()) + .def(double() * py::self) + .def(py::self / double()) + + .def(py::self * doubleVel()) + .def(doubleVel() * py::self) + .def(py::self / doubleVel()) + + .def(py::self + py::self) + .def(py::self - py::self) + .def("__neg__", [](const TwistVel &a) { + return operator-(a); + }, py::is_operator()) + ; + + m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const TwistVel&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Twist&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const TwistVel&, const Twist&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Global + // -------------------- + m.def("diff", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("diff", (VectorVel (*)(const RotationVel&, const RotationVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + m.def("diff", (TwistVel (*)(const FrameVel&, const FrameVel&, double)) &KDL::diff, + py::arg("a"), py::arg("b"), py::arg("dt")=1.0); + + m.def("addDelta", (VectorVel (*)(const VectorVel&, const VectorVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("addDelta", (RotationVel (*)(const RotationVel&, const VectorVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); + m.def("addDelta", (FrameVel (*)(const FrameVel&, const TwistVel&, double)) &KDL::addDelta, + py::arg("a"), py::arg("da"), py::arg("dt")=1.0); +} diff --git a/python_orocos_kdl/PyKDL/framevel.sip b/python_orocos_kdl/PyKDL/framevel.sip index a3e0ad0ac..110d8344a 100644 --- a/python_orocos_kdl/PyKDL/framevel.sip +++ b/python_orocos_kdl/PyKDL/framevel.sip @@ -73,6 +73,7 @@ VectorVel operator + (const Vector& r1,const VectorVel& r2); VectorVel operator - (const Vector& r1,const VectorVel& r2); VectorVel operator + (const VectorVel& r1,const Vector& r2); VectorVel operator - (const VectorVel& r1,const Vector& r2); + VectorVel operator * (const VectorVel& r1,const VectorVel& r2); VectorVel operator * (const VectorVel& r1,const Vector& r2); VectorVel operator * (const Vector& r1,const VectorVel& r2); diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp new file mode 100644 index 000000000..f883988fc --- /dev/null +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -0,0 +1,457 @@ +//Copyright (C) 2007 Ruben Smits +// +//Version: 1.0 +//Author: Ruben Smits +//Author: Zihan Chen +//Maintainer: Ruben Smits +//URL: http://www.orocos.org/kdl +// +//This library 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 2.1 of the License, or (at your option) any later version. +// +//This library 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 +//Lesser General Public License for more details. +// +//You should have received a copy of the GNU Lesser General Public +//License along with this library; if not, write to the Free Software +//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "PyKDL.h" + +namespace py = pybind11; +using namespace KDL; + +void init_kinfam(pybind11::module &m) +{ + // -------------------- + // Joint + // -------------------- + py::class_ joint(m, "Joint"); + py::enum_(joint, "JointType") + .value("RotAxis", Joint::JointType::RotAxis) + .value("RotX", Joint::JointType::RotX) + .value("RotY", Joint::JointType::RotY) + .value("RotZ", Joint::JointType::RotZ) + .value("TransAxis", Joint::JointType::TransAxis) + .value("TransX", Joint::JointType::TransX) + .value("TransY", Joint::JointType::TransY) + .value("TransZ", Joint::JointType::TransZ) + .value("None", Joint::JointType::None) + .export_values() + ; + + joint.def(py::init<>()); + joint.def(py::init(), + py::arg("name"), py::arg("type")=Joint::JointType::None, py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("type")=Joint::JointType::None, py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("name"), py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init(), + py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0, + py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); + joint.def(py::init()); + joint.def("pose", &Joint::pose); + joint.def("twist", &Joint::twist); + joint.def("JointAxis", &Joint::JointAxis); + joint.def("JointOrigin", &Joint::JointOrigin); + joint.def("getName", &Joint::getName); + joint.def("getType", &Joint::getType); + joint.def("getTypeName", &Joint::getTypeName); + joint.def("__repr__", [](const Joint &j) { + std::ostringstream oss; + oss << j; + return oss.str(); + }); + + + // -------------------- + // RotationalInertia + // -------------------- + py::class_(m, "RotationalInertia") + .def(py::init(), + py::arg("Ixx")=0, py::arg("Iyy")=0, py::arg("Izz")=0, + py::arg("Ixy")=0, py::arg("Ixz")=0, py::arg("Iyz")=0) + .def_static("Zero", &RotationalInertia::Zero) + .def("__getitem__", [](const RotationalInertia &inertia, int i) { + if (i < 0 || i > 8) { + throw py::index_error("RotationalInertia index out of range"); + } + return inertia.data[i]; + }) + .def("__setitem__", [](RotationalInertia &inertia, int i, double value) { + if (i < 0 || i > 8) { + throw py::index_error("RotationalInertia index out of range"); + } + inertia.data[i] = value; + }) + .def(double() * py::self) + .def(py::self + py::self) + .def(py::self * Vector()) + ; + + // -------------------- + // RigidBodyInertia + // -------------------- + py::class_(m, "RigidBodyInertia") + .def(py::init(), + py::arg("m")=0, py::arg("oc")=Vector::Zero(), py::arg("Ic")=RotationalInertia::Zero()) + .def_static("Zero", &RigidBodyInertia::Zero) + .def("RefPoint", &RigidBodyInertia::RefPoint) + .def("getMass", &RigidBodyInertia::getMass) + .def("getCOG", &RigidBodyInertia::getCOG) + .def("getRotationalInertia", &RigidBodyInertia::getRotationalInertia) + .def(double() * py::self) + .def(py::self + py::self) + .def(py::self * Twist()) + .def(Frame() * py::self) + .def(Rotation() * py::self) + ; + + // -------------------- + // Segment + // -------------------- + py::class_(m, "Segment") + .def(py::init(), + py::arg("name"), py::arg("joint")=Joint(), py::arg("f_tip")=Frame::Identity(), py::arg("I")=RigidBodyInertia::Zero()) + .def(py::init(), + py::arg("joint")=Joint(), py::arg("f_tip")=Frame::Identity(), py::arg("I")=RigidBodyInertia::Zero()) + .def(py::init()) + .def("getFrameToTip", &Segment::getFrameToTip) + .def("pose", &Segment::pose) + .def("twist", &Segment::twist) + .def("getName", &Segment::getName) + .def("getJoint", &Segment::getJoint) + .def("getInertia", &Segment::getInertia) + .def("setInertia", &Segment::setInertia) + ; + + // -------------------- + // Chain + // -------------------- + py::class_(m, "Chain") + .def(py::init<>()) + .def(py::init()) + .def("addSegment", &Chain::addSegment) + .def("addChain", &Chain::addChain) + .def("getNrOfJoints", &Chain::getNrOfJoints) + .def("getNrOfSegments", &Chain::getNrOfSegments) + .def("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment) + .def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment) + ; + + // -------------------- + // Tree + // -------------------- + py::class_(m, "Tree") + .def(py::init(), py::arg("root_name")="root") + .def("addSegment", &Tree::addSegment) + .def("addChain", &Tree::addChain) + .def("addTree", &Tree::addTree) + .def("getNrOfJoints", &Tree::getNrOfJoints) + .def("getNrOfSegments", &Tree::getNrOfSegments) + .def("getSegment", &Tree::getSegment) + .def("getRootSegment", &Tree::getRootSegment) + .def("getChain", [](const Tree &tree, const std::string& chain_root, const std::string& chain_tip) { + Chain* chain = new Chain(); + tree.getChain(chain_root, chain_tip, *chain); + return chain; + }) + ; + + // -------------------- + // JntArray + // -------------------- + py::class_(m, "JntArray") + .def(py::init()) + .def(py::init()) + .def("rows", &JntArray::rows) + .def("columns", &JntArray::columns) + .def("resize", &JntArray::resize) + .def("__getitem__", [](const JntArray &ja, int i) { + if (i < 0 || i > ja.rows()) { + throw py::index_error("JntArray index out of range"); + } + return ja(i); + }) + .def("__setitem__", [](JntArray &ja, int i, double value) { + if (i < 0 || i > ja.rows()) { + throw py::index_error("JntArray index out of range"); + } + ja(i) = value; + }) + .def("__repr__", [](const JntArray &ja) { + std::ostringstream oss; + oss << ja; + return oss.str(); + }) + .def(py::self == py::self) + ; + + m.def("Add", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Add); + m.def("Subtract", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Subtract); + m.def("Multiply", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Multiply); + m.def("Divide", (void (*)(const JntArray&, const double&, JntArray&)) &KDL::Divide); + m.def("MultiplyJacobian", (void (*)(const Jacobian&, const JntArray&, Twist&)) &KDL::MultiplyJacobian); + m.def("SetToZero", (void (*)(JntArray&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const JntArray&, const JntArray&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // JntArrayVel + // -------------------- + py::class_(m, "JntArrayVel") + .def_readwrite("q", &JntArrayVel::q) + .def_readwrite("qdot", &JntArrayVel::qdot) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def("resize", &JntArrayVel::resize) + .def("value", &JntArrayVel::value) + .def("deriv", &JntArrayVel::deriv) + ; + + m.def("Add", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Add); + m.def("Add", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Add); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Subtract); + m.def("Subtract", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Subtract); + m.def("Multiply", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Multiply); + m.def("Multiply", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Multiply); + m.def("Divide", (void (*)(const JntArrayVel&, const double&, JntArrayVel&)) &KDL::Divide); + m.def("Divide", (void (*)(const JntArrayVel&, const doubleVel&, JntArrayVel&)) &KDL::Divide); + m.def("SetToZero", (void (*)(JntArrayVel&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const JntArrayVel&, const JntArrayVel&, double)) &KDL::Equal, + py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); + + + // -------------------- + // Jacobian + // -------------------- + py::class_(m, "Jacobian") + .def(py::init()) + .def(py::init()) + .def("rows", &Jacobian::rows) + .def("columns", &Jacobian::columns) + .def("resize", &Jacobian::resize) + .def("getColumn", &Jacobian::getColumn) + .def("setColumn", &Jacobian::setColumn) + .def("changeRefPoint", &Jacobian::changeRefPoint) + .def("changeBase", &Jacobian::changeBase) + .def("changeRefFrame", &Jacobian::changeRefFrame) + .def("__getitem__", [](const Jacobian &jac, std::tuple idx) { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j > jac.columns()) { + throw py::index_error("Jacobian index out of range"); + } + return jac((unsigned int)i, (unsigned int)j); + }) + .def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j > jac.columns()) { + throw py::index_error("Jacobian index out of range"); + } + jac((unsigned int)i, (unsigned int)j) = value; + }) + .def("__repr__", [](const Jacobian &jac) { + std::ostringstream oss; + oss << jac; + return oss.str(); + }) + ; + + m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero); + m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint); + m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase); + m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame); + + + // -------------------- + // SolverI + // -------------------- + py::class_(m, "SolverI") + .def("getError", &SolverI::getError) + .def("strError", &SolverI::strError) + .def("updateInternalDataStructures", &SolverI::updateInternalDataStructures) + ; + + // -------------------- + // ChainFkSolverPos + // -------------------- + py::class_(m, "ChainFkSolverPos") + .def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1) + ; + + // -------------------- + // ChainFkSolverVel + // -------------------- + py::class_(m, "ChainFkSolverVel") + .def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1) + ; + + // ------------------------------ + // ChainFkSolverPos_recursive + // ------------------------------ + py::class_(m, "ChainFkSolverPos_recursive") + .def(py::init()) + ; + + + // ------------------------------ + // ChainFkSolverVel_recursive + // ------------------------------ + py::class_(m, "ChainFkSolverVel_recursive") + .def(py::init()) + ; + + // -------------------- + // ChainIkSolverPos + // -------------------- + py::class_(m, "ChainIkSolverPos") + .def("CartToJnt", (int (ChainIkSolverPos::*)(const JntArray&, const Frame&, JntArray&)) &ChainIkSolverPos::CartToJnt, + py::arg("q_init"), py::arg("p_in"), py::arg("q_out")) + ; + + // -------------------- + // ChainIkSolverVel + // -------------------- + py::class_(m, "ChainIkSolverVel") + .def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const Twist&, JntArray&)) &ChainIkSolverVel::CartToJnt, + py::arg("q_in"), py::arg("v_in"), py::arg("qdot_out")) + .def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt, + py::arg("q_init"), py::arg("v_in"), py::arg("q_out")) + ; + + + // -------------------- + // ChainIkSolverPos_NR + // -------------------- + py::class_(m, "ChainIkSolverPos_NR") + .def(py::init(), + py::arg("chain"), py::arg("fksolver"), py::arg("iksolver"), + py::arg("maxiter")=100, py::arg("eps")=epsilon) + ; + + // --------------------------- + // ChainIkSolverPos_NR_JL + // --------------------------- + py::class_(m, "ChainIkSolverPos_NR_JL") + .def(py::init(), + py::arg("chain"), py::arg("q_min"), py::arg("q_max"), py::arg("fksolver"), py::arg("iksolver"), + py::arg("maxiter")=100, py::arg("eps")=epsilon) + ; + + // ------------------------- + // ChainIkSolverVel_pinv + // ------------------------- + py::class_(m, "ChainIkSolverVel_pinv") + .def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150) + ; + + // ------------------------- + // ChainIkSolverVel_wdls + // ------------------------- + py::class_(m, "ChainIkSolverVel_wdls") + .def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150) + .def("setWeightTS", &ChainIkSolverVel_wdls::setWeightTS) + .def("setWeightJS", &ChainIkSolverVel_wdls::setWeightJS) + .def("setLambda", &ChainIkSolverVel_wdls::setLambda) + .def("setEps", &ChainIkSolverVel_wdls::setEps) + .def("setMaxIter", &ChainIkSolverVel_wdls::setMaxIter) + .def("getNrZeroSigmas", &ChainIkSolverVel_wdls::getNrZeroSigmas) + .def("getSigmaMin", &ChainIkSolverVel_wdls::getSigmaMin) + .def("getSigma", &ChainIkSolverVel_wdls::getSigma) + .def("getEps", &ChainIkSolverVel_wdls::getEps) + .def("getLambda", &ChainIkSolverVel_wdls::getLambda) + .def("getLambdaScaled", &ChainIkSolverVel_wdls::getLambdaScaled) + .def("getSVDResult", &ChainIkSolverVel_wdls::getSVDResult) + ; + + // ------------------------- + // ChainIkSolverPos_LMA + // ------------------------- + py::class_(m, "ChainIkSolverPos_LMA") + .def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=500, py::arg("eps_joints")=0.000000000000001) + ; + + // ------------------------- + // ChainIkSolverVel_pinv_nso + // ------------------------- + py::class_(m, "ChainIkSolverVel_pinv_nso") + .def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150, py::arg("alpha")=0.25) + .def("setWeights", &ChainIkSolverVel_pinv_nso::setWeights) + .def("setOptPos", &ChainIkSolverVel_pinv_nso::setOptPos) + .def("setAlpha", &ChainIkSolverVel_pinv_nso::setAlpha) + .def("getWeights", &ChainIkSolverVel_pinv_nso::getWeights) + .def("getOptPos", &ChainIkSolverVel_pinv_nso::getOptPos) + .def("getAlpha", &ChainIkSolverVel_pinv_nso::getAlpha) + ; + + // ------------------------------ + // ChainIkSolverVel_pinv_givens + // ------------------------------ + py::class_(m, "ChainIkSolverVel_pinv_givens") + .def(py::init()) + ; + + // ------------------------------ + // ChainJntToJacSolver + // ------------------------------ + py::class_(m, "ChainJntToJacSolver") + .def(py::init()) + .def("JntToJac", &ChainJntToJacSolver::JntToJac, + py::arg("q_in"), py::arg("jac"), py::arg("segmentNR")=-1) + .def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints) + ; + + // ------------------------------ + // ChainIdSolver + // ------------------------------ + py::class_(m, "ChainIdSolver") + .def("CartToJnt", &ChainIdSolver::CartToJnt) + ; + + // ------------------------------ + // ChainIdSolver_RNE + // ------------------------------ + py::class_(m, "ChainIdSolver_RNE") + .def(py::init()) + ; +} From d6ef153bc74b98bc3451f0dbd41f8b771bc15b35 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 8 Feb 2020 00:07:54 +0100 Subject: [PATCH 002/136] Working CMake with PyBind11 --- .gitmodules | 4 +++ python_orocos_kdl/CMakeLists.txt | 46 +++++++++++++++++--------------- python_orocos_kdl/pybind11 | 1 + 3 files changed, 29 insertions(+), 22 deletions(-) create mode 100644 .gitmodules create mode 160000 python_orocos_kdl/pybind11 diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..36b541a9c --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "python_orocos_kdl/pybind11"] + path = python_orocos_kdl/pybind11 + url = https://github.com/pybind/pybind11.git + branch = v2.4.3 diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index d5af15734..b04c2bfdb 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -9,27 +9,31 @@ find_package(orocos_kdl REQUIRED) include_directories(${orocos_kdl_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) -option(BUILD_PYKDL_PYBIND11 OFF) -if (BUILD_PYKDL_PYBIND11) - find_package(pybind11 REQUIRED) - pybind11_add_module(PyKDL - PyKDL/PyKDL.h - PyKDL/PyKDL.cpp - PyKDL/frames.cpp - PyKDL/kinfam.cpp - PyKDL/framevel.cpp - PyKDL/dynamics.cpp) - target_link_libraries(PyKDL PRIVATE ${orocos_kdl_LIBRARIES}) +if(DEFINED ENV{ROS_PYTHON_VERSION}) + SET(PYTHON_VERSION $ENV{ROS_PYTHON_VERSION} CACHE STRING "Python Version") +else() + SET(PYTHON_VERSION 2 CACHE STRING "Python Version") +endif() + +find_package(PythonInterp ${PYTHON_VERSION} REQUIRED) +find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED) +execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) +set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) +set(LIBRARY_NAME "PyKDL") +option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" OFF) +if (BUILD_PYKDL_PYBIND11) + add_subdirectory(pybind11) + pybind11_add_module(${LIBRARY_NAME} + PyKDL/PyKDL.h + PyKDL/PyKDL.cpp + PyKDL/frames.cpp + PyKDL/kinfam.cpp + PyKDL/framevel.cpp + PyKDL/dynamics.cpp) + target_link_libraries(${LIBRARY_NAME} PRIVATE ${orocos_kdl_LIBRARIES}) + install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}.so" DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}") else (BUILD_PYKDL_PYBIND11) - if(DEFINED ENV{ROS_PYTHON_VERSION}) - SET(PYTHON_VERSION $ENV{ROS_PYTHON_VERSION} CACHE STRING "Python Version") - else() - SET(PYTHON_VERSION 2 CACHE STRING "Python Version") - endif() - find_package(PythonInterp ${PYTHON_VERSION} REQUIRED) - find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED) - execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(SIP REQUIRED) include(SIPMacros) @@ -39,11 +43,9 @@ else (BUILD_PYKDL_PYBIND11) file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.sip") set(SIP_INCLUDES ${SIP_FILES}) set(SIP_EXTRA_OPTIONS "-o") - set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) - add_sip_python_module(PyKDL PyKDL/PyKDL.sip ${orocos_kdl_LIBRARIES}) + add_sip_python_module(${LIBRARY_NAME} ${LIBRARY_NAME}/${LIBRARY_NAME}.sip ${orocos_kdl_LIBRARIES}) endif(BUILD_PYKDL_PYBIND11) - install(FILES package.xml DESTINATION share/python_orocos_kdl) find_package(catkin QUIET) diff --git a/python_orocos_kdl/pybind11 b/python_orocos_kdl/pybind11 new file mode 160000 index 000000000..80d452484 --- /dev/null +++ b/python_orocos_kdl/pybind11 @@ -0,0 +1 @@ +Subproject commit 80d452484c5409444b0ec19383faa84bb7a4d351 From aae2a1775f5cf4163d4523f70e37839fc6b1e685 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 8 Feb 2020 00:10:47 +0100 Subject: [PATCH 003/136] Test PyBind11 in travis --- .travis.yml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 83a7a5b30..9e3759020 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,8 +4,10 @@ compiler: - gcc - clang env: - - OROCOS_KDL_BUILD_TYPE=Debug - - OROCOS_KDL_BUILD_TYPE=Release + - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=OFF + - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=ON + - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF + - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=ON before_script: - sudo apt-get -qq update @@ -23,7 +25,7 @@ before_script: - cd python_orocos_kdl - mkdir build - cd build - - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} ./.. + - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./.. #compile and install python bindings - make - sudo make install From 4c4193eb9c60247d66e4d9973ec865da95c31f82 Mon Sep 17 00:00:00 2001 From: Zihan Chen Date: Fri, 7 Feb 2020 18:59:20 -0800 Subject: [PATCH 004/136] Added pickle support for frames and framevel --- python_orocos_kdl/PyKDL/frames.cpp | 69 +++++++++++++++++++++++++ python_orocos_kdl/PyKDL/framevel.cpp | 56 ++++++++++++++++++++ python_orocos_kdl/tests/framestest.py | 7 +-- python_orocos_kdl/tests/frameveltest.py | 4 +- 4 files changed, 131 insertions(+), 5 deletions(-) diff --git a/python_orocos_kdl/PyKDL/frames.cpp b/python_orocos_kdl/PyKDL/frames.cpp index 3c6350847..d366ad3ec 100644 --- a/python_orocos_kdl/PyKDL/frames.cpp +++ b/python_orocos_kdl/PyKDL/frames.cpp @@ -74,6 +74,21 @@ void init_frames(py::module &m) .def_static("Zero", &Vector::Zero) .def("Norm", &Vector::Norm) .def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon) + .def(py::pickle( + [](const Vector &v) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(v.x(), v.y(), v.z()); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 3) { + std::cout << "size error\n"; + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + Vector v(t[0].cast(), t[1].cast(), t[2].cast()); + return v; + })) ; m.def("SetToZero", (void (*)(Vector&)) &KDL::SetToZero); @@ -155,6 +170,18 @@ void init_frames(py::module &m) .def(py::self == py::self) .def(py::self != py::self) .def(py::self * py::self) + .def(py::pickle( + [](const Rotation &rot) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + double roll{0}, pitch{0}, yaw{0}; + rot.GetRPY(roll, pitch, yaw); + return py::make_tuple(roll, pitch, yaw); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 3) { throw std::runtime_error("Invalid state!"); } + /* Create a new C++ instance */ + return Rotation::RPY(t[0].cast(), t[1].cast(), t[2].cast()); + })) ; m.def("Equal", (bool (*)(const Rotation&, const Rotation&, double eps)) &KDL::Equal, @@ -211,6 +238,20 @@ void init_frames(py::module &m) .def(py::self * py::self) .def(py::self == py::self) .def(py::self != py::self) + .def(py::pickle( + [](const Frame &frm) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(frm.M, frm.p); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 2) { + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + Frame frm(t[0].cast(), t[1].cast()); + return frm; + })) ; m.def("Equal", (bool (*)(const Frame&, const Frame&, double eps)) &KDL::Equal, @@ -258,6 +299,20 @@ void init_frames(py::module &m) .def("__neg__", [](const Twist &a) { return operator-(a); }, py::is_operator()) + .def(py::pickle( + [](const Twist &tt) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tt.vel, tt.rot); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 2) { + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + Twist tt(t[0].cast(), t[1].cast()); + return tt; + })) ; m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot); @@ -308,6 +363,20 @@ void init_frames(py::module &m) .def("__neg__", [](const Wrench &w) { return operator-(w); }, py::is_operator()) + .def(py::pickle( + [](const Wrench &wr) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(wr.force, wr.torque); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 2) { + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + Wrench wr(t[0].cast(), t[1].cast()); + return wr; + })) ; m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero); diff --git a/python_orocos_kdl/PyKDL/framevel.cpp b/python_orocos_kdl/PyKDL/framevel.cpp index 9a4a46224..b30dc6464 100644 --- a/python_orocos_kdl/PyKDL/framevel.cpp +++ b/python_orocos_kdl/PyKDL/framevel.cpp @@ -80,6 +80,20 @@ void init_framevel(pybind11::module &m) .def("__neg__", [](const VectorVel &a) { return operator-(a); }, py::is_operator()) + .def(py::pickle( + [](const VectorVel &vv) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(vv.p, vv.v); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 2) { + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + VectorVel vv(t[0].cast(), t[1].cast()); + return vv; + })) ; m.def("Equal", (bool (*)(const VectorVel&, const VectorVel&, double)) &KDL::Equal, @@ -132,6 +146,20 @@ void init_framevel(pybind11::module &m) .def(py::self * py::self) .def(Rotation() * py::self) .def(py::self * Rotation()) + .def(py::pickle( + [](const RotationVel &rv) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(rv.R, rv.w); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 2) { + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + RotationVel rv(t[0].cast(), t[1].cast()); + return rv; + })) ; m.def("Equal", (bool (*)(const RotationVel&, const RotationVel&, double)) &KDL::Equal, @@ -170,6 +198,20 @@ void init_framevel(pybind11::module &m) .def(py::self * py::self) .def(Frame() * py::self) .def(py::self * Frame()) + .def(py::pickle( + [](const FrameVel &fv) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(fv.M, fv.p); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 2) { + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + FrameVel rv(t[0].cast(), t[1].cast()); + return rv; + })) ; m.def("Equal", (bool (*)(const FrameVel&, const FrameVel&, double)) &KDL::Equal, @@ -213,6 +255,20 @@ void init_framevel(pybind11::module &m) .def("__neg__", [](const TwistVel &a) { return operator-(a); }, py::is_operator()) + .def(py::pickle( + [](const TwistVel &tv) { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tv.vel, tv.rot); + }, + [](py::tuple t) { // __setstate__ + if (t.size() != 2) { + throw std::runtime_error("Invalid state!"); + } + + /* Create a new C++ instance */ + TwistVel tv(t[0].cast(), t[1].cast()); + return tv; + })) ; m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero); diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index de40b7505..267a87a30 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -160,16 +160,17 @@ def testFrame(self): self.assertEqual(F.Inverse()*v,F.Inverse(v)) def testPickle(self): - import pickle + # import pickle + import cPickle as pickle data = {} data['v'] = Vector(1,2,3) - data['rot'] = Rotation.RotX(1.3) + data['rot'] = Rotation().EulerZYZ(1, 2, 3) data['fr'] = Frame(data['rot'], data['v']) data['tw'] = Twist(data['v'], Vector(4,5,6)) data['wr'] = Wrench(Vector(0.1,0.2,0.3), data['v']) f = open('/tmp/pickle_test', 'w') - pickle.dump(data, f) + pickle.dump(data, f, 2) f.close() f = open('/tmp/pickle_test', 'r') diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index e93313a33..a7bb6267a 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -85,7 +85,7 @@ def testFrameVel(self): def testPickle(self): rot = Rotation.RotX(1.3) - import pickle + import cPickle as pickle data = {} data['vv'] = VectorVel(Vector(1,2,3), Vector(4,5,6)) data['rv'] = RotationVel(rot, Vector(4.1,5.1,6.1)) @@ -93,7 +93,7 @@ def testPickle(self): data['tv'] = TwistVel(data['vv'], data['vv']) f = open('/tmp/pickle_test_kdl_framevel', 'w') - pickle.dump(data, f) + pickle.dump(data, f, 2) f.close() f = open('/tmp/pickle_test_kdl_framevel', 'r') From cf757f8d38899119e1293e5ca6af365e4645d6d4 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 9 Feb 2020 15:20:34 +0100 Subject: [PATCH 005/136] Add description to default args This prevents docstrings like: __init__(self: PyKDL.Segment, name: unicode, joint: PyKDL.Joint = NoName:[None, axis: [ 0, 0, 0], origin[ 0, 0, 0]], f_tip: PyKDL.Frame = [[ 1, 0, 0; 0, 1, 0; 0, 0, 1] [ 0, 0, 0]], I: PyKDL.RigidBodyInertia = ) -> None Instead the docstring looks like: __init__(self: PyKDL.Segment, name: unicode, joint: PyKDL.Joint = Joint(), f_tip: PyKDL.Frame = Frame.Identity(), I: PyKDL.RigidBodyInertia = RigidBodyInertia.Zero()) -> None --- python_orocos_kdl/PyKDL/kinfam.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/kinfam.cpp index f883988fc..73b86b4ee 100644 --- a/python_orocos_kdl/PyKDL/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/kinfam.cpp @@ -123,7 +123,7 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_(m, "RigidBodyInertia") .def(py::init(), - py::arg("m")=0, py::arg("oc")=Vector::Zero(), py::arg("Ic")=RotationalInertia::Zero()) + py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero()"), py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero()")) .def_static("Zero", &RigidBodyInertia::Zero) .def("RefPoint", &RigidBodyInertia::RefPoint) .def("getMass", &RigidBodyInertia::getMass) @@ -141,9 +141,9 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_(m, "Segment") .def(py::init(), - py::arg("name"), py::arg("joint")=Joint(), py::arg("f_tip")=Frame::Identity(), py::arg("I")=RigidBodyInertia::Zero()) + py::arg("name"), py::arg_v("joint", Joint(), "Joint()"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity()"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero()")) .def(py::init(), - py::arg("joint")=Joint(), py::arg("f_tip")=Frame::Identity(), py::arg("I")=RigidBodyInertia::Zero()) + py::arg_v("joint", Joint(), "Joint()"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity()"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero()")) .def(py::init()) .def("getFrameToTip", &Segment::getFrameToTip) .def("pose", &Segment::pose) From ebacacb81e63c5af512977f8c6695ee7a949cb47 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 10 Feb 2020 15:11:13 +0100 Subject: [PATCH 006/136] Use install(TARGETS) for PyBind --- python_orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index b04c2bfdb..f48225af0 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -32,7 +32,7 @@ if (BUILD_PYKDL_PYBIND11) PyKDL/framevel.cpp PyKDL/dynamics.cpp) target_link_libraries(${LIBRARY_NAME} PRIVATE ${orocos_kdl_LIBRARIES}) - install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}.so" DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}") + install(TARGETS ${LIBRARY_NAME} DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}") else (BUILD_PYKDL_PYBIND11) list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(SIP REQUIRED) From 2d75ad045102f52510cf82aa96e17cb5ddeaf153 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 10 Feb 2020 15:30:52 +0100 Subject: [PATCH 007/136] (travis) specify os and dist --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index 9e3759020..2558356da 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,6 @@ language: cpp +os: linux +dist: xenial compiler: - gcc From c9249d48fd441437f174fe467fd0e120627a8297 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 14 Feb 2020 20:25:48 +0100 Subject: [PATCH 008/136] Move pybind11 and sip into seperate folders --- python_orocos_kdl/CMakeLists.txt | 14 +++++++------- python_orocos_kdl/PyKDL/{ => pybind11}/PyKDL.cpp | 0 python_orocos_kdl/PyKDL/{ => pybind11}/PyKDL.h | 0 .../PyKDL/{ => pybind11}/dynamics.cpp | 0 python_orocos_kdl/PyKDL/{ => pybind11}/frames.cpp | 0 .../PyKDL/{ => pybind11}/framevel.cpp | 0 python_orocos_kdl/PyKDL/{ => pybind11}/kinfam.cpp | 0 python_orocos_kdl/PyKDL/{ => sip}/PyKDL.sip | 0 python_orocos_kdl/PyKDL/{ => sip}/dynamics.sip | 0 python_orocos_kdl/PyKDL/{ => sip}/frames.sip | 0 python_orocos_kdl/PyKDL/{ => sip}/framevel.sip | 0 python_orocos_kdl/PyKDL/{ => sip}/kinfam.sip | 0 python_orocos_kdl/PyKDL/{ => sip}/std_string.sip | 0 13 files changed, 7 insertions(+), 7 deletions(-) rename python_orocos_kdl/PyKDL/{ => pybind11}/PyKDL.cpp (100%) rename python_orocos_kdl/PyKDL/{ => pybind11}/PyKDL.h (100%) rename python_orocos_kdl/PyKDL/{ => pybind11}/dynamics.cpp (100%) rename python_orocos_kdl/PyKDL/{ => pybind11}/frames.cpp (100%) rename python_orocos_kdl/PyKDL/{ => pybind11}/framevel.cpp (100%) rename python_orocos_kdl/PyKDL/{ => pybind11}/kinfam.cpp (100%) rename python_orocos_kdl/PyKDL/{ => sip}/PyKDL.sip (100%) rename python_orocos_kdl/PyKDL/{ => sip}/dynamics.sip (100%) rename python_orocos_kdl/PyKDL/{ => sip}/frames.sip (100%) rename python_orocos_kdl/PyKDL/{ => sip}/framevel.sip (100%) rename python_orocos_kdl/PyKDL/{ => sip}/kinfam.sip (100%) rename python_orocos_kdl/PyKDL/{ => sip}/std_string.sip (100%) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index f48225af0..91911ede7 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -25,12 +25,12 @@ option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" OFF) if (BUILD_PYKDL_PYBIND11) add_subdirectory(pybind11) pybind11_add_module(${LIBRARY_NAME} - PyKDL/PyKDL.h - PyKDL/PyKDL.cpp - PyKDL/frames.cpp - PyKDL/kinfam.cpp - PyKDL/framevel.cpp - PyKDL/dynamics.cpp) + PyKDL/pybind11/PyKDL.h + PyKDL/pybind11/PyKDL.cpp + PyKDL/pybind11/frames.cpp + PyKDL/pybind11/kinfam.cpp + PyKDL/pybind11/framevel.cpp + PyKDL/pybind11/dynamics.cpp) target_link_libraries(${LIBRARY_NAME} PRIVATE ${orocos_kdl_LIBRARIES}) install(TARGETS ${LIBRARY_NAME} DESTINATION "${PYTHON_SITE_PACKAGES_INSTALL_DIR}") else (BUILD_PYKDL_PYBIND11) @@ -43,7 +43,7 @@ else (BUILD_PYKDL_PYBIND11) file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.sip") set(SIP_INCLUDES ${SIP_FILES}) set(SIP_EXTRA_OPTIONS "-o") - add_sip_python_module(${LIBRARY_NAME} ${LIBRARY_NAME}/${LIBRARY_NAME}.sip ${orocos_kdl_LIBRARIES}) + add_sip_python_module(${LIBRARY_NAME} ${LIBRARY_NAME}/sip/${LIBRARY_NAME}.sip ${orocos_kdl_LIBRARIES}) endif(BUILD_PYKDL_PYBIND11) install(FILES package.xml DESTINATION share/python_orocos_kdl) diff --git a/python_orocos_kdl/PyKDL/PyKDL.cpp b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp similarity index 100% rename from python_orocos_kdl/PyKDL/PyKDL.cpp rename to python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp diff --git a/python_orocos_kdl/PyKDL/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h similarity index 100% rename from python_orocos_kdl/PyKDL/PyKDL.h rename to python_orocos_kdl/PyKDL/pybind11/PyKDL.h diff --git a/python_orocos_kdl/PyKDL/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp similarity index 100% rename from python_orocos_kdl/PyKDL/dynamics.cpp rename to python_orocos_kdl/PyKDL/pybind11/dynamics.cpp diff --git a/python_orocos_kdl/PyKDL/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp similarity index 100% rename from python_orocos_kdl/PyKDL/frames.cpp rename to python_orocos_kdl/PyKDL/pybind11/frames.cpp diff --git a/python_orocos_kdl/PyKDL/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp similarity index 100% rename from python_orocos_kdl/PyKDL/framevel.cpp rename to python_orocos_kdl/PyKDL/pybind11/framevel.cpp diff --git a/python_orocos_kdl/PyKDL/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp similarity index 100% rename from python_orocos_kdl/PyKDL/kinfam.cpp rename to python_orocos_kdl/PyKDL/pybind11/kinfam.cpp diff --git a/python_orocos_kdl/PyKDL/PyKDL.sip b/python_orocos_kdl/PyKDL/sip/PyKDL.sip similarity index 100% rename from python_orocos_kdl/PyKDL/PyKDL.sip rename to python_orocos_kdl/PyKDL/sip/PyKDL.sip diff --git a/python_orocos_kdl/PyKDL/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip similarity index 100% rename from python_orocos_kdl/PyKDL/dynamics.sip rename to python_orocos_kdl/PyKDL/sip/dynamics.sip diff --git a/python_orocos_kdl/PyKDL/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip similarity index 100% rename from python_orocos_kdl/PyKDL/frames.sip rename to python_orocos_kdl/PyKDL/sip/frames.sip diff --git a/python_orocos_kdl/PyKDL/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip similarity index 100% rename from python_orocos_kdl/PyKDL/framevel.sip rename to python_orocos_kdl/PyKDL/sip/framevel.sip diff --git a/python_orocos_kdl/PyKDL/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip similarity index 100% rename from python_orocos_kdl/PyKDL/kinfam.sip rename to python_orocos_kdl/PyKDL/sip/kinfam.sip diff --git a/python_orocos_kdl/PyKDL/std_string.sip b/python_orocos_kdl/PyKDL/sip/std_string.sip similarity index 100% rename from python_orocos_kdl/PyKDL/std_string.sip rename to python_orocos_kdl/PyKDL/sip/std_string.sip From 09acd10e418e3c75b30b4b9c661189dea5aa14b0 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 14 Feb 2020 20:31:08 +0100 Subject: [PATCH 009/136] Depend on all SIP includes, to trigger a rebuild if any of the sip files changed --- python_orocos_kdl/CMakeLists.txt | 2 +- python_orocos_kdl/cmake/SIPMacros.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 91911ede7..dcf44d64a 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -40,7 +40,7 @@ else (BUILD_PYKDL_PYBIND11) include_directories(${SIP_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS}) - file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/*.sip") + file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/${LIBRARY_NAME}/sip/*.sip") set(SIP_INCLUDES ${SIP_FILES}) set(SIP_EXTRA_OPTIONS "-o") add_sip_python_module(${LIBRARY_NAME} ${LIBRARY_NAME}/sip/${LIBRARY_NAME}.sip ${orocos_kdl_LIBRARIES}) diff --git a/python_orocos_kdl/cmake/SIPMacros.cmake b/python_orocos_kdl/cmake/SIPMacros.cmake index e2f89ef2a..5e560148b 100644 --- a/python_orocos_kdl/cmake/SIPMacros.cmake +++ b/python_orocos_kdl/cmake/SIPMacros.cmake @@ -97,7 +97,7 @@ MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP) COMMAND ${CMAKE_COMMAND} -E echo ${message} COMMAND ${CMAKE_COMMAND} -E touch ${_sip_output_files} COMMAND ${SIP_EXECUTABLE} ${_sip_tags} ${_sip_x} ${SIP_EXTRA_OPTIONS} -j ${SIP_CONCAT_PARTS} -c ${CMAKE_CURRENT_SIP_OUTPUT_DIR} ${_sip_includes} ${_abs_module_sip} - DEPENDS ${_abs_module_sip} ${SIP_EXTRA_FILES_DEPEND} + DEPENDS ${SIP_INCLUDES} ${SIP_EXTRA_FILES_DEPEND} ) # not sure if type MODULE could be uses anywhere, limit to cygwin for now IF (CYGWIN OR APPLE) From c5c7a616e891ee80a27958df00b2aa05346e552c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 14 Feb 2020 20:35:08 +0100 Subject: [PATCH 010/136] Whitespace fixes in SIP files --- python_orocos_kdl/PyKDL/sip/dynamics.sip | 6 +-- python_orocos_kdl/PyKDL/sip/frames.sip | 40 ++++++++-------- python_orocos_kdl/PyKDL/sip/framevel.sip | 22 ++++----- python_orocos_kdl/PyKDL/sip/kinfam.sip | 58 ++++++++++++------------ python_orocos_kdl/cmake/SIPMacros.cmake | 2 +- 5 files changed, 64 insertions(+), 64 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index e0096ddc8..43bc6dc92 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -60,7 +60,7 @@ bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,d bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); class ChainDynParam { - + %TypeHeaderCode #include using namespace KDL; @@ -70,6 +70,6 @@ public: ChainDynParam(const Chain& chain, Vector _grav); int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis); - int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); - int JntToGravity(const JntArray &q,JntArray &gravity); + int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); + int JntToGravity(const JntArray &q,JntArray &gravity); }; diff --git a/python_orocos_kdl/PyKDL/sip/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip index 84302a674..f437479e3 100644 --- a/python_orocos_kdl/PyKDL/sip/frames.sip +++ b/python_orocos_kdl/PyKDL/sip/frames.sip @@ -45,8 +45,8 @@ public: double __getitem__ (int index) const; %MethodCode if (a0 < 0 || a0 > 2) { - PyErr_SetString(PyExc_IndexError, "Vector index out of range"); - return 0; + PyErr_SetString(PyExc_IndexError, "Vector index out of range"); + return 0; } sipRes=(*sipCpp)(a0); %End @@ -111,7 +111,7 @@ public: double Xy,double Yy,double Zy, double Xz,double Yz,double Zz); Rotation(const Vector& x,const Vector& y,const Vector& z); - + double __getitem__(SIP_PYTUPLE) const; %MethodCode int i,j; @@ -141,13 +141,13 @@ public: std::string s(oss.str()); sipRes=&s; %End - + void SetInverse(); Rotation Inverse() const /Factory/; Vector Inverse(const Vector& v) const /Factory/; Wrench Inverse(const Wrench& w) const /Factory/; Twist Inverse(const Twist& t) const /Factory/; - + static Rotation Identity()/Factory/; static Rotation RotX(double angle)/Factory/; static Rotation RotY(double angle)/Factory/; @@ -158,12 +158,12 @@ public: static Rotation RPY(double roll,double pitch,double yaw)/Factory/; static Rotation EulerZYX(double Alfa,double Beta,double Gamma)/Factory/; static Rotation Quaternion(double x, double y, double z, double w)/Factory/; - + void DoRotX(double angle); void DoRotY(double angle); void DoRotZ(double angle); - + Vector GetRot() const /Factory/; double GetRotAngle(Vector& axis /Out/,double eps=epsilon) const; void GetEulerZYZ(double& alfa /Out/,double& beta /Out/,double& gamma /Out/) const; @@ -171,11 +171,11 @@ public: void GetEulerZYX(double& Alfa /Out/,double& Beta /Out/,double& Gamma /Out/) const; void GetQuaternion(double& x /Out/,double& y /Out/,double& z /Out/, double& w) const; - + Vector operator*(const Vector& v) const /Numeric,Factory/; Twist operator*(const Twist& arg) const /Numeric,Factory/; Wrench operator*(const Wrench& arg) const /Numeric,Factory/; - + Vector UnitX() const /Factory/; Vector UnitY() const /Factory/; Vector UnitZ() const /Factory/; @@ -209,10 +209,10 @@ public: Frame(const Vector& V); Frame(const Rotation& R); Frame(); - + Vector p; Rotation M; - + double __getitem__ (SIP_PYTUPLE) const; %MethodCode int i,j; @@ -256,7 +256,7 @@ public: Vector operator*(const Vector& arg) const /Numeric,Factory/; Wrench operator * (const Wrench& arg) const /Numeric,Factory/; Twist operator * (const Twist& arg) const /Numeric,Factory/; - + static Frame Identity() /Factory/; void Integrate(const Twist& t_this,double frequency); @@ -273,7 +273,7 @@ bool Equal(const Frame& a,const Frame& b,double eps=epsilon); bool operator==(const Frame& a,const Frame& b); bool operator!=(const Frame& a,const Frame& b); -class Twist +class Twist { %TypeHeaderCode @@ -285,10 +285,10 @@ using namespace KDL; public: Vector vel; Vector rot; - + Twist(); Twist(const Vector& _vel,const Vector& _rot); - + Twist& operator-=(const Twist& arg); Twist& operator+=(const Twist& arg); @@ -320,9 +320,9 @@ public: static Twist Zero() /Factory/; void ReverseSign(); - + Twist RefPoint(const Vector& v_base_AB) const /Factory/; - + %PickleCode const sipTypeDef *vector_type = sipFindType("Vector"); sipRes = Py_BuildValue("OO", sipConvertFromType(&(sipCpp->vel), vector_type, Py_None), @@ -346,7 +346,7 @@ bool operator!=(const Twist& a,const Twist& b); class Wrench { - + %TypeHeaderCode #include #include @@ -357,10 +357,10 @@ using namespace KDL; public: Vector force; Vector torque; - + Wrench(); Wrench(const Vector& force,const Vector& torque); - + Wrench& operator-=(const Wrench& arg); Wrench& operator+=(const Wrench& arg); diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 110d8344a..953b9b14f 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -21,7 +21,7 @@ class doubleVel { - + %TypeHeaderCode #include using namespace KDL; @@ -46,8 +46,8 @@ using namespace KDL; %End public: Vector p; - Vector v; - + Vector v; + VectorVel(); VectorVel(const Vector& _p,const Vector& _v); VectorVel(const Vector& _p); @@ -113,9 +113,9 @@ public: Vector deriv() const; - VectorVel UnitX() const; - VectorVel UnitY() const; - VectorVel UnitZ() const; + VectorVel UnitX() const; + VectorVel UnitY() const; + VectorVel UnitZ() const; static RotationVel Identity(); RotationVel Inverse() const; VectorVel Inverse(const VectorVel& arg) const; @@ -126,7 +126,7 @@ public: void DoRotY(const doubleVel& angle); void DoRotZ(const doubleVel& angle); static RotationVel RotX(const doubleVel& angle); - static RotationVel RotY(const doubleVel& angle); + static RotationVel RotY(const doubleVel& angle); static RotationVel RotZ(const doubleVel& angle); static RotationVel Rot(const Vector& rotvec,const doubleVel& angle); static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle); @@ -155,7 +155,7 @@ bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); class FrameVel { - + %TypeHeaderCode #include using namespace KDL; @@ -169,7 +169,7 @@ public: FrameVel(const Frame& _T); FrameVel(const Frame& _T,const Twist& _t); FrameVel(const RotationVel& _M,const VectorVel& _p); - + Frame value() const; Twist deriv() const; @@ -207,7 +207,7 @@ class TwistVel %TypeHeaderCode #include using namespace KDL; -%End +%End public: VectorVel vel; @@ -224,7 +224,7 @@ public: TwistVel& operator+=(const TwistVel& arg); static TwistVel Zero(); - + void ReverseSign(); TwistVel RefPoint(const VectorVel& v_base_AB); diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index f66eba8ef..cd080d50f 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -121,12 +121,12 @@ public: Joint(JointType type=None,double scale=1,double offset=0, double inertia=0,const double damping=0,double stiffness=0); Joint(std::string name, Vector origin, Vector axis, JointType type, double scale=1, double offset=0, - double inertia=0, double damping=0, double stiffness=0); + double inertia=0, double damping=0, double stiffness=0); Joint(Vector origin, Vector axis, JointType type, double scale=1, double offset=0, - double inertia=0, double damping=0, double stiffness=0); - + double inertia=0, double damping=0, double stiffness=0); + Joint(const Joint& in); - + Frame pose(const double& q)const /Factory/ ; Twist twist(const double& qdot)const /Factory/ ; Vector JointAxis() const /Factory/; @@ -156,7 +156,7 @@ using namespace KDL; %End public: RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); - + static RotationalInertia Zero()/Factory/; double __getitem__(int index); @@ -192,9 +192,9 @@ using namespace KDL; %End public: RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); - static RigidBodyInertia Zero() /Factory/; + static RigidBodyInertia Zero() /Factory/; RigidBodyInertia RefPoint(const Vector& p) /Factory/; - double getMass()const /Factory/; + double getMass()const /Factory/; Vector getCOG() const /Factory/; RotationalInertia getRotationalInertia() const /Factory/; }; @@ -206,7 +206,7 @@ RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I) /Factory class Segment { - + %TypeHeaderCode #include #include @@ -224,7 +224,7 @@ public: std::string s(ss.str()); sipRes=&s; %End - + const Frame& getFrameToTip()const /Factory/; Frame pose(const double& q)const /Factory/ ; Twist twist(const double& q,const double& qdot)const /Factory/ ; @@ -236,7 +236,7 @@ public: class Chain { - + %TypeHeaderCode #include using namespace KDL; @@ -245,13 +245,13 @@ using namespace KDL; public: Chain(); Chain(const Chain& in); - + void addSegment(const Segment& segment); void addChain(const Chain& chain); - + unsigned int getNrOfJoints()const; unsigned int getNrOfSegments()const; - + const Segment& getSegment(unsigned int nr)const /Factory/; }; @@ -296,7 +296,7 @@ public: } sipRes=(*sipCpp)(a0); %End - + void __setitem__(int index, double value); %MethodCode if (a0 < 0 || a0 >= (int)sipCpp->rows()) { @@ -339,7 +339,7 @@ public: JntArrayVel(const JntArray& q,const JntArray& qdot); JntArrayVel(const JntArray& q); void resize(unsigned int newSize); - + JntArray value()const /Factory/; JntArray deriv()const /Factory/; }; @@ -378,7 +378,7 @@ public: } sipRes=(*sipCpp)(i,j); %End - + void __setitem__(SIP_PYTUPLE,double value); %MethodCode int i,j; @@ -399,14 +399,14 @@ public: %End Twist getColumn(unsigned int i) const /Factory/; void setColumn(unsigned int i,const Twist& t); - + void changeRefPoint(const Vector& base_AB); void changeBase(const Rotation& rot); void changeRefFrame(const Frame& frame); }; void SetToZero(Jacobian& jac); - + void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); @@ -497,7 +497,7 @@ using namespace KDL; public: ChainIkSolverPos_NR(const Chain& chain,ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver, unsigned int maxiter=100,double eps=epsilon); - + virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); virtual void updateInternalDataStructures(); }; @@ -509,10 +509,10 @@ class ChainIkSolverPos_NR_JL : ChainIkSolverPos using namespace KDL; %End public: - ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max, + ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray &q_min,const JntArray &q_max, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver, unsigned int maxiter=100,double eps=epsilon); - + virtual int CartToJnt(const JntArray& q_init , const Frame& p_in ,JntArray& q_out); virtual void updateInternalDataStructures(); }; @@ -525,7 +525,7 @@ using namespace KDL; %End public: ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); - + virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); }; @@ -538,7 +538,7 @@ using namespace KDL; %End public: ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150); - + virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); virtual void updateInternalDataStructures(); @@ -562,7 +562,7 @@ public: } Eigen::MatrixXd Mx; Mx=Eigen::MatrixXd::Identity(numRows,numCols); - + for (Py_ssize_t r=0;r& f_ext,JntArray &torques); - virtual void updateInternalDataStructures(); + ChainIdSolver_RNE(const Chain& chain,Vector grav); + int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const std::vector& f_ext,JntArray &torques); + virtual void updateInternalDataStructures(); }; diff --git a/python_orocos_kdl/cmake/SIPMacros.cmake b/python_orocos_kdl/cmake/SIPMacros.cmake index 5e560148b..836387ad3 100644 --- a/python_orocos_kdl/cmake/SIPMacros.cmake +++ b/python_orocos_kdl/cmake/SIPMacros.cmake @@ -93,7 +93,7 @@ MACRO(ADD_SIP_PYTHON_MODULE MODULE_NAME MODULE_SIP) ENDFOREACH(CONCAT_NUM RANGE 0 ${SIP_CONCAT_PARTS} ) ADD_CUSTOM_COMMAND( - OUTPUT ${_sip_output_files} + OUTPUT ${_sip_output_files} COMMAND ${CMAKE_COMMAND} -E echo ${message} COMMAND ${CMAKE_COMMAND} -E touch ${_sip_output_files} COMMAND ${SIP_EXECUTABLE} ${_sip_tags} ${_sip_x} ${SIP_EXTRA_OPTIONS} -j ${SIP_CONCAT_PARTS} -c ${CMAKE_CURRENT_SIP_OUTPUT_DIR} ${_sip_includes} ${_abs_module_sip} From f9c182204b7cae367d11ce59add3fcd7ac1efaf9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 15 Feb 2020 10:12:24 +0100 Subject: [PATCH 011/136] Use PyBind11 by default --- python_orocos_kdl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index dcf44d64a..2f9b5926d 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -21,7 +21,7 @@ execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) set(LIBRARY_NAME "PyKDL") -option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" OFF) +option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" ON) if (BUILD_PYKDL_PYBIND11) add_subdirectory(pybind11) pybind11_add_module(${LIBRARY_NAME} From 1f0e3e0349a323cf212e04fdc8c2955a66efc019 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 11:03:48 +0100 Subject: [PATCH 012/136] Remove double line --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index d366ad3ec..fb130ea9f 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -70,7 +70,6 @@ void init_frames(py::module &m) .def("__neg__", [](const Vector &a) { return operator-(a); }, py::is_operator()) - .def("x", (double (Vector::*)(void) const) &Vector::x, "Get x") .def_static("Zero", &Vector::Zero) .def("Norm", &Vector::Norm) .def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon) From 931b4e204641250a949ccf83429274122f8c42c6 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 11:04:57 +0100 Subject: [PATCH 013/136] remove bracket from default value descriptions --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index 73b86b4ee..4b7b25edc 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -123,7 +123,7 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_(m, "RigidBodyInertia") .def(py::init(), - py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero()"), py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero()")) + py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero"), py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero")) .def_static("Zero", &RigidBodyInertia::Zero) .def("RefPoint", &RigidBodyInertia::RefPoint) .def("getMass", &RigidBodyInertia::getMass) @@ -141,9 +141,9 @@ void init_kinfam(pybind11::module &m) // -------------------- py::class_(m, "Segment") .def(py::init(), - py::arg("name"), py::arg_v("joint", Joint(), "Joint()"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity()"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero()")) + py::arg("name"), py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")) .def(py::init(), - py::arg_v("joint", Joint(), "Joint()"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity()"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero()")) + py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")) .def(py::init()) .def("getFrameToTip", &Segment::getFrameToTip) .def("pose", &Segment::pose) From 8b42269eeca469dccace2b9436b07476e3a49617 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 11:38:39 +0100 Subject: [PATCH 014/136] Add pybind11 stl container support --- python_orocos_kdl/PyKDL/pybind11/PyKDL.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h index 7fdcc6f2b..0d1c056d2 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h @@ -23,6 +23,7 @@ #include #include +#include void init_frames(pybind11::module &m); void init_kinfam(pybind11::module &m); From ce174ed0b8cdb93135c366886d3f34fd96d6bd9b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 11:39:13 +0100 Subject: [PATCH 015/136] Remove get(Root)Segment of Tree from PyKDL --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index 4b7b25edc..ece636860 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -178,8 +178,6 @@ void init_kinfam(pybind11::module &m) .def("addTree", &Tree::addTree) .def("getNrOfJoints", &Tree::getNrOfJoints) .def("getNrOfSegments", &Tree::getNrOfSegments) - .def("getSegment", &Tree::getSegment) - .def("getRootSegment", &Tree::getRootSegment) .def("getChain", [](const Tree &tree, const std::string& chain_root, const std::string& chain_tip) { Chain* chain = new Chain(); tree.getChain(chain_root, chain_tip, *chain); From a0497c7de9bf2a16029b955505bbf9ca2750147a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 17:21:10 +0100 Subject: [PATCH 016/136] (PyKDL) use classnames in declerations --- python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp | 1 + python_orocos_kdl/PyKDL/pybind11/PyKDL.h | 1 + python_orocos_kdl/PyKDL/pybind11/dynamics.cpp | 66 +- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 562 +++++++++--------- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 323 +++++----- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 504 ++++++++-------- 6 files changed, 752 insertions(+), 705 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp index 79a8f2319..7524e4691 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp @@ -26,6 +26,7 @@ namespace py = pybind11; + PYBIND11_MODULE(PyKDL, m) { m.doc() = "Orocos KDL Python wrapper"; // optional module docstring diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h index 0d1c056d2..db1ca3785 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h @@ -25,6 +25,7 @@ #include #include + void init_frames(pybind11::module &m); void init_kinfam(pybind11::module &m); void init_framevel(pybind11::module &m); diff --git a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp index 095e04a68..5bb0afa3c 100644 --- a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp @@ -36,33 +36,36 @@ void init_dynamics(pybind11::module &m) // -------------------- // JntSpaceInertiaMatrix // -------------------- - py::class_(m, "JntSpaceInertiaMatrix") - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def("resize", &JntSpaceInertiaMatrix::resize) - .def("rows", &JntSpaceInertiaMatrix::rows) - .def("columns", &JntSpaceInertiaMatrix::columns) - .def("__getitem__", [](const JntSpaceInertiaMatrix &jm, std::tuple idx) { - int i = std::get<0>(idx); - int j = std::get<1>(idx); - if (i < 0 || i > jm.rows() || j < 0 || j > jm.columns()) { - throw py::index_error("Inertia index out of range"); - } - return jm((unsigned int)i, (unsigned int)j); - }) - .def("__repr__", [](const JntSpaceInertiaMatrix &jm) { - std::ostringstream oss; - for (size_t r = 0; r < jm.rows(); r++) { - for (size_t c = 0; c < jm.columns(); c++) { - oss << std::setw(KDL_FRAME_WIDTH) << jm(r, c); - } - oss << std::endl; + py::class_ jnt_space_inertia_matrix(m, "JntSpaceInertiaMatrix"); + jnt_space_inertia_matrix.def(py::init<>()); + jnt_space_inertia_matrix.def(py::init()); + jnt_space_inertia_matrix.def(py::init()); + jnt_space_inertia_matrix.def("resize", &JntSpaceInertiaMatrix::resize); + jnt_space_inertia_matrix.def("rows", &JntSpaceInertiaMatrix::rows); + jnt_space_inertia_matrix.def("columns", &JntSpaceInertiaMatrix::columns); + jnt_space_inertia_matrix.def("__getitem__", [](const JntSpaceInertiaMatrix &jm, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > jm.rows() || j < 0 || j > jm.columns()) + throw py::index_error("Inertia index out of range"); + + return jm((unsigned int)i, (unsigned int)j); + }); + jnt_space_inertia_matrix.def("__repr__", [](const JntSpaceInertiaMatrix &jm) + { + std::ostringstream oss; + for (size_t r = 0; r < jm.rows(); r++) + { + for (size_t c = 0; c < jm.columns(); c++) + { + oss << std::setw(KDL_FRAME_WIDTH) << jm(r, c); } - return oss.str(); - }) - .def(py::self == py::self) - ; + oss << std::endl; + } + return oss.str(); + }); + jnt_space_inertia_matrix.def(py::self == py::self); m.def("Add", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&, JntSpaceInertiaMatrix&)) &KDL::Add); m.def("Subtract", (void (*)(const JntSpaceInertiaMatrix&, const JntSpaceInertiaMatrix&,JntSpaceInertiaMatrix&)) &KDL::Subtract); @@ -77,10 +80,9 @@ void init_dynamics(pybind11::module &m) // -------------------- // ChainDynParam // -------------------- - py::class_(m, "ChainDynParam") - .def(py::init()) - .def("JntToCoriolis", &ChainDynParam::JntToCoriolis) - .def("JntToMass", &ChainDynParam::JntToMass) - .def("JntToGravity", &ChainDynParam::JntToGravity) - ; + py::class_ chain_dyn_param(m, "ChainDynParam"); + chain_dyn_param.def(py::init()); + chain_dyn_param.def("JntToCoriolis", &ChainDynParam::JntToCoriolis); + chain_dyn_param.def("JntToMass", &ChainDynParam::JntToMass); + chain_dyn_param.def("JntToGravity", &ChainDynParam::JntToGravity); } diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index fb130ea9f..7bed44a1f 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -28,67 +28,75 @@ namespace py = pybind11; using namespace KDL; + void init_frames(py::module &m) { // -------------------- // Vector // -------------------- - py::class_(m, "Vector") - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def("x", (void (Vector::*)(double)) &Vector::x, "Set x") - .def("y", (void (Vector::*)(double)) &Vector::y, "Set y") - .def("z", (void (Vector::*)(double)) &Vector::z, "Set z") - .def("x", (double (Vector::*)(void) const) &Vector::x, "Get x") - .def("y", (double (Vector::*)(void) const) &Vector::y, "Get y") - .def("z", (double (Vector::*)(void) const) &Vector::z, "Get z") - .def("__getitem__", [](const Vector &v, int i) { - if (i < 0 || i > 2) throw py::index_error("Vector index out of range"); - return v(i); - }) - .def("__setitem__", [](Vector &v, int i, double value) { - if (i < 0 || i > 2) throw py::index_error("Vector index out of range"); - v(i) = value; - }) - .def("__repr__", [](const Vector &v) { - std::ostringstream oss; - oss< vector(m, "Vector"); + vector.def(py::init<>()); + vector.def(py::init()); + vector.def(py::init()); + vector.def("x", (void (Vector::*)(double)) &Vector::x); + vector.def("y", (void (Vector::*)(double)) &Vector::y); + vector.def("z", (void (Vector::*)(double)) &Vector::z); + vector.def("x", (double (Vector::*)(void) const) &Vector::x); + vector.def("y", (double (Vector::*)(void) const) &Vector::y); + vector.def("z", (double (Vector::*)(void) const) &Vector::z); + vector.def("__getitem__", [](const Vector &v, int i) + { + if (i < 0 || i > 2) + throw py::index_error("Vector index out of range"); + + return v(i); + }); + vector.def("__setitem__", [](Vector &v, int i, double value) + { + if (i < 0 || i > 2) + throw py::index_error("Vector index out of range"); + + v(i) = value; + }); + vector.def("__repr__", [](const Vector &v) + { + std::ostringstream oss; + oss << v; + return oss.str(); + }); + vector.def("ReverseSign", &Vector::ReverseSign); + vector.def(py::self -= py::self); + vector.def(py::self += py::self); + vector.def(py::self + py::self); + vector.def(py::self - py::self); + vector.def(py::self * double()); + vector.def(double() * py::self); + vector.def(py::self / double()); + vector.def(py::self * py::self); + vector.def(py::self == py::self); + vector.def(py::self != py::self); + vector.def("__neg__", [](const Vector &a) + { + return operator-(a); + }, py::is_operator()); + vector.def_static("Zero", &Vector::Zero); + vector.def("Norm", &Vector::Norm); + vector.def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon); + vector.def(py::pickle( + [](const Vector &v) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(v.x(), v.y(), v.z()); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 3) { - std::cout << "size error\n"; + [](py::tuple t) + { // __setstate__ + if (t.size() != 3) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ Vector v(t[0].cast(), t[1].cast(), t[2].cast()); return v; - })) - ; + })); m.def("SetToZero", (void (*)(Vector&)) &KDL::SetToZero); m.def("dot", (double (*)(const Vector&, const Vector&)) &KDL::dot); @@ -99,89 +107,101 @@ void init_frames(py::module &m) // -------------------- // Rotation // -------------------- - py::class_(m, "Rotation") - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def("__getitem__", [](const Rotation &r, int i, int j) { - if (i < 0 || i > 2 || j < 0 || j > 2) throw py::index_error("Vector index out of range"); - return r(i, j); - }) - .def("__repr__", [](const Rotation &r) { + py::class_ rotation(m, "Rotation"); + rotation.def(py::init<>()); + rotation.def(py::init()); + rotation.def(py::init()); + rotation.def(py::init()); + rotation.def("__getitem__", [](const Rotation &r, int i, int j) + { + if (i < 0 || i > 2 || j < 0 || j > 2) + throw py::index_error("Rotation index out of range"); + + return r(i, j); + }); + rotation.def("__repr__", [](const Rotation &r) + { std::ostringstream oss; - oss<(), t[1].cast(), t[2].cast()); - })) - ; + })); m.def("Equal", (bool (*)(const Rotation&, const Rotation&, double eps)) &KDL::Equal, py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); @@ -190,68 +210,70 @@ void init_frames(py::module &m) // -------------------- // Frame // -------------------- - py::class_(m, "Frame") - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init<>()) - .def_readwrite("M", &Frame::M) - .def_readwrite("p", &Frame::p) - .def("__getitem__", [](const Frame &frm, std::tuple idx) { - int i = std::get<0>(idx); - int j = std::get<1>(idx); - if (i < 0 || i > 2 || j < 0 || j > 3) { - throw py::index_error("Frame index out of range"); - } - return frm((unsigned int)i, (unsigned int)j); - }) - .def("__setitem__", [](Frame &frm, std::tuple idx, double value) { - int i = std::get<0>(idx); - int j = std::get<1>(idx); - if (i < 0 || i > 2 || j < 0 || j > 3) { - throw py::index_error("Frame index out of range"); - } - if (j == 3) { - frm.p(i) = value; - } else { - frm.M(i, j) = value; - } - }) - .def("__repr__", [](const Frame &frm) { - std::ostringstream oss; - oss << frm; - return oss.str(); - }) - .def("DH_Craig1989", &Frame::DH_Craig1989) - .def("DH", &Frame::DH) - .def("Inverse", (Frame (Frame::*)() const) &Frame::Inverse) - .def("Inverse", (Vector (Frame::*)(const Vector&) const) &Frame::Inverse) - .def("Inverse", (Wrench (Frame::*)(const Wrench&) const) &Frame::Inverse) - .def("Inverse", (Twist (Frame::*)(const Twist&) const) &Frame::Inverse) - .def_static("Identity", &Frame::Identity) - .def("Integrate", &Frame::Integrate) - .def(py::self * Vector()) - .def(py::self * Wrench()) - .def(py::self * Twist()) - .def(py::self * py::self) - .def(py::self == py::self) - .def(py::self != py::self) - .def(py::pickle( - [](const Frame &frm) { // __getstate__ + py::class_ frame(m, "Frame"); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init()); + frame.def(py::init<>()); + frame.def_readwrite("M", &Frame::M); + frame.def_readwrite("p", &Frame::p); + frame.def("__getitem__", [](const Frame &frm, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) + throw py::index_error("Frame index out of range"); + + return frm((unsigned int)i, (unsigned int)j); + }); + frame.def("__setitem__", [](Frame &frm, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 3) + throw py::index_error("Frame index out of range"); + + if (j == 3) + frm.p(i) = value; + else + frm.M(i, j) = value; + }); + frame.def("__repr__", [](const Frame &frm) + { + std::ostringstream oss; + oss << frm; + return oss.str(); + }); + frame.def("DH_Craig1989", &Frame::DH_Craig1989); + frame.def("DH", &Frame::DH); + frame.def("Inverse", (Frame (Frame::*)() const) &Frame::Inverse); + frame.def("Inverse", (Vector (Frame::*)(const Vector&) const) &Frame::Inverse); + frame.def("Inverse", (Wrench (Frame::*)(const Wrench&) const) &Frame::Inverse); + frame.def("Inverse", (Twist (Frame::*)(const Twist&) const) &Frame::Inverse); + frame.def_static("Identity", &Frame::Identity); + frame.def("Integrate", &Frame::Integrate); + frame.def(py::self * Vector()); + frame.def(py::self * Wrench()); + frame.def(py::self * Twist()); + frame.def(py::self * py::self); + frame.def(py::self == py::self); + frame.def(py::self != py::self); + frame.def(py::pickle( + [](const Frame &frm) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(frm.M, frm.p); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 2) { + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ Frame frm(t[0].cast(), t[1].cast()); return frm; - })) - ; + })); m.def("Equal", (bool (*)(const Frame&, const Frame&, double eps)) &KDL::Equal, py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); @@ -260,59 +282,63 @@ void init_frames(py::module &m) // -------------------- // Twist // -------------------- - py::class_(m, "Twist") - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def_readwrite("vel", &Twist::vel) - .def_readwrite("rot", &Twist::rot) - .def("__getitem__", [](const Twist &t, int i) { - if (i < 0 || i > 5) { - throw py::index_error("Twist index out of range"); - } - return t(i); - }) - .def("__setitem__", [](Twist &t, int i, double value) { - if (i < 0 || i > 5) { - throw py::index_error("Twist index out of range"); - } - t(i) = value; - }) - .def("__repr__", [](const Twist &t) { - std::ostringstream oss; - oss << t; - return oss.str(); - }) - .def_static("Zero", &Twist::Zero) - .def("ReverseSign", &Twist::ReverseSign) - .def("RefPoint", &Twist::RefPoint) - .def(py::self -= py::self) - .def(py::self += py::self) - .def(py::self * double()) - .def(double() * py::self) - .def(py::self / double()) - .def(py::self + py::self) - .def(py::self - py::self) - .def(py::self == py::self) - .def(py::self != py::self) - .def("__neg__", [](const Twist &a) { - return operator-(a); - }, py::is_operator()) - .def(py::pickle( - [](const Twist &tt) { // __getstate__ + py::class_ twist(m, "Twist"); + twist.def(py::init<>()); + twist.def(py::init()); + twist.def(py::init()); + twist.def_readwrite("vel", &Twist::vel); + twist.def_readwrite("rot", &Twist::rot); + twist.def("__getitem__", [](const Twist &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + return t(i); + }); + twist.def("__setitem__", [](Twist &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + t(i) = value; + }); + twist.def("__repr__", [](const Twist &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + twist.def_static("Zero", &Twist::Zero); + twist.def("ReverseSign", &Twist::ReverseSign); + twist.def("RefPoint", &Twist::RefPoint); + twist.def(py::self -= py::self); + twist.def(py::self += py::self); + twist.def(py::self * double()); + twist.def(double() * py::self); + twist.def(py::self / double()); + twist.def(py::self + py::self); + twist.def(py::self - py::self); + twist.def(py::self == py::self); + twist.def(py::self != py::self); + twist.def("__neg__", [](const Twist &a) + { + return operator-(a); + }, py::is_operator()); + twist.def(py::pickle( + [](const Twist &tt) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(tt.vel, tt.rot); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 2) { + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ Twist tt(t[0].cast(), t[1].cast()); return tt; - })) - ; + })); m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot); m.def("dot", (double (*)(const Wrench&, const Twist&)) &KDL::dot); @@ -324,59 +350,63 @@ void init_frames(py::module &m) // -------------------- // Wrench // -------------------- - py::class_(m, "Wrench") - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def_readwrite("force", &Wrench::force) - .def_readwrite("torque", &Wrench::torque) - .def("__getitem__", [](const Wrench &t, int i) { - if (i < 0 || i > 5) { - throw py::index_error("Wrench index out of range"); - } - return t(i); - }) - .def("__setitem__", [](Wrench &t, int i, double value) { - if (i < 0 || i > 5) { - throw py::index_error("Wrench index out of range"); - } - t(i) = value; - }) - .def("__repr__", [](const Wrench &t) { - std::ostringstream oss; - oss << t; - return oss.str(); - }) - .def_static("Zero", &Wrench::Zero) - .def("ReverseSign", &Wrench::ReverseSign) - .def("RefPoint", &Wrench::RefPoint) - .def(py::self -= py::self) - .def(py::self += py::self) - .def(py::self * double()) - .def(double() * py::self) - .def(py::self / double()) - .def(py::self + py::self) - .def(py::self - py::self) - .def(py::self == py::self) - .def(py::self != py::self) - .def("__neg__", [](const Wrench &w) { - return operator-(w); - }, py::is_operator()) - .def(py::pickle( - [](const Wrench &wr) { // __getstate__ + py::class_ wrench(m, "Wrench"); + wrench.def(py::init<>()); + wrench.def(py::init()); + wrench.def(py::init()); + wrench.def_readwrite("force", &Wrench::force); + wrench.def_readwrite("torque", &Wrench::torque); + wrench.def("__getitem__", [](const Wrench &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + return t(i); + }); + wrench.def("__setitem__", [](Wrench &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + t(i) = value; + }); + wrench.def("__repr__", [](const Wrench &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + wrench.def_static("Zero", &Wrench::Zero); + wrench.def("ReverseSign", &Wrench::ReverseSign); + wrench.def("RefPoint", &Wrench::RefPoint); + wrench.def(py::self -= py::self); + wrench.def(py::self += py::self); + wrench.def(py::self * double()); + wrench.def(double() * py::self); + wrench.def(py::self / double()); + wrench.def(py::self + py::self); + wrench.def(py::self - py::self); + wrench.def(py::self == py::self); + wrench.def(py::self != py::self); + wrench.def("__neg__", [](const Wrench &w) + { + return operator-(w); + }, py::is_operator()); + wrench.def(py::pickle( + [](const Wrench &wr) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(wr.force, wr.torque); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 2) { + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ Wrench wr(t[0].cast(), t[1].cast()); return wr; - })) - ; + })); m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero); m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal, diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index b30dc6464..d4286f3f2 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -20,6 +20,7 @@ //License along with this library; if not, write to the Free Software //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + #include #include #include "PyKDL.h" @@ -33,11 +34,11 @@ void init_framevel(pybind11::module &m) // -------------------- // doubleVel // -------------------- - py::class_ >(m, "doubleVel") - .def_readwrite("t", &doubleVel::t) - .def_readwrite("grad", &doubleVel::grad) - .def(py::init<>()) - ; + py::class_ double_vel(m, "doubleVel"); + double_vel.def(py::init<>()); + double_vel.def_readwrite("t", &doubleVel::t); + double_vel.def_readwrite("grad", &doubleVel::grad); + m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, py::arg("a"), py::arg("b"), py::arg("dt")=1.0); m.def("addDelta", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::addDelta, @@ -45,56 +46,61 @@ void init_framevel(pybind11::module &m) m.def("Equal", (bool (*)(const doubleVel&, const doubleVel&, double)) &KDL::Equal, py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); - py::class_(m, "VectorVel") - .def_readwrite("p", &VectorVel::p) - .def_readwrite("v", &VectorVel::v) - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def("value", &VectorVel::value) - .def("deriv", &VectorVel::deriv) - .def_static("Zero", &VectorVel::Zero) - .def("ReverseSign", &VectorVel::ReverseSign) - .def("Norm", &VectorVel::Norm) - .def(py::self += py::self) - .def(py::self -= py::self) - .def(py::self + py::self) - .def(py::self - py::self) - .def(Vector() + py::self) - .def(Vector() - py::self) - .def(py::self + Vector()) - .def(py::self - Vector()) - - .def(py::self * py::self) - .def(py::self * Vector()) - .def(Vector() * py::self) - .def(double() * py::self) - .def(py::self * double()) - .def(doubleVel() * py::self) - .def(py::self * doubleVel()) - .def(Rotation() * py::self) - - .def(py::self / double()) - .def(py::self / doubleVel()) - .def("__neg__", [](const VectorVel &a) { - return operator-(a); - }, py::is_operator()) - .def(py::pickle( - [](const VectorVel &vv) { // __getstate__ + + // -------------------- + // VectorVel + // -------------------- + py::class_ vector_vel(m, "VectorVel"); + vector_vel.def_readwrite("p", &VectorVel::p); + vector_vel.def_readwrite("v", &VectorVel::v); + vector_vel.def(py::init<>()); + vector_vel.def(py::init()); + vector_vel.def(py::init()); + vector_vel.def(py::init()); + vector_vel.def("value", &VectorVel::value); + vector_vel.def("deriv", &VectorVel::deriv); + vector_vel.def_static("Zero", &VectorVel::Zero); + vector_vel.def("ReverseSign", &VectorVel::ReverseSign); + vector_vel.def("Norm", &VectorVel::Norm); + vector_vel.def(py::self += py::self); + vector_vel.def(py::self -= py::self); + vector_vel.def(py::self + py::self); + vector_vel.def(py::self - py::self); + vector_vel.def(Vector() + py::self); + vector_vel.def(Vector() - py::self); + vector_vel.def(py::self + Vector()); + vector_vel.def(py::self - Vector()); + + vector_vel.def(py::self * py::self); + vector_vel.def(py::self * Vector()); + vector_vel.def(Vector() * py::self); + vector_vel.def(double() * py::self); + vector_vel.def(py::self * double()); + vector_vel.def(doubleVel() * py::self); + vector_vel.def(py::self * doubleVel()); + vector_vel.def(Rotation() * py::self); + + vector_vel.def(py::self / double()); + vector_vel.def(py::self / doubleVel()); + vector_vel.def("__neg__", [](const VectorVel &a) + { + return operator-(a); + }, py::is_operator()); + vector_vel.def(py::pickle( + [](const VectorVel &vv) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(vv.p, vv.v); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 2) { + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ VectorVel vv(t[0].cast(), t[1].cast()); return vv; - })) - ; + })); m.def("Equal", (bool (*)(const VectorVel&, const VectorVel&, double)) &KDL::Equal, py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); @@ -111,56 +117,56 @@ void init_framevel(pybind11::module &m) // -------------------- // RotationVel // -------------------- - py::class_(m, "RotationVel") - .def_readwrite("R", &RotationVel::R) - .def_readwrite("w", &RotationVel::w) - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def("value", &RotationVel::value) - .def("deriv", &RotationVel::deriv) - .def("UnitX", &RotationVel::UnitX) - .def("UnitY", &RotationVel::UnitY) - .def("UnitZ", &RotationVel::UnitZ) - .def_static("Identity", &RotationVel::Identity) - .def("Inverse", (RotationVel (RotationVel::*)(void) const) &RotationVel::Inverse) - .def("Inverse", (VectorVel (RotationVel::*)(const VectorVel&) const) &RotationVel::Inverse) - .def("Inverse", (VectorVel (RotationVel::*)(const Vector&) const) &RotationVel::Inverse) - .def("DoRotX", &RotationVel::DoRotX) - .def("DoRotY", &RotationVel::DoRotY) - .def("DoRotZ", &RotationVel::DoRotZ) - .def_static("RotX", &RotationVel::RotX) - .def_static("RotY", &RotationVel::RotY) - .def_static("RotZ", &RotationVel::RotZ) - .def_static("Rot", &RotationVel::Rot) - .def_static("Rot2", &RotationVel::Rot2) - - .def("Inverse", (TwistVel (RotationVel::*)(const TwistVel&) const) &RotationVel::Inverse) - .def("Inverse", (TwistVel (RotationVel::*)(const Twist&) const) &RotationVel::Inverse) - - .def(py::self * VectorVel()) - .def(py::self * Vector()) - .def(py::self * TwistVel()) - .def(py::self * Twist()) - .def(py::self * py::self) - .def(Rotation() * py::self) - .def(py::self * Rotation()) - .def(py::pickle( - [](const RotationVel &rv) { // __getstate__ + py::class_ rotation_vel(m, "RotationVel"); + rotation_vel.def_readwrite("R", &RotationVel::R); + rotation_vel.def_readwrite("w", &RotationVel::w); + rotation_vel.def(py::init<>()); + rotation_vel.def(py::init()); + rotation_vel.def(py::init()); + rotation_vel.def(py::init()); + rotation_vel.def("value", &RotationVel::value); + rotation_vel.def("deriv", &RotationVel::deriv); + rotation_vel.def("UnitX", &RotationVel::UnitX); + rotation_vel.def("UnitY", &RotationVel::UnitY); + rotation_vel.def("UnitZ", &RotationVel::UnitZ); + rotation_vel.def_static("Identity", &RotationVel::Identity); + rotation_vel.def("Inverse", (RotationVel (RotationVel::*)(void) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const VectorVel&) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (VectorVel (RotationVel::*)(const Vector&) const) &RotationVel::Inverse); + rotation_vel.def("DoRotX", &RotationVel::DoRotX); + rotation_vel.def("DoRotY", &RotationVel::DoRotY); + rotation_vel.def("DoRotZ", &RotationVel::DoRotZ); + rotation_vel.def_static("RotX", &RotationVel::RotX); + rotation_vel.def_static("RotY", &RotationVel::RotY); + rotation_vel.def_static("RotZ", &RotationVel::RotZ); + rotation_vel.def_static("Rot", &RotationVel::Rot); + rotation_vel.def_static("Rot2", &RotationVel::Rot2); + + rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const TwistVel&) const) &RotationVel::Inverse); + rotation_vel.def("Inverse", (TwistVel (RotationVel::*)(const Twist&) const) &RotationVel::Inverse); + + rotation_vel.def(py::self * VectorVel()); + rotation_vel.def(py::self * Vector()); + rotation_vel.def(py::self * TwistVel()); + rotation_vel.def(py::self * Twist()); + rotation_vel.def(py::self * py::self); + rotation_vel.def(Rotation() * py::self); + rotation_vel.def(py::self * Rotation()); + rotation_vel.def(py::pickle( + [](const RotationVel &rv) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(rv.R, rv.w); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 2) { + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ RotationVel rv(t[0].cast(), t[1].cast()); return rv; - })) - ; + })); m.def("Equal", (bool (*)(const RotationVel&, const RotationVel&, double)) &KDL::Equal, py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); @@ -173,46 +179,46 @@ void init_framevel(pybind11::module &m) // -------------------- // FrameVel // -------------------- - py::class_(m, "FrameVel") - .def_readwrite("M", &FrameVel::M) - .def_readwrite("p", &FrameVel::p) - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def("value", &FrameVel::value) - .def("deriv", &FrameVel::deriv) - .def_static("Identity", &FrameVel::Identity) - .def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse) - .def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse) - .def("Inverse", (VectorVel (FrameVel::*)(const Vector&) const) &FrameVel::Inverse) - .def(py::self * VectorVel()) - .def(py::self * Vector()) - .def("GetFrame", &FrameVel::GetFrame) - .def("GetTwist", &FrameVel::GetTwist) - .def("Inverse", (TwistVel (FrameVel::*)(const TwistVel&) const) &FrameVel::Inverse) - .def("Inverse", (TwistVel (FrameVel::*)(const Twist&) const) &FrameVel::Inverse) - .def(py::self * TwistVel()) - .def(py::self * Twist()) - .def(py::self * py::self) - .def(Frame() * py::self) - .def(py::self * Frame()) - .def(py::pickle( - [](const FrameVel &fv) { // __getstate__ + py::class_ frame_vel(m, "FrameVel"); + frame_vel.def_readwrite("M", &FrameVel::M); + frame_vel.def_readwrite("p", &FrameVel::p); + frame_vel.def(py::init<>()); + frame_vel.def(py::init()); + frame_vel.def(py::init()); + frame_vel.def(py::init()); + frame_vel.def(py::init()); + frame_vel.def("value", &FrameVel::value); + frame_vel.def("deriv", &FrameVel::deriv); + frame_vel.def_static("Identity", &FrameVel::Identity); + frame_vel.def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse); + frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse); + frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const Vector&) const) &FrameVel::Inverse); + frame_vel.def(py::self * VectorVel()); + frame_vel.def(py::self * Vector()); + frame_vel.def("GetFrame", &FrameVel::GetFrame); + frame_vel.def("GetTwist", &FrameVel::GetTwist); + frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const TwistVel&) const) &FrameVel::Inverse); + frame_vel.def("Inverse", (TwistVel (FrameVel::*)(const Twist&) const) &FrameVel::Inverse); + frame_vel.def(py::self * TwistVel()); + frame_vel.def(py::self * Twist()); + frame_vel.def(py::self * py::self); + frame_vel.def(Frame() * py::self); + frame_vel.def(py::self * Frame()); + frame_vel.def(py::pickle( + [](const FrameVel &fv) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(fv.M, fv.p); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 2) { + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ FrameVel rv(t[0].cast(), t[1].cast()); return rv; - })) - ; + })); m.def("Equal", (bool (*)(const FrameVel&, const FrameVel&, double)) &KDL::Equal, py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); @@ -225,51 +231,52 @@ void init_framevel(pybind11::module &m) // -------------------- // TwistVel // -------------------- - py::class_(m, "TwistVel") - .def_readwrite("vel", &TwistVel::vel) - .def_readwrite("rot", &TwistVel::rot) - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def("value", &TwistVel::value) - .def("deriv", &TwistVel::deriv) - .def_static("Zero", &TwistVel::Zero) - .def("ReverseSign", &TwistVel::ReverseSign) - .def("RefPoint", &TwistVel::RefPoint) - .def("GetTwist", &TwistVel::GetTwist) - .def("GetTwistDot", &TwistVel::GetTwistDot) - - .def(py::self -= py::self) - .def(py::self += py::self) - .def(py::self * double()) - .def(double() * py::self) - .def(py::self / double()) - - .def(py::self * doubleVel()) - .def(doubleVel() * py::self) - .def(py::self / doubleVel()) - - .def(py::self + py::self) - .def(py::self - py::self) - .def("__neg__", [](const TwistVel &a) { - return operator-(a); - }, py::is_operator()) - .def(py::pickle( - [](const TwistVel &tv) { // __getstate__ + py::class_ twist_vel(m, "TwistVel"); + twist_vel.def_readwrite("vel", &TwistVel::vel); + twist_vel.def_readwrite("rot", &TwistVel::rot); + twist_vel.def(py::init<>()); + twist_vel.def(py::init()); + twist_vel.def(py::init()); + twist_vel.def(py::init()); + twist_vel.def("value", &TwistVel::value); + twist_vel.def("deriv", &TwistVel::deriv); + twist_vel.def_static("Zero", &TwistVel::Zero); + twist_vel.def("ReverseSign", &TwistVel::ReverseSign); + twist_vel.def("RefPoint", &TwistVel::RefPoint); + twist_vel.def("GetTwist", &TwistVel::GetTwist); + twist_vel.def("GetTwistDot", &TwistVel::GetTwistDot); + + twist_vel.def(py::self -= py::self); + twist_vel.def(py::self += py::self); + twist_vel.def(py::self * double()); + twist_vel.def(double() * py::self); + twist_vel.def(py::self / double()); + + twist_vel.def(py::self * doubleVel()); + twist_vel.def(doubleVel() * py::self); + twist_vel.def(py::self / doubleVel()); + + twist_vel.def(py::self + py::self); + twist_vel.def(py::self - py::self); + twist_vel.def("__neg__", [](const TwistVel &a) + { + return operator-(a); + }, py::is_operator()); + twist_vel.def(py::pickle( + [](const TwistVel &tv) + { // __getstate__ /* Return a tuple that fully encodes the state of the object */ return py::make_tuple(tv.vel, tv.rot); }, - [](py::tuple t) { // __setstate__ - if (t.size() != 2) { + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - } /* Create a new C++ instance */ TwistVel tv(t[0].cast(), t[1].cast()); return tv; - })) - ; + })); m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero); m.def("Equal", (bool (*)(const TwistVel&, const TwistVel&, double)) &KDL::Equal, diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index ece636860..d47cf7a11 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -46,24 +46,24 @@ namespace py = pybind11; using namespace KDL; + void init_kinfam(pybind11::module &m) { // -------------------- // Joint // -------------------- py::class_ joint(m, "Joint"); - py::enum_(joint, "JointType") - .value("RotAxis", Joint::JointType::RotAxis) - .value("RotX", Joint::JointType::RotX) - .value("RotY", Joint::JointType::RotY) - .value("RotZ", Joint::JointType::RotZ) - .value("TransAxis", Joint::JointType::TransAxis) - .value("TransX", Joint::JointType::TransX) - .value("TransY", Joint::JointType::TransY) - .value("TransZ", Joint::JointType::TransZ) - .value("None", Joint::JointType::None) - .export_values() - ; + py::enum_ joint_type(joint, "JointType"); + joint_type.value("RotAxis", Joint::JointType::RotAxis); + joint_type.value("RotX", Joint::JointType::RotX); + joint_type.value("RotY", Joint::JointType::RotY); + joint_type.value("RotZ", Joint::JointType::RotZ); + joint_type.value("TransAxis", Joint::JointType::TransAxis); + joint_type.value("TransX", Joint::JointType::TransX); + joint_type.value("TransY", Joint::JointType::TransY); + joint_type.value("TransZ", Joint::JointType::TransZ); + joint_type.value("None", Joint::JointType::None); + joint_type.export_values(); joint.def(py::init<>()); joint.def(py::init(), @@ -86,7 +86,8 @@ void init_kinfam(pybind11::module &m) joint.def("getName", &Joint::getName); joint.def("getType", &Joint::getType); joint.def("getTypeName", &Joint::getTypeName); - joint.def("__repr__", [](const Joint &j) { + joint.def("__repr__", [](const Joint &j) + { std::ostringstream oss; oss << j; return oss.str(); @@ -96,123 +97,131 @@ void init_kinfam(pybind11::module &m) // -------------------- // RotationalInertia // -------------------- - py::class_(m, "RotationalInertia") - .def(py::init(), - py::arg("Ixx")=0, py::arg("Iyy")=0, py::arg("Izz")=0, - py::arg("Ixy")=0, py::arg("Ixz")=0, py::arg("Iyz")=0) - .def_static("Zero", &RotationalInertia::Zero) - .def("__getitem__", [](const RotationalInertia &inertia, int i) { - if (i < 0 || i > 8) { - throw py::index_error("RotationalInertia index out of range"); - } - return inertia.data[i]; - }) - .def("__setitem__", [](RotationalInertia &inertia, int i, double value) { - if (i < 0 || i > 8) { - throw py::index_error("RotationalInertia index out of range"); - } - inertia.data[i] = value; - }) - .def(double() * py::self) - .def(py::self + py::self) - .def(py::self * Vector()) - ; + py::class_ rotational_inertia(m, "RotationalInertia"); + rotational_inertia.def(py::init(), + py::arg("Ixx")=0, py::arg("Iyy")=0, py::arg("Izz")=0, + py::arg("Ixy")=0, py::arg("Ixz")=0, py::arg("Iyz")=0); + rotational_inertia.def_static("Zero", &RotationalInertia::Zero); + rotational_inertia.def("__getitem__", [](const RotationalInertia &inertia, int i) + { + if (i < 0 || i > 8) + throw py::index_error("RotationalInertia index out of range"); + + return inertia.data[i]; + }); + rotational_inertia.def("__setitem__", [](RotationalInertia &inertia, int i, double value) + { + if (i < 0 || i > 8) + throw py::index_error("RotationalInertia index out of range"); + + inertia.data[i] = value; + }); + rotational_inertia.def(double() * py::self); + rotational_inertia.def(py::self + py::self); + rotational_inertia.def(py::self * Vector()); + // -------------------- // RigidBodyInertia // -------------------- - py::class_(m, "RigidBodyInertia") - .def(py::init(), - py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero"), py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero")) - .def_static("Zero", &RigidBodyInertia::Zero) - .def("RefPoint", &RigidBodyInertia::RefPoint) - .def("getMass", &RigidBodyInertia::getMass) - .def("getCOG", &RigidBodyInertia::getCOG) - .def("getRotationalInertia", &RigidBodyInertia::getRotationalInertia) - .def(double() * py::self) - .def(py::self + py::self) - .def(py::self * Twist()) - .def(Frame() * py::self) - .def(Rotation() * py::self) - ; + py::class_ rigid_body_inertia(m, "RigidBodyInertia"); + rigid_body_inertia.def(py::init(), + py::arg("m")=0, py::arg_v("oc", Vector::Zero(), "Vector.Zero"), + py::arg_v("Ic", RotationalInertia::Zero(), "RigidBodyInertia.Zero")); + rigid_body_inertia.def_static("Zero", &RigidBodyInertia::Zero); + rigid_body_inertia.def("RefPoint", &RigidBodyInertia::RefPoint); + rigid_body_inertia.def("getMass", &RigidBodyInertia::getMass); + rigid_body_inertia.def("getCOG", &RigidBodyInertia::getCOG); + rigid_body_inertia.def("getRotationalInertia", &RigidBodyInertia::getRotationalInertia); + rigid_body_inertia.def(double() * py::self); + rigid_body_inertia.def(py::self + py::self); + rigid_body_inertia.def(py::self * Twist()); + rigid_body_inertia.def(Frame() * py::self); + rigid_body_inertia.def(Rotation() * py::self); + // -------------------- // Segment // -------------------- - py::class_(m, "Segment") - .def(py::init(), - py::arg("name"), py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")) - .def(py::init(), - py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")) - .def(py::init()) - .def("getFrameToTip", &Segment::getFrameToTip) - .def("pose", &Segment::pose) - .def("twist", &Segment::twist) - .def("getName", &Segment::getName) - .def("getJoint", &Segment::getJoint) - .def("getInertia", &Segment::getInertia) - .def("setInertia", &Segment::setInertia) - ; + py::class_ segment(m, "Segment"); + segment.def(py::init(), + py::arg("name"), py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), + py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")); + segment.def(py::init(), + py::arg_v("joint", Joint(), "Joint"), py::arg_v("f_tip", Frame::Identity(), "Frame.Identity"), + py::arg_v("I", RigidBodyInertia::Zero(), "RigidBodyInertia.Zero")); + segment.def(py::init()); + segment.def("getFrameToTip", &Segment::getFrameToTip); + segment.def("pose", &Segment::pose); + segment.def("twist", &Segment::twist); + segment.def("getName", &Segment::getName); + segment.def("getJoint", &Segment::getJoint); + segment.def("getInertia", &Segment::getInertia); + segment.def("setInertia", &Segment::setInertia); + // -------------------- // Chain // -------------------- - py::class_(m, "Chain") - .def(py::init<>()) - .def(py::init()) - .def("addSegment", &Chain::addSegment) - .def("addChain", &Chain::addChain) - .def("getNrOfJoints", &Chain::getNrOfJoints) - .def("getNrOfSegments", &Chain::getNrOfSegments) - .def("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment) - .def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment) - ; + py::class_ chain(m, "Chain"); + chain.def(py::init<>()); + chain.def(py::init()); + chain.def("addSegment", &Chain::addSegment); + chain.def("addChain", &Chain::addChain); + chain.def("getNrOfJoints", &Chain::getNrOfJoints); + chain.def("getNrOfSegments", &Chain::getNrOfSegments); + chain.def("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment); + chain.def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment); + // -------------------- // Tree // -------------------- - py::class_(m, "Tree") - .def(py::init(), py::arg("root_name")="root") - .def("addSegment", &Tree::addSegment) - .def("addChain", &Tree::addChain) - .def("addTree", &Tree::addTree) - .def("getNrOfJoints", &Tree::getNrOfJoints) - .def("getNrOfSegments", &Tree::getNrOfSegments) - .def("getChain", [](const Tree &tree, const std::string& chain_root, const std::string& chain_tip) { - Chain* chain = new Chain(); - tree.getChain(chain_root, chain_tip, *chain); - return chain; - }) - ; + py::class_ tree(m, "Tree"); + tree.def(py::init(), py::arg("root_name")="root"); + tree.def("addSegment", &Tree::addSegment); + tree.def("addChain", &Tree::addChain); + tree.def("addTree", &Tree::addTree); + tree.def("getNrOfJoints", &Tree::getNrOfJoints); + tree.def("getNrOfSegments", &Tree::getNrOfSegments); + tree.def("getChain", [](const Tree &tree, const std::string& chain_root, const std::string& chain_tip) + { + Chain* chain = new Chain(); + tree.getChain(chain_root, chain_tip, *chain); + return chain; + }); + // -------------------- // JntArray // -------------------- - py::class_(m, "JntArray") - .def(py::init()) - .def(py::init()) - .def("rows", &JntArray::rows) - .def("columns", &JntArray::columns) - .def("resize", &JntArray::resize) - .def("__getitem__", [](const JntArray &ja, int i) { - if (i < 0 || i > ja.rows()) { - throw py::index_error("JntArray index out of range"); - } - return ja(i); - }) - .def("__setitem__", [](JntArray &ja, int i, double value) { - if (i < 0 || i > ja.rows()) { - throw py::index_error("JntArray index out of range"); - } - ja(i) = value; - }) - .def("__repr__", [](const JntArray &ja) { - std::ostringstream oss; - oss << ja; - return oss.str(); - }) - .def(py::self == py::self) - ; + py::class_ jnt_array(m, "JntArray"); + jnt_array.def(py::init()); + jnt_array.def(py::init()); + jnt_array.def("rows", &JntArray::rows); + jnt_array.def("columns", &JntArray::columns); + jnt_array.def("resize", &JntArray::resize); + jnt_array.def("__getitem__", [](const JntArray &ja, int i) + { + if (i < 0 || i > ja.rows()) + throw py::index_error("JntArray index out of range"); + + return ja(i); + }); + jnt_array.def("__setitem__", [](JntArray &ja, int i, double value) + { + if (i < 0 || i > ja.rows()) + throw py::index_error("JntArray index out of range"); + + ja(i) = value; + }); + jnt_array.def("__repr__", [](const JntArray &ja) + { + std::ostringstream oss; + oss << ja; + return oss.str(); + }); + jnt_array.def(py::self == py::self); m.def("Add", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Add); m.def("Subtract", (void (*)(const JntArray&, const JntArray&, JntArray&)) &KDL::Subtract); @@ -227,16 +236,15 @@ void init_kinfam(pybind11::module &m) // -------------------- // JntArrayVel // -------------------- - py::class_(m, "JntArrayVel") - .def_readwrite("q", &JntArrayVel::q) - .def_readwrite("qdot", &JntArrayVel::qdot) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def("resize", &JntArrayVel::resize) - .def("value", &JntArrayVel::value) - .def("deriv", &JntArrayVel::deriv) - ; + py::class_ jnt_array_vel(m, "JntArrayVel"); + jnt_array_vel.def_readwrite("q", &JntArrayVel::q); + jnt_array_vel.def_readwrite("qdot", &JntArrayVel::qdot); + jnt_array_vel.def(py::init()); + jnt_array_vel.def(py::init()); + jnt_array_vel.def(py::init()); + jnt_array_vel.def("resize", &JntArrayVel::resize); + jnt_array_vel.def("value", &JntArrayVel::value); + jnt_array_vel.def("deriv", &JntArrayVel::deriv); m.def("Add", (void (*)(const JntArrayVel&, const JntArrayVel&, JntArrayVel&)) &KDL::Add); m.def("Add", (void (*)(const JntArrayVel&, const JntArray&, JntArrayVel&)) &KDL::Add); @@ -254,39 +262,39 @@ void init_kinfam(pybind11::module &m) // -------------------- // Jacobian // -------------------- - py::class_(m, "Jacobian") - .def(py::init()) - .def(py::init()) - .def("rows", &Jacobian::rows) - .def("columns", &Jacobian::columns) - .def("resize", &Jacobian::resize) - .def("getColumn", &Jacobian::getColumn) - .def("setColumn", &Jacobian::setColumn) - .def("changeRefPoint", &Jacobian::changeRefPoint) - .def("changeBase", &Jacobian::changeBase) - .def("changeRefFrame", &Jacobian::changeRefFrame) - .def("__getitem__", [](const Jacobian &jac, std::tuple idx) { - int i = std::get<0>(idx); - int j = std::get<1>(idx); - if (i < 0 || i > 5 || j < 0 || j > jac.columns()) { - throw py::index_error("Jacobian index out of range"); - } - return jac((unsigned int)i, (unsigned int)j); - }) - .def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) { - int i = std::get<0>(idx); - int j = std::get<1>(idx); - if (i < 0 || i > 5 || j < 0 || j > jac.columns()) { - throw py::index_error("Jacobian index out of range"); - } - jac((unsigned int)i, (unsigned int)j) = value; - }) - .def("__repr__", [](const Jacobian &jac) { - std::ostringstream oss; - oss << jac; - return oss.str(); - }) - ; + py::class_ jacobian(m, "Jacobian"); + jacobian.def(py::init()); + jacobian.def(py::init()); + jacobian.def("rows", &Jacobian::rows); + jacobian.def("columns", &Jacobian::columns); + jacobian.def("resize", &Jacobian::resize); + jacobian.def("getColumn", &Jacobian::getColumn); + jacobian.def("setColumn", &Jacobian::setColumn); + jacobian.def("changeRefPoint", &Jacobian::changeRefPoint); + jacobian.def("changeBase", &Jacobian::changeBase); + jacobian.def("changeRefFrame", &Jacobian::changeRefFrame); + jacobian.def("__getitem__", [](const Jacobian &jac, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j > jac.columns()) + throw py::index_error("Jacobian index out of range"); + return jac((unsigned int)i, (unsigned int)j); + }); + jacobian.def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j > jac.columns()) + throw py::index_error("Jacobian index out of range"); + jac((unsigned int)i, (unsigned int)j) = value; + }); + jacobian.def("__repr__", [](const Jacobian &jac) + { + std::ostringstream oss; + oss << jac; + return oss.str(); + }); m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero); m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint); @@ -297,159 +305,157 @@ void init_kinfam(pybind11::module &m) // -------------------- // SolverI // -------------------- - py::class_(m, "SolverI") - .def("getError", &SolverI::getError) - .def("strError", &SolverI::strError) - .def("updateInternalDataStructures", &SolverI::updateInternalDataStructures) - ; + py::class_ solver_i(m, "SolverI"); + solver_i.def("getError", &SolverI::getError); + solver_i.def("strError", &SolverI::strError); + solver_i.def("updateInternalDataStructures", &SolverI::updateInternalDataStructures); + // -------------------- // ChainFkSolverPos // -------------------- - py::class_(m, "ChainFkSolverPos") - .def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart, - py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1) - ; + py::class_ chain_fk_solver_pos(m, "ChainFkSolverPos"); + chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + // -------------------- // ChainFkSolverVel // -------------------- - py::class_(m, "ChainFkSolverVel") - .def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart, - py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1) - ; + py::class_ chain_fk_solver_vel(m, "ChainFkSolverVel"); + chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + // ------------------------------ // ChainFkSolverPos_recursive // ------------------------------ - py::class_(m, "ChainFkSolverPos_recursive") - .def(py::init()) - ; + py::class_ chain_fk_solver_pos_recursive(m, "ChainFkSolverPos_recursive"); + chain_fk_solver_pos_recursive.def(py::init()); // ------------------------------ // ChainFkSolverVel_recursive // ------------------------------ - py::class_(m, "ChainFkSolverVel_recursive") - .def(py::init()) - ; + py::class_ chain_fk_solver_vel_recursive(m, "ChainFkSolverVel_recursive"); + chain_fk_solver_vel_recursive.def(py::init()); + // -------------------- // ChainIkSolverPos // -------------------- - py::class_(m, "ChainIkSolverPos") - .def("CartToJnt", (int (ChainIkSolverPos::*)(const JntArray&, const Frame&, JntArray&)) &ChainIkSolverPos::CartToJnt, - py::arg("q_init"), py::arg("p_in"), py::arg("q_out")) - ; + py::class_ chain_ik_solver_pos(m, "ChainIkSolverPos"); + chain_ik_solver_pos.def("CartToJnt", (int (ChainIkSolverPos::*)(const JntArray&, const Frame&, JntArray&)) &ChainIkSolverPos::CartToJnt, + py::arg("q_init"), py::arg("p_in"), py::arg("q_out")); + // -------------------- // ChainIkSolverVel // -------------------- - py::class_(m, "ChainIkSolverVel") - .def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const Twist&, JntArray&)) &ChainIkSolverVel::CartToJnt, - py::arg("q_in"), py::arg("v_in"), py::arg("qdot_out")) - .def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt, - py::arg("q_init"), py::arg("v_in"), py::arg("q_out")) - ; + py::class_ chain_ik_solver_vel(m, "ChainIkSolverVel"); + chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const Twist&, JntArray&)) &ChainIkSolverVel::CartToJnt, + py::arg("q_in"), py::arg("v_in"), py::arg("qdot_out")); + chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt, + py::arg("q_init"), py::arg("v_in"), py::arg("q_out")); - // -------------------- + // ---------------------- // ChainIkSolverPos_NR - // -------------------- - py::class_(m, "ChainIkSolverPos_NR") - .def(py::init(), - py::arg("chain"), py::arg("fksolver"), py::arg("iksolver"), - py::arg("maxiter")=100, py::arg("eps")=epsilon) - ; + // ---------------------- + py::class_ chain_ik_solver_pos_NR(m, "ChainIkSolverPos_NR"); + chain_ik_solver_pos_NR.def(py::init(), + py::arg("chain"), py::arg("fksolver"), py::arg("iksolver"), + py::arg("maxiter")=100, py::arg("eps")=epsilon); + // --------------------------- // ChainIkSolverPos_NR_JL // --------------------------- - py::class_(m, "ChainIkSolverPos_NR_JL") - .def(py::init(), - py::arg("chain"), py::arg("q_min"), py::arg("q_max"), py::arg("fksolver"), py::arg("iksolver"), - py::arg("maxiter")=100, py::arg("eps")=epsilon) - ; + py::class_ chain_ik_solver_pos_NR_JL(m, "ChainIkSolverPos_NR_JL"); + chain_ik_solver_pos_NR_JL.def(py::init(), + py::arg("chain"), py::arg("q_min"), py::arg("q_max"), py::arg("fksolver"), + py::arg("iksolver"), py::arg("maxiter")=100, py::arg("eps")=epsilon); + // ------------------------- // ChainIkSolverVel_pinv // ------------------------- - py::class_(m, "ChainIkSolverVel_pinv") - .def(py::init(), - py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150) - ; + py::class_ chain_ik_solver_vel_pinv(m, "ChainIkSolverVel_pinv"); + chain_ik_solver_vel_pinv.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150); + // ------------------------- // ChainIkSolverVel_wdls // ------------------------- - py::class_(m, "ChainIkSolverVel_wdls") - .def(py::init(), - py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150) - .def("setWeightTS", &ChainIkSolverVel_wdls::setWeightTS) - .def("setWeightJS", &ChainIkSolverVel_wdls::setWeightJS) - .def("setLambda", &ChainIkSolverVel_wdls::setLambda) - .def("setEps", &ChainIkSolverVel_wdls::setEps) - .def("setMaxIter", &ChainIkSolverVel_wdls::setMaxIter) - .def("getNrZeroSigmas", &ChainIkSolverVel_wdls::getNrZeroSigmas) - .def("getSigmaMin", &ChainIkSolverVel_wdls::getSigmaMin) - .def("getSigma", &ChainIkSolverVel_wdls::getSigma) - .def("getEps", &ChainIkSolverVel_wdls::getEps) - .def("getLambda", &ChainIkSolverVel_wdls::getLambda) - .def("getLambdaScaled", &ChainIkSolverVel_wdls::getLambdaScaled) - .def("getSVDResult", &ChainIkSolverVel_wdls::getSVDResult) - ; + py::class_ chain_ik_solver_vel_wdls(m, "ChainIkSolverVel_wdls"); + chain_ik_solver_vel_wdls.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150); + chain_ik_solver_vel_wdls.def("setWeightTS", &ChainIkSolverVel_wdls::setWeightTS); + chain_ik_solver_vel_wdls.def("setWeightJS", &ChainIkSolverVel_wdls::setWeightJS); + chain_ik_solver_vel_wdls.def("setLambda", &ChainIkSolverVel_wdls::setLambda); + chain_ik_solver_vel_wdls.def("setEps", &ChainIkSolverVel_wdls::setEps); + chain_ik_solver_vel_wdls.def("setMaxIter", &ChainIkSolverVel_wdls::setMaxIter); + chain_ik_solver_vel_wdls.def("getNrZeroSigmas", &ChainIkSolverVel_wdls::getNrZeroSigmas); + chain_ik_solver_vel_wdls.def("getSigmaMin", &ChainIkSolverVel_wdls::getSigmaMin); + chain_ik_solver_vel_wdls.def("getSigma", &ChainIkSolverVel_wdls::getSigma); + chain_ik_solver_vel_wdls.def("getEps", &ChainIkSolverVel_wdls::getEps); + chain_ik_solver_vel_wdls.def("getLambda", &ChainIkSolverVel_wdls::getLambda); + chain_ik_solver_vel_wdls.def("getLambdaScaled", &ChainIkSolverVel_wdls::getLambdaScaled); + chain_ik_solver_vel_wdls.def("getSVDResult", &ChainIkSolverVel_wdls::getSVDResult); + // ------------------------- // ChainIkSolverPos_LMA // ------------------------- - py::class_(m, "ChainIkSolverPos_LMA") - .def(py::init(), - py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=500, py::arg("eps_joints")=0.000000000000001) - ; + py::class_ chain_ik_solver_pos_LMA(m, "ChainIkSolverPos_LMA"); + chain_ik_solver_pos_LMA.def(py::init(), + py::arg("chain"), py::arg("eps")=1e-5, py::arg("maxiter")=500, + py::arg("eps_joints")=1e-15); - // ------------------------- - // ChainIkSolverVel_pinv_nso - // ------------------------- - py::class_(m, "ChainIkSolverVel_pinv_nso") - .def(py::init(), - py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150, py::arg("alpha")=0.25) - .def("setWeights", &ChainIkSolverVel_pinv_nso::setWeights) - .def("setOptPos", &ChainIkSolverVel_pinv_nso::setOptPos) - .def("setAlpha", &ChainIkSolverVel_pinv_nso::setAlpha) - .def("getWeights", &ChainIkSolverVel_pinv_nso::getWeights) - .def("getOptPos", &ChainIkSolverVel_pinv_nso::getOptPos) - .def("getAlpha", &ChainIkSolverVel_pinv_nso::getAlpha) - ; - // ------------------------------ + // ---------------------------- + // ChainIkSolverVel_pinv_nso + // ---------------------------- + py::class_ chain_ik_solver_vel_pinv_nso(m, "ChainIkSolverVel_pinv_nso"); + chain_ik_solver_vel_pinv_nso.def(py::init(), + py::arg("chain"), py::arg("eps")=0.00001, py::arg("maxiter")=150, py::arg("alpha")=0.25); + chain_ik_solver_vel_pinv_nso.def("setWeights", &ChainIkSolverVel_pinv_nso::setWeights); + chain_ik_solver_vel_pinv_nso.def("setOptPos", &ChainIkSolverVel_pinv_nso::setOptPos); + chain_ik_solver_vel_pinv_nso.def("setAlpha", &ChainIkSolverVel_pinv_nso::setAlpha); + chain_ik_solver_vel_pinv_nso.def("getWeights", &ChainIkSolverVel_pinv_nso::getWeights); + chain_ik_solver_vel_pinv_nso.def("getOptPos", &ChainIkSolverVel_pinv_nso::getOptPos); + chain_ik_solver_vel_pinv_nso.def("getAlpha", &ChainIkSolverVel_pinv_nso::getAlpha); + + + // ------------------------------- // ChainIkSolverVel_pinv_givens - // ------------------------------ - py::class_(m, "ChainIkSolverVel_pinv_givens") - .def(py::init()) - ; + // ------------------------------- + py::class_ chain_ik_solver_vel_pinv_givens(m, "ChainIkSolverVel_pinv_givens"); + chain_ik_solver_vel_pinv_givens.def(py::init()); + // ------------------------------ // ChainJntToJacSolver // ------------------------------ - py::class_(m, "ChainJntToJacSolver") - .def(py::init()) - .def("JntToJac", &ChainJntToJacSolver::JntToJac, - py::arg("q_in"), py::arg("jac"), py::arg("segmentNR")=-1) - .def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints) - ; + py::class_ chain_jnt_to_jac_solver(m, "ChainJntToJacSolver"); + chain_jnt_to_jac_solver.def(py::init()); + chain_jnt_to_jac_solver.def("JntToJac", &ChainJntToJacSolver::JntToJac, + py::arg("q_in"), py::arg("jac"), py::arg("segmentNR")=-1); + chain_jnt_to_jac_solver.def("setLockedJoints", &ChainJntToJacSolver::setLockedJoints); + // ------------------------------ // ChainIdSolver // ------------------------------ - py::class_(m, "ChainIdSolver") - .def("CartToJnt", &ChainIdSolver::CartToJnt) - ; + py::class_ chain_id_solver(m, "ChainIdSolver"); + chain_id_solver.def("CartToJnt", &ChainIdSolver::CartToJnt); + // ------------------------------ // ChainIdSolver_RNE // ------------------------------ - py::class_(m, "ChainIdSolver_RNE") - .def(py::init()) - ; + py::class_ chain_id_solver_RNE(m, "ChainIdSolver_RNE"); + chain_id_solver_RNE.def(py::init()); } From 77f6ecf8b2d2822151862d86887e1968a65761ef Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 20:34:31 +0100 Subject: [PATCH 017/136] Move declerations so class is defined before used by other classes/functions This prevents any cpp scoping in docstrings --- python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp | 2 +- python_orocos_kdl/PyKDL/pybind11/PyKDL.h | 2 +- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 268 +++++++++--------- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 118 ++++---- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 87 +++--- 5 files changed, 239 insertions(+), 238 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp index 7524e4691..b9c8f508b 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp @@ -32,7 +32,7 @@ PYBIND11_MODULE(PyKDL, m) m.doc() = "Orocos KDL Python wrapper"; // optional module docstring m.attr("__version__") = KDL_VERSION_STRING; init_frames(m); - init_kinfam(m); init_framevel(m); + init_kinfam(m); init_dynamics(m); } diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h index db1ca3785..aa188ca5f 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h @@ -27,6 +27,6 @@ void init_frames(pybind11::module &m); -void init_kinfam(pybind11::module &m); void init_framevel(pybind11::module &m); +void init_kinfam(pybind11::module &m); void init_dynamics(pybind11::module &m); diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 7bed44a1f..3759b452c 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -104,6 +104,140 @@ void init_frames(py::module &m) py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + // -------------------- + // Wrench + // -------------------- + py::class_ wrench(m, "Wrench"); + wrench.def(py::init<>()); + wrench.def(py::init()); + wrench.def(py::init()); + wrench.def_readwrite("force", &Wrench::force); + wrench.def_readwrite("torque", &Wrench::torque); + wrench.def("__getitem__", [](const Wrench &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + return t(i); + }); + wrench.def("__setitem__", [](Wrench &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Wrench index out of range"); + + t(i) = value; + }); + wrench.def("__repr__", [](const Wrench &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + wrench.def_static("Zero", &Wrench::Zero); + wrench.def("ReverseSign", &Wrench::ReverseSign); + wrench.def("RefPoint", &Wrench::RefPoint); + wrench.def(py::self -= py::self); + wrench.def(py::self += py::self); + wrench.def(py::self * double()); + wrench.def(double() * py::self); + wrench.def(py::self / double()); + wrench.def(py::self + py::self); + wrench.def(py::self - py::self); + wrench.def(py::self == py::self); + wrench.def(py::self != py::self); + wrench.def("__neg__", [](const Wrench &w) + { + return operator-(w); + }, py::is_operator()); + wrench.def(py::pickle( + [](const Wrench &wr) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(wr.force, wr.torque); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Wrench wr(t[0].cast(), t[1].cast()); + return wr; + })); + + m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + + // -------------------- + // Twist + // -------------------- + py::class_ twist(m, "Twist"); + twist.def(py::init<>()); + twist.def(py::init()); + twist.def(py::init()); + twist.def_readwrite("vel", &Twist::vel); + twist.def_readwrite("rot", &Twist::rot); + twist.def("__getitem__", [](const Twist &t, int i) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + return t(i); + }); + twist.def("__setitem__", [](Twist &t, int i, double value) + { + if (i < 0 || i > 5) + throw py::index_error("Twist index out of range"); + + t(i) = value; + }); + twist.def("__repr__", [](const Twist &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); + twist.def_static("Zero", &Twist::Zero); + twist.def("ReverseSign", &Twist::ReverseSign); + twist.def("RefPoint", &Twist::RefPoint); + twist.def(py::self -= py::self); + twist.def(py::self += py::self); + twist.def(py::self * double()); + twist.def(double() * py::self); + twist.def(py::self / double()); + twist.def(py::self + py::self); + twist.def(py::self - py::self); + twist.def(py::self == py::self); + twist.def(py::self != py::self); + twist.def("__neg__", [](const Twist &a) + { + return operator-(a); + }, py::is_operator()); + twist.def(py::pickle( + [](const Twist &tt) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tt.vel, tt.rot); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + Twist tt(t[0].cast(), t[1].cast()); + return tt; + })); + + m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot); + m.def("dot", (double (*)(const Wrench&, const Twist&)) &KDL::dot); + m.def("SetToZero", (void (*)(Twist&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const Twist&, const Twist&, double eps)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + // -------------------- // Rotation // -------------------- @@ -279,140 +413,6 @@ void init_frames(py::module &m) py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); - // -------------------- - // Twist - // -------------------- - py::class_ twist(m, "Twist"); - twist.def(py::init<>()); - twist.def(py::init()); - twist.def(py::init()); - twist.def_readwrite("vel", &Twist::vel); - twist.def_readwrite("rot", &Twist::rot); - twist.def("__getitem__", [](const Twist &t, int i) - { - if (i < 0 || i > 5) - throw py::index_error("Twist index out of range"); - - return t(i); - }); - twist.def("__setitem__", [](Twist &t, int i, double value) - { - if (i < 0 || i > 5) - throw py::index_error("Twist index out of range"); - - t(i) = value; - }); - twist.def("__repr__", [](const Twist &t) - { - std::ostringstream oss; - oss << t; - return oss.str(); - }); - twist.def_static("Zero", &Twist::Zero); - twist.def("ReverseSign", &Twist::ReverseSign); - twist.def("RefPoint", &Twist::RefPoint); - twist.def(py::self -= py::self); - twist.def(py::self += py::self); - twist.def(py::self * double()); - twist.def(double() * py::self); - twist.def(py::self / double()); - twist.def(py::self + py::self); - twist.def(py::self - py::self); - twist.def(py::self == py::self); - twist.def(py::self != py::self); - twist.def("__neg__", [](const Twist &a) - { - return operator-(a); - }, py::is_operator()); - twist.def(py::pickle( - [](const Twist &tt) - { // __getstate__ - /* Return a tuple that fully encodes the state of the object */ - return py::make_tuple(tt.vel, tt.rot); - }, - [](py::tuple t) - { // __setstate__ - if (t.size() != 2) - throw std::runtime_error("Invalid state!"); - - /* Create a new C++ instance */ - Twist tt(t[0].cast(), t[1].cast()); - return tt; - })); - - m.def("dot", (double (*)(const Twist&, const Wrench&)) &KDL::dot); - m.def("dot", (double (*)(const Wrench&, const Twist&)) &KDL::dot); - m.def("SetToZero", (void (*)(Twist&)) &KDL::SetToZero); - m.def("Equal", (bool (*)(const Twist&, const Twist&, double eps)) &KDL::Equal, - py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); - - - // -------------------- - // Wrench - // -------------------- - py::class_ wrench(m, "Wrench"); - wrench.def(py::init<>()); - wrench.def(py::init()); - wrench.def(py::init()); - wrench.def_readwrite("force", &Wrench::force); - wrench.def_readwrite("torque", &Wrench::torque); - wrench.def("__getitem__", [](const Wrench &t, int i) - { - if (i < 0 || i > 5) - throw py::index_error("Wrench index out of range"); - - return t(i); - }); - wrench.def("__setitem__", [](Wrench &t, int i, double value) - { - if (i < 0 || i > 5) - throw py::index_error("Wrench index out of range"); - - t(i) = value; - }); - wrench.def("__repr__", [](const Wrench &t) - { - std::ostringstream oss; - oss << t; - return oss.str(); - }); - wrench.def_static("Zero", &Wrench::Zero); - wrench.def("ReverseSign", &Wrench::ReverseSign); - wrench.def("RefPoint", &Wrench::RefPoint); - wrench.def(py::self -= py::self); - wrench.def(py::self += py::self); - wrench.def(py::self * double()); - wrench.def(double() * py::self); - wrench.def(py::self / double()); - wrench.def(py::self + py::self); - wrench.def(py::self - py::self); - wrench.def(py::self == py::self); - wrench.def(py::self != py::self); - wrench.def("__neg__", [](const Wrench &w) - { - return operator-(w); - }, py::is_operator()); - wrench.def(py::pickle( - [](const Wrench &wr) - { // __getstate__ - /* Return a tuple that fully encodes the state of the object */ - return py::make_tuple(wr.force, wr.torque); - }, - [](py::tuple t) - { // __setstate__ - if (t.size() != 2) - throw std::runtime_error("Invalid state!"); - - /* Create a new C++ instance */ - Wrench wr(t[0].cast(), t[1].cast()); - return wr; - })); - - m.def("SetToZero", (void (*)(Wrench&)) &KDL::SetToZero); - m.def("Equal", (bool (*)(const Wrench&, const Wrench&, double eps)) &KDL::Equal, - py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); - - // -------------------- // Global // -------------------- diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index d4286f3f2..5109e0c32 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -114,6 +114,65 @@ void init_framevel(pybind11::module &m) m.def("dot", (doubleVel (*)(const Vector&, const VectorVel&)) &KDL::dot); + // -------------------- + // TwistVel + // -------------------- + py::class_ twist_vel(m, "TwistVel"); + twist_vel.def_readwrite("vel", &TwistVel::vel); + twist_vel.def_readwrite("rot", &TwistVel::rot); + twist_vel.def(py::init<>()); + twist_vel.def(py::init()); + twist_vel.def(py::init()); + twist_vel.def(py::init()); + twist_vel.def("value", &TwistVel::value); + twist_vel.def("deriv", &TwistVel::deriv); + twist_vel.def_static("Zero", &TwistVel::Zero); + twist_vel.def("ReverseSign", &TwistVel::ReverseSign); + twist_vel.def("RefPoint", &TwistVel::RefPoint); + twist_vel.def("GetTwist", &TwistVel::GetTwist); + twist_vel.def("GetTwistDot", &TwistVel::GetTwistDot); + + twist_vel.def(py::self -= py::self); + twist_vel.def(py::self += py::self); + twist_vel.def(py::self * double()); + twist_vel.def(double() * py::self); + twist_vel.def(py::self / double()); + + twist_vel.def(py::self * doubleVel()); + twist_vel.def(doubleVel() * py::self); + twist_vel.def(py::self / doubleVel()); + + twist_vel.def(py::self + py::self); + twist_vel.def(py::self - py::self); + twist_vel.def("__neg__", [](const TwistVel &a) + { + return operator-(a); + }, py::is_operator()); + twist_vel.def(py::pickle( + [](const TwistVel &tv) + { // __getstate__ + /* Return a tuple that fully encodes the state of the object */ + return py::make_tuple(tv.vel, tv.rot); + }, + [](py::tuple t) + { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + + /* Create a new C++ instance */ + TwistVel tv(t[0].cast(), t[1].cast()); + return tv; + })); + + m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero); + m.def("Equal", (bool (*)(const TwistVel&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const Twist&, const TwistVel&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + m.def("Equal", (bool (*)(const TwistVel&, const Twist&, double)) &KDL::Equal, + py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); + + // -------------------- // RotationVel // -------------------- @@ -228,65 +287,6 @@ void init_framevel(pybind11::module &m) py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); - // -------------------- - // TwistVel - // -------------------- - py::class_ twist_vel(m, "TwistVel"); - twist_vel.def_readwrite("vel", &TwistVel::vel); - twist_vel.def_readwrite("rot", &TwistVel::rot); - twist_vel.def(py::init<>()); - twist_vel.def(py::init()); - twist_vel.def(py::init()); - twist_vel.def(py::init()); - twist_vel.def("value", &TwistVel::value); - twist_vel.def("deriv", &TwistVel::deriv); - twist_vel.def_static("Zero", &TwistVel::Zero); - twist_vel.def("ReverseSign", &TwistVel::ReverseSign); - twist_vel.def("RefPoint", &TwistVel::RefPoint); - twist_vel.def("GetTwist", &TwistVel::GetTwist); - twist_vel.def("GetTwistDot", &TwistVel::GetTwistDot); - - twist_vel.def(py::self -= py::self); - twist_vel.def(py::self += py::self); - twist_vel.def(py::self * double()); - twist_vel.def(double() * py::self); - twist_vel.def(py::self / double()); - - twist_vel.def(py::self * doubleVel()); - twist_vel.def(doubleVel() * py::self); - twist_vel.def(py::self / doubleVel()); - - twist_vel.def(py::self + py::self); - twist_vel.def(py::self - py::self); - twist_vel.def("__neg__", [](const TwistVel &a) - { - return operator-(a); - }, py::is_operator()); - twist_vel.def(py::pickle( - [](const TwistVel &tv) - { // __getstate__ - /* Return a tuple that fully encodes the state of the object */ - return py::make_tuple(tv.vel, tv.rot); - }, - [](py::tuple t) - { // __setstate__ - if (t.size() != 2) - throw std::runtime_error("Invalid state!"); - - /* Create a new C++ instance */ - TwistVel tv(t[0].cast(), t[1].cast()); - return tv; - })); - - m.def("SetToZero", (void (*)(TwistVel&)) &KDL::SetToZero); - m.def("Equal", (bool (*)(const TwistVel&, const TwistVel&, double)) &KDL::Equal, - py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); - m.def("Equal", (bool (*)(const Twist&, const TwistVel&, double)) &KDL::Equal, - py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); - m.def("Equal", (bool (*)(const TwistVel&, const Twist&, double)) &KDL::Equal, - py::arg("a"), py::arg("b"), py::arg("eps")=epsilon); - - // -------------------- // Global // -------------------- diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index d47cf7a11..7cab3cbf8 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -192,6 +192,50 @@ void init_kinfam(pybind11::module &m) }); + // -------------------- + // Jacobian + // -------------------- + py::class_ jacobian(m, "Jacobian"); + jacobian.def(py::init()); + jacobian.def(py::init()); + jacobian.def("rows", &Jacobian::rows); + jacobian.def("columns", &Jacobian::columns); + jacobian.def("resize", &Jacobian::resize); + jacobian.def("getColumn", &Jacobian::getColumn); + jacobian.def("setColumn", &Jacobian::setColumn); + jacobian.def("changeRefPoint", &Jacobian::changeRefPoint); + jacobian.def("changeBase", &Jacobian::changeBase); + jacobian.def("changeRefFrame", &Jacobian::changeRefFrame); + jacobian.def("__getitem__", [](const Jacobian &jac, std::tuple idx) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j > jac.columns()) + throw py::index_error("Jacobian index out of range"); + return jac((unsigned int)i, (unsigned int)j); + }); + jacobian.def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 5 || j < 0 || j > jac.columns()) + throw py::index_error("Jacobian index out of range"); + + jac((unsigned int)i, (unsigned int)j) = value; + }); + jacobian.def("__repr__", [](const Jacobian &jac) + { + std::ostringstream oss; + oss << jac; + return oss.str(); + }); + + m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero); + m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint); + m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase); + m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame); + + // -------------------- // JntArray // -------------------- @@ -259,49 +303,6 @@ void init_kinfam(pybind11::module &m) py::arg("src1"), py::arg("src2"), py::arg("eps")=epsilon); - // -------------------- - // Jacobian - // -------------------- - py::class_ jacobian(m, "Jacobian"); - jacobian.def(py::init()); - jacobian.def(py::init()); - jacobian.def("rows", &Jacobian::rows); - jacobian.def("columns", &Jacobian::columns); - jacobian.def("resize", &Jacobian::resize); - jacobian.def("getColumn", &Jacobian::getColumn); - jacobian.def("setColumn", &Jacobian::setColumn); - jacobian.def("changeRefPoint", &Jacobian::changeRefPoint); - jacobian.def("changeBase", &Jacobian::changeBase); - jacobian.def("changeRefFrame", &Jacobian::changeRefFrame); - jacobian.def("__getitem__", [](const Jacobian &jac, std::tuple idx) - { - int i = std::get<0>(idx); - int j = std::get<1>(idx); - if (i < 0 || i > 5 || j < 0 || j > jac.columns()) - throw py::index_error("Jacobian index out of range"); - return jac((unsigned int)i, (unsigned int)j); - }); - jacobian.def("__setitem__", [](Jacobian &jac, std::tuple idx, double value) - { - int i = std::get<0>(idx); - int j = std::get<1>(idx); - if (i < 0 || i > 5 || j < 0 || j > jac.columns()) - throw py::index_error("Jacobian index out of range"); - jac((unsigned int)i, (unsigned int)j) = value; - }); - jacobian.def("__repr__", [](const Jacobian &jac) - { - std::ostringstream oss; - oss << jac; - return oss.str(); - }); - - m.def("SetToZero", (void (*)(Jacobian&)) &KDL::SetToZero); - m.def("changeRefPoint", (void (*)(const Jacobian&, const Vector&, Jacobian&)) &KDL::changeRefPoint); - m.def("changeBase", (void (*)(const Jacobian&, const Rotation&, Jacobian&)) &KDL::changeBase); - m.def("SetToZero", (void (*)(const Jacobian&, const Frame&, Jacobian&)) &KDL::changeRefFrame); - - // -------------------- // SolverI // -------------------- From 8ed2b21766b6ed7391654d1b1d772fa3179d6117 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 20:43:37 +0100 Subject: [PATCH 018/136] remove unneeded uint cast in frame __getitem__ --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 3759b452c..521c1dab1 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -359,7 +359,7 @@ void init_frames(py::module &m) if (i < 0 || i > 2 || j < 0 || j > 3) throw py::index_error("Frame index out of range"); - return frm((unsigned int)i, (unsigned int)j); + return frm(i, j); }); frame.def("__setitem__", [](Frame &frm, std::tuple idx, double value) { From 27d92f76fdf3b9a61eda92aec4293124fcaad272 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 16 Feb 2020 22:49:32 +0100 Subject: [PATCH 019/136] Add pybind11 eigen support --- python_orocos_kdl/PyKDL/pybind11/PyKDL.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h index aa188ca5f..78bc0d889 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h @@ -22,6 +22,7 @@ #include +#include #include #include From d4095f0f92b8ef919fd808008741915e8b3271e1 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 17 Feb 2020 09:51:59 +0100 Subject: [PATCH 020/136] Add __setitem__ to jnt_space_inertia_matrix --- python_orocos_kdl/PyKDL/pybind11/dynamics.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp index 5bb0afa3c..fef190194 100644 --- a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp @@ -52,6 +52,15 @@ void init_dynamics(pybind11::module &m) return jm((unsigned int)i, (unsigned int)j); }); + jnt_space_inertia_matrix.def("__setitem__", [](JntSpaceInertiaMatrix &jm, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > jm.rows() || j < 0 || j > jm.columns()) + throw py::index_error("Inertia index out of range"); + + jm((unsigned int)i, (unsigned int)j) = value; + }); jnt_space_inertia_matrix.def("__repr__", [](const JntSpaceInertiaMatrix &jm) { std::ostringstream oss; From 9d5863883cca7cf8fcc73c2739da5b1a7d706a26 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 17 Feb 2020 09:52:30 +0100 Subject: [PATCH 021/136] fix __getitem__ of rotation --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 521c1dab1..51f0c725e 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -246,10 +246,12 @@ void init_frames(py::module &m) rotation.def(py::init()); rotation.def(py::init()); rotation.def(py::init()); - rotation.def("__getitem__", [](const Rotation &r, int i, int j) + rotation.def("__getitem__", [](const Rotation &r, std::tuple idx) { + int i = std::get<0>(idx); + int j = std::get<1>(idx); if (i < 0 || i > 2 || j < 0 || j > 2) - throw py::index_error("Rotation index out of range"); + throw py::index_error("Rotation index out of range"); return r(i, j); }); From 194c762bcb9ce93ad798564b69e92cdb891ed685 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 17 Feb 2020 09:52:49 +0100 Subject: [PATCH 022/136] Add __setitem__ to rotation --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 51f0c725e..848da49f0 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -255,6 +255,15 @@ void init_frames(py::module &m) return r(i, j); }); + rotation.def("__setitem__", [](Rotation &r, std::tuple idx, double value) + { + int i = std::get<0>(idx); + int j = std::get<1>(idx); + if (i < 0 || i > 2 || j < 0 || j > 2) + throw py::index_error("Rotation index out of range"); + + r(i, j) = value; + }); rotation.def("__repr__", [](const Rotation &r) { std::ostringstream oss; From 2c7b6339646ff3e46edfac37694518dd34f20457 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 17 Feb 2020 13:47:04 +0100 Subject: [PATCH 023/136] (PyKDL) remove manifest.xml --- python_orocos_kdl/manifest.xml | 22 ---------------------- 1 file changed, 22 deletions(-) delete mode 100644 python_orocos_kdl/manifest.xml diff --git a/python_orocos_kdl/manifest.xml b/python_orocos_kdl/manifest.xml deleted file mode 100644 index ae3cd5ac7..000000000 --- a/python_orocos_kdl/manifest.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - python_orocos_kdl - - - Ruben Smits - LGPL - - http://ros.org/wiki/python_orocos_kdl - - - - - - - - - - - - From 9d7b663991544971a748a24735c16b1f607b0ea3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 17 Feb 2020 13:53:35 +0100 Subject: [PATCH 024/136] (PyKDL) remove mainpage.dox --- python_orocos_kdl/mainpage.dox | 26 -------------------------- 1 file changed, 26 deletions(-) delete mode 100644 python_orocos_kdl/mainpage.dox diff --git a/python_orocos_kdl/mainpage.dox b/python_orocos_kdl/mainpage.dox deleted file mode 100644 index 0be6e3112..000000000 --- a/python_orocos_kdl/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b python_orocos_kdl is ... - - - - -\section codeapi Code API - - - - -*/ From 65b1d8fd2464758e276d6611ffb1f04dd98cb90c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 19 Feb 2020 00:28:37 +0100 Subject: [PATCH 025/136] (PyKDL) Add properties to Vector --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 848da49f0..99a6f0174 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -44,6 +44,9 @@ void init_frames(py::module &m) vector.def("x", (double (Vector::*)(void) const) &Vector::x); vector.def("y", (double (Vector::*)(void) const) &Vector::y); vector.def("z", (double (Vector::*)(void) const) &Vector::z); + vector.def_property("x", (double (Vector::*)(void) const) &Vector::x, (void (Vector::*)(double)) &Vector::x); + vector.def_property("y", (double (Vector::*)(void) const) &Vector::y, (void (Vector::*)(double)) &Vector::y); + vector.def_property("z", (double (Vector::*)(void) const) &Vector::z, (void (Vector::*)(double)) &Vector::z); vector.def("__getitem__", [](const Vector &v, int i) { if (i < 0 || i > 2) From c22207726bf3785e379dabf7ba7cae14e74eed5e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 19 Feb 2020 00:30:15 +0100 Subject: [PATCH 026/136] (PyKDL) add comment to use properties in Vector --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 99a6f0174..c4f3ee1dc 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -38,12 +38,12 @@ void init_frames(py::module &m) vector.def(py::init<>()); vector.def(py::init()); vector.def(py::init()); - vector.def("x", (void (Vector::*)(double)) &Vector::x); - vector.def("y", (void (Vector::*)(double)) &Vector::y); - vector.def("z", (void (Vector::*)(double)) &Vector::z); - vector.def("x", (double (Vector::*)(void) const) &Vector::x); - vector.def("y", (double (Vector::*)(void) const) &Vector::y); - vector.def("z", (double (Vector::*)(void) const) &Vector::z); + vector.def("x", (void (Vector::*)(double)) &Vector::x, "Consider using the property 'x'"); + vector.def("y", (void (Vector::*)(double)) &Vector::y, "Consider using the property 'y'"); + vector.def("z", (void (Vector::*)(double)) &Vector::z, "Consider using the property 'z'"); + vector.def("x", (double (Vector::*)(void) const) &Vector::x, "Consider using the property 'x'"); + vector.def("y", (double (Vector::*)(void) const) &Vector::y, "Consider using the property 'y'"); + vector.def("z", (double (Vector::*)(void) const) &Vector::z, "Consider using the property 'z'"); vector.def_property("x", (double (Vector::*)(void) const) &Vector::x, (void (Vector::*)(double)) &Vector::x); vector.def_property("y", (double (Vector::*)(void) const) &Vector::y, (void (Vector::*)(double)) &Vector::y); vector.def_property("z", (double (Vector::*)(void) const) &Vector::z, (void (Vector::*)(double)) &Vector::z); From 4b5f8e17e9ac47007c884a07b5ebe9d1f3b19757 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 14 Mar 2020 10:20:57 +0100 Subject: [PATCH 027/136] (PyKDL) style update test --- python_orocos_kdl/tests/PyKDLtest.py | 2 + python_orocos_kdl/tests/framestest.py | 230 ++++++++++++------------ python_orocos_kdl/tests/frameveltest.py | 108 +++++------ python_orocos_kdl/tests/kinfamtest.py | 129 +++++++------ 4 files changed, 234 insertions(+), 235 deletions(-) diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index daf806b8c..bd555b2eb 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -19,6 +19,8 @@ # You should have received a copy of the GNU Lesser General Public # License along with this library; if not, write to the Free Software # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + import unittest import kinfamtest import framestest diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 267a87a30..993ecab14 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -24,150 +24,150 @@ from PyKDL import * from math import * -class FramesTestFunctions(unittest.TestCase): - def testVector2(self,v): - self.assertEqual(2*v-v,v) - self.assertEqual(v*2-v,v) - self.assertEqual(v+v+v-2*v,v) - v2=Vector(v) - self.assertEqual(v,v2) - v2+=v - self.assertEqual(2*v,v2) - v2-=v - self.assertEqual(v,v2) +class FramesTestFunctions(unittest.TestCase): + def testVector2(self, v): + self.assertEqual(2*v-v, v) + self.assertEqual(v*2-v, v) + self.assertEqual(v+v+v-2*v, v) + v2 = Vector(v) + self.assertEqual(v, v2) + v2 += v + self.assertEqual(2*v, v2) + v2 -= v + self.assertEqual(v, v2) v2.ReverseSign() - self.assertEqual(v,-v2) + self.assertEqual(v, -v2) def testVector(self): - v=Vector(3,4,5) + v = Vector(3, 4, 5) self.testVector2(v) - v=Vector.Zero() + v = Vector.Zero() self.testVector2(v) - def testTwist2(self,t): - self.assertEqual(2*t-t,t) - self.assertEqual(t*2-t,t) - self.assertEqual(t+t+t-2*t,t) - t2=Twist(t) - self.assertEqual(t,t2) - t2+=t - self.assertEqual(2*t,t2) - t2-=t - self.assertEqual(t,t2) + def testTwist2(self, t): + self.assertEqual(2*t-t, t) + self.assertEqual(t*2-t, t) + self.assertEqual(t+t+t-2*t, t) + t2 = Twist(t) + self.assertEqual(t, t2) + t2 += t + self.assertEqual(2*t, t2) + t2 -= t + self.assertEqual(t, t2) t.ReverseSign() - self.assertEqual(t,-t2) + self.assertEqual(t, -t2) def testTwist(self): - t=Twist(Vector(6,3,5),Vector(4,-2,7)) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) self.testTwist2(t) - t=Twist.Zero() + t = Twist.Zero() self.testTwist2(t) - t=Twist(Vector(0,-9,-3),Vector(1,-2,-4)) - - def testWrench2(self,w): - self.assertEqual(2*w-w,w) - self.assertEqual(w*2-w,w) - self.assertEqual(w+w+w-2*w,w) - w2=Wrench(w) - self.assertEqual(w,w2) - w2+=w - self.assertEqual(2*w,w2) - w2-=w - self.assertEqual(w,w2) + t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) + + def testWrench2(self, w): + self.assertEqual(2*w-w, w) + self.assertEqual(w*2-w, w) + self.assertEqual(w+w+w-2*w, w) + w2 = Wrench(w) + self.assertEqual(w, w2) + w2 += w + self.assertEqual(2*w, w2) + w2 -= w + self.assertEqual(w, w2) w.ReverseSign() - self.assertEqual(w,-w2) + self.assertEqual(w, -w2) def testWrench(self): - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) self.testWrench2(w) - w=Wrench.Zero() + w = Wrench.Zero() self.testWrench2(w) - w=Wrench(Vector(2,1,4),Vector(5,3,1)) + w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1)) self.testWrench2(w) - def testRotation2(self,v,a,b,c): - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - R=Rotation.RPY(a,b,c) - - self.assertAlmostEqual(dot(R.UnitX(),R.UnitX()),1.0,15) - self.assertEqual(dot(R.UnitY(),R.UnitY()),1.0) - self.assertEqual(dot(R.UnitZ(),R.UnitZ()),1.0) - self.assertAlmostEqual(dot(R.UnitX(),R.UnitY()),0.0,15) - self.assertAlmostEqual(dot(R.UnitX(),R.UnitZ()),0.0,15) - self.assertEqual(dot(R.UnitY(),R.UnitZ()),0.0) - R2=Rotation(R) - self.assertEqual(R,R2) - self.assertAlmostEqual((R*v).Norm(),v.Norm(),14) - self.assertEqual(R.Inverse(R*v),v) - self.assertEqual(R.Inverse(R*t),t) - self.assertEqual(R.Inverse(R*w),w) - self.assertEqual(R*R.Inverse(v),v) - self.assertEqual(R*Rotation.Identity(),R) - self.assertEqual(Rotation.Identity()*R,R) - self.assertEqual(R*(R*(R*v)),(R*R*R)*v) - self.assertEqual(R*(R*(R*t)),(R*R*R)*t) - self.assertEqual(R*(R*(R*w)),(R*R*R)*w) - self.assertEqual(R*R.Inverse(),Rotation.Identity()) - self.assertEqual(R.Inverse()*R,Rotation.Identity()) - self.assertEqual(R.Inverse()*v,R.Inverse(v)) - (ra,rb,rc)=R.GetRPY() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertEqual(rc,c) - R = Rotation.EulerZYX(a,b,c) - (ra,rb,rc)=R.GetEulerZYX() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertEqual(rc,c) - R = Rotation.EulerZYZ(a,b,c) - (ra,rb,rc)=R.GetEulerZYZ() - self.assertEqual(ra,a) - self.assertEqual(rb,b) - self.assertAlmostEqual(rc,c,15) - (angle,v2)= R.GetRotAngle() - R2=Rotation.Rot(v2,angle) - self.assertEqual(R2,R) - R2=Rotation.Rot(v2*1E20,angle) - self.assertEqual(R,R2) - v2=Vector(6,2,4) - self.assertAlmostEqual(v2.Norm(),sqrt(dot(v2,v2)),14) + def testRotation2(self, v, a, b, c): + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + R = Rotation.RPY(a, b, c) + + self.assertAlmostEqual(dot(R.UnitX(), R.UnitX()), 1.0, 15) + self.assertEqual(dot(R.UnitY(), R.UnitY()), 1.0) + self.assertEqual(dot(R.UnitZ(), R.UnitZ()), 1.0) + self.assertAlmostEqual(dot(R.UnitX(), R.UnitY()), 0.0, 15) + self.assertAlmostEqual(dot(R.UnitX(), R.UnitZ()), 0.0, 15) + self.assertEqual(dot(R.UnitY(), R.UnitZ()), 0.0) + R2 = Rotation(R) + self.assertEqual(R, R2) + self.assertAlmostEqual((R*v).Norm(), v.Norm(), 14) + self.assertEqual(R.Inverse(R*v), v) + self.assertEqual(R.Inverse(R*t), t) + self.assertEqual(R.Inverse(R*w), w) + self.assertEqual(R*R.Inverse(v), v) + self.assertEqual(R*Rotation.Identity(), R) + self.assertEqual(Rotation.Identity()*R, R) + self.assertEqual(R*(R*(R*v)), (R*R*R)*v) + self.assertEqual(R*(R*(R*t)), (R*R*R)*t) + self.assertEqual(R*(R*(R*w)), (R*R*R)*w) + self.assertEqual(R*R.Inverse(), Rotation.Identity()) + self.assertEqual(R.Inverse()*R, Rotation.Identity()) + self.assertEqual(R.Inverse()*v, R.Inverse(v)) + (ra, rb, rc) = R.GetRPY() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertEqual(rc, c) + R = Rotation.EulerZYX(a, b, c) + (ra, rb, rc) = R.GetEulerZYX() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertEqual(rc, c) + R = Rotation.EulerZYZ(a, b, c) + (ra, rb, rc) = R.GetEulerZYZ() + self.assertEqual(ra, a) + self.assertEqual(rb, b) + self.assertAlmostEqual(rc, c, 15) + (angle, v2) = R.GetRotAngle() + R2 = Rotation.Rot(v2, angle) + self.assertEqual(R2, R) + R2 = Rotation.Rot(v2*1e20, angle) + self.assertEqual(R, R2) + v2 = Vector(6, 2, 4) + self.assertAlmostEqual(v2.Norm(), sqrt(dot(v2, v2)), 14) def testRotation(self): - self.testRotation2(Vector(3,4,5),radians(10),radians(20),radians(30)) + self.testRotation2(Vector(3, 4, 5), radians(10), radians(20), radians(30)) def testFrame(self): - v=Vector(3,4,5) - w=Wrench(Vector(7,-1,3),Vector(2,-3,3)) - t=Twist(Vector(6,3,5),Vector(4,-2,7)) - F = Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1)) - F2=Frame(F) - self.assertEqual(F,F2) - self.assertEqual(F.Inverse(F*v),v) - self.assertEqual(F.Inverse(F*t),t) - self.assertEqual(F.Inverse(F*w),w) - self.assertEqual(F*F.Inverse(v),v) - self.assertEqual(F*F.Inverse(t),t) - self.assertEqual(F*F.Inverse(w),w) - self.assertEqual(F*Frame.Identity(),F) - self.assertEqual(Frame.Identity()*F,F) - self.assertEqual(F*(F*(F*v)),(F*F*F)*v) - self.assertEqual(F*(F*(F*t)),(F*F*F)*t) - self.assertEqual(F*(F*(F*w)),(F*F*F)*w) - self.assertEqual(F*F.Inverse(),Frame.Identity()) - self.assertEqual(F.Inverse()*F,Frame.Identity()) - self.assertEqual(F.Inverse()*v,F.Inverse(v)) + v = Vector(3, 4, 5) + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + F = Frame(Rotation.EulerZYX(radians(10), radians(20), radians(-10)), Vector(4, -2, 1)) + F2 = Frame(F) + self.assertEqual(F, F2) + self.assertEqual(F.Inverse(F*v), v) + self.assertEqual(F.Inverse(F*t), t) + self.assertEqual(F.Inverse(F*w), w) + self.assertEqual(F*F.Inverse(v), v) + self.assertEqual(F*F.Inverse(t), t) + self.assertEqual(F*F.Inverse(w), w) + self.assertEqual(F*Frame.Identity(), F) + self.assertEqual(Frame.Identity()*F, F) + self.assertEqual(F*(F*(F*v)), (F*F*F)*v) + self.assertEqual(F*(F*(F*t)), (F*F*F)*t) + self.assertEqual(F*(F*(F*w)), (F*F*F)*w) + self.assertEqual(F*F.Inverse(), Frame.Identity()) + self.assertEqual(F.Inverse()*F, Frame.Identity()) + self.assertEqual(F.Inverse()*v, F.Inverse(v)) def testPickle(self): # import pickle import cPickle as pickle data = {} - data['v'] = Vector(1,2,3) + data['v'] = Vector(1, 2, 3) data['rot'] = Rotation().EulerZYZ(1, 2, 3) data['fr'] = Frame(data['rot'], data['v']) - data['tw'] = Twist(data['v'], Vector(4,5,6)) - data['wr'] = Wrench(Vector(0.1,0.2,0.3), data['v']) + data['tw'] = Twist(data['v'], Vector(4, 5, 6)) + data['wr'] = Wrench(Vector(0.1, 0.2, 0.3), data['v']) f = open('/tmp/pickle_test', 'w') pickle.dump(data, f, 2) @@ -181,7 +181,7 @@ def testPickle(self): def suite(): - suite=unittest.TestSuite() + suite = unittest.TestSuite() suite.addTest(FramesTestFunctions('testVector')) suite.addTest(FramesTestFunctions('testTwist')) suite.addTest(FramesTestFunctions('testWrench')) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index a7bb6267a..73d651b62 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -24,71 +24,71 @@ from PyKDL import * from math import * -class FrameVelTestFunctions(unittest.TestCase): +class FrameVelTestFunctions(unittest.TestCase): def testVectorVel(self): - v=VectorVel(Vector(3,-4,5),Vector(6,3,-5)) - vt = Vector(-4,-6,-8); - self.assertTrue(Equal( 2*v-v,v)) - self.assertTrue(Equal( v*2-v,v)) - self.assertTrue(Equal( v+v+v-2*v,v)) - v2=VectorVel(v) - self.assertTrue(Equal( v,v2)) - v2+=v - self.assertTrue(Equal( 2*v,v2)) - v2-=v - self.assertTrue(Equal( v,v2)) + v = VectorVel(Vector(3, -4, 5), Vector(6, 3, -5)) + vt = Vector(-4, -6, -8) + self.assertTrue(Equal(2*v-v, v)) + self.assertTrue(Equal(v*2-v, v)) + self.assertTrue(Equal(v+v+v-2*v, v)) + v2 = VectorVel(v) + self.assertTrue(Equal(v, v2)) + v2 += v + self.assertTrue(Equal(2*v, v2)) + v2 -= v + self.assertTrue(Equal(v, v2)) v2.ReverseSign() - self.assertTrue(Equal( v,-v2)) - self.assertTrue(Equal( v*vt,-vt*v)) - v2 = VectorVel(Vector(-5,-6,-3),Vector(3,4,5)) - self.assertTrue(Equal( v*v2,-v2*v)) + self.assertTrue(Equal(v, -v2)) + self.assertTrue(Equal(v*vt, -vt*v)) + v2 = VectorVel(Vector(-5, -6, -3), Vector(3, 4, 5)) + self.assertTrue(Equal(v*v2, -v2*v)) def testRotationVel(self): - v=VectorVel(Vector(9,4,-2),Vector(-5,6,-2)) - vt=Vector(2,3,4) - a= radians(-15) - b= radians(20) - c= radians(-80) - R = RotationVel(Rotation.RPY(a,b,c),Vector(2,4,1)) - R2=RotationVel(R) - self.assertTrue(Equal(R,R2)) - self.assertTrue(Equal((R*v).Norm(),(v.Norm()))) - self.assertTrue(Equal(R.Inverse(R*v),v)) - self.assertTrue(Equal(R*R.Inverse(v),v)) - self.assertTrue(Equal(R*Rotation.Identity(),R)) - self.assertTrue(Equal(Rotation.Identity()*R,R)) - self.assertTrue(Equal(R*(R*(R*v)),(R*R*R)*v)) - self.assertTrue(Equal(R*(R*(R*vt)),(R*R*R)*vt)) - self.assertTrue(Equal(R*R.Inverse(),RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*R,RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*v,R.Inverse(v))) + v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2)) + vt = Vector(2, 3, 4) + a = radians(-15) + b = radians(20) + c = radians(-80) + R = RotationVel(Rotation.RPY(a, b, c), Vector(2, 4, 1)) + R2 = RotationVel(R) + self.assertTrue(Equal(R, R2)) + self.assertTrue(Equal((R*v).Norm(), (v.Norm()))) + self.assertTrue(Equal(R.Inverse(R*v), v)) + self.assertTrue(Equal(R*R.Inverse(v), v)) + self.assertTrue(Equal(R*Rotation.Identity(), R)) + self.assertTrue(Equal(Rotation.Identity()*R, R)) + self.assertTrue(Equal(R*(R*(R*v)), (R*R*R)*v)) + self.assertTrue(Equal(R*(R*(R*vt)), (R*R*R)*vt)) + self.assertTrue(Equal(R*R.Inverse(), RotationVel.Identity())) + self.assertTrue(Equal(R.Inverse()*R, RotationVel.Identity())) + self.assertTrue(Equal(R.Inverse()*v, R.Inverse(v))) def testFrameVel(self): - v=VectorVel(Vector(3,4,5),Vector(-2,-4,-1)) - vt=Vector(-1,0,-10) - F = FrameVel(Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1)), - Twist(Vector(2,-2,-2),Vector(-5,-3,-2))) - F2=FrameVel(F) - self.assertTrue(Equal( F,F2)) - self.assertTrue(Equal( F.Inverse(F*v),v)) - self.assertTrue(Equal( F.Inverse(F*vt), vt)) - self.assertTrue(Equal( F*F.Inverse(v),v)) - self.assertTrue(Equal( F*F.Inverse(vt),vt)) - self.assertTrue(Equal( F*Frame.Identity(),F)) - self.assertTrue(Equal( Frame.Identity()*F,F)) - self.assertTrue(Equal( F*(F*(F*v)),(F*F*F)*v)) - self.assertTrue(Equal( F*(F*(F*vt)),(F*F*F)*vt)) - self.assertTrue(Equal( F*F.Inverse(),FrameVel.Identity())) - self.assertTrue(Equal( F.Inverse()*F,Frame.Identity())) - self.assertTrue(Equal( F.Inverse()*vt,F.Inverse(vt))) + v = VectorVel(Vector(3, 4, 5), Vector(-2, -4, -1)) + vt = Vector(-1, 0, -10) + F = FrameVel(Frame(Rotation.EulerZYX(radians(10), radians(20), radians(-10)), Vector(4, -2, 1)), + Twist(Vector(2, -2, -2), Vector(-5, -3, -2))) + F2 = FrameVel(F) + self.assertTrue(Equal(F, F2)) + self.assertTrue(Equal(F.Inverse(F*v), v)) + self.assertTrue(Equal(F.Inverse(F*vt), vt)) + self.assertTrue(Equal(F*F.Inverse(v), v)) + self.assertTrue(Equal(F*F.Inverse(vt), vt)) + self.assertTrue(Equal(F*Frame.Identity(), F)) + self.assertTrue(Equal(Frame.Identity()*F, F)) + self.assertTrue(Equal(F*(F*(F*v)), (F*F*F)*v)) + self.assertTrue(Equal(F*(F*(F*vt)), (F*F*F)*vt)) + self.assertTrue(Equal(F*F.Inverse(), FrameVel.Identity())) + self.assertTrue(Equal(F.Inverse()*F, Frame.Identity())) + self.assertTrue(Equal(F.Inverse()*vt, F.Inverse(vt))) def testPickle(self): rot = Rotation.RotX(1.3) import cPickle as pickle data = {} - data['vv'] = VectorVel(Vector(1,2,3), Vector(4,5,6)) - data['rv'] = RotationVel(rot, Vector(4.1,5.1,6.1)) + data['vv'] = VectorVel(Vector(1, 2, 3), Vector(4, 5, 6)) + data['rv'] = RotationVel(rot, Vector(4.1, 5.1, 6.1)) data['fv'] = FrameVel(data['rv'], data['vv']) data['tv'] = TwistVel(data['vv'], data['vv']) @@ -115,7 +115,7 @@ def testPickle(self): def suite(): - suite=unittest.TestSuite() + suite = unittest.TestSuite() suite.addTest(FrameVelTestFunctions('testVectorVel')) suite.addTest(FrameVelTestFunctions('testRotationVel')) suite.addTest(FrameVelTestFunctions('testFrameVel')) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 6983a45ac..51f4dddfa 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -21,130 +21,127 @@ import gc -import random -import unittest -from math import * - import psutil from PyKDL import * +import random +import unittest class KinfamTestFunctions(unittest.TestCase): - def setUp(self): self.chain = Chain() self.chain.addSegment(Segment(Joint(Joint.RotZ), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotX), - Frame(Vector(0.0,0.0,0.9)))) + Frame(Vector(0.0, 0.0, 0.9)))) self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(-0.4,0.0,0.0)))) + Frame(Vector(-0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotY), - Frame(Vector(0.0,0.0,1.2)))) + Frame(Vector(0.0, 0.0, 1.2)))) self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(0.4,0.0,0.0)))) + Frame(Vector(0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransZ), - Frame(Vector(0.0,0.0,1.4)))) + Frame(Vector(0.0, 0.0, 1.4)))) self.chain.addSegment(Segment(Joint(Joint.TransX), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransY), - Frame(Vector(0.0,0.0,0.4)))) + Frame(Vector(0.0, 0.0, 0.4)))) self.chain.addSegment(Segment(Joint(Joint.None), - Frame(Vector(0.0,0.0,0.0)))) + Frame(Vector(0.0, 0.0, 0.0)))) - self.jacsolver = ChainJntToJacSolver(self.chain) + self.jacsolver = ChainJntToJacSolver(self.chain) self.fksolverpos = ChainFkSolverPos_recursive(self.chain) self.fksolvervel = ChainFkSolverVel_recursive(self.chain) self.iksolvervel = ChainIkSolverVel_pinv(self.chain) - self.iksolverpos = ChainIkSolverPos_NR(self.chain,self.fksolverpos,self.iksolvervel) + self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel) def testFkPosAndJac(self): deltaq = 1E-4 epsJ = 1E-4 - F1=Frame() - F2=Frame() + F1 = Frame() + F2 = Frame() - q=JntArray(self.chain.getNrOfJoints()) - jac=Jacobian(self.chain.getNrOfJoints()) + q = JntArray(self.chain.getNrOfJoints()) + jac = Jacobian(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) - self.jacsolver.JntToJac(q,jac) + self.jacsolver.JntToJac(q, jac) for i in range(q.rows()): - oldqi=q[i]; - q[i]=oldqi+deltaq - self.assertTrue(0==self.fksolverpos.JntToCart(q,F2)) - q[i]=oldqi-deltaq - self.assertTrue(0==self.fksolverpos.JntToCart(q,F1)) - q[i]=oldqi - Jcol1 = diff(F1,F2,2*deltaq) - Jcol2 = Twist(Vector(jac[0,i],jac[1,i],jac[2,i]), - Vector(jac[3,i],jac[4,i],jac[5,i])) - self.assertEqual(Jcol1,Jcol2); + oldqi = q[i] + q[i] = oldqi+deltaq + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F2)) + q[i] = oldqi-deltaq + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1)) + q[i] = oldqi + Jcol1 = diff(F1, F2, 2*deltaq) + Jcol2 = Twist(Vector(jac[0, i], jac[1, i], jac[2, i]), + Vector(jac[3, i], jac[4, i], jac[5, i])) + self.assertEqual(Jcol1, Jcol2) def testFkVelAndJac(self): deltaq = 1E-4 - epsJ = 1E-4 + epsJ = 1E-4 - q=JntArray(self.chain.getNrOfJoints()) - qdot=JntArray(self.chain.getNrOfJoints()) + q = JntArray(self.chain.getNrOfJoints()) + qdot = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - qdot[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) + qdot[i] = random.uniform(-3.14, 3.14) - qvel=JntArrayVel(q,qdot); - jac=Jacobian(self.chain.getNrOfJoints()) + qvel = JntArrayVel(q, qdot) + jac = Jacobian(self.chain.getNrOfJoints()) - cart = FrameVel.Identity(); - t = Twist.Zero(); + cart = FrameVel.Identity() + t = Twist.Zero() - self.jacsolver.JntToJac(qvel.q,jac) - self.assertTrue(self.fksolvervel.JntToCart(qvel,cart)==0) - MultiplyJacobian(jac,qvel.qdot,t) - self.assertEqual(cart.deriv(),t) + self.jacsolver.JntToJac(qvel.q, jac) + self.assertTrue(self.fksolvervel.JntToCart(qvel, cart) == 0) + MultiplyJacobian(jac, qvel.qdot, t) + self.assertEqual(cart.deriv(), t) def testFkVelAndIkVel(self): epsJ = 1E-7 - q=JntArray(self.chain.getNrOfJoints()) - qdot=JntArray(self.chain.getNrOfJoints()) + q = JntArray(self.chain.getNrOfJoints()) + qdot = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) - qdot[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) + qdot[i] = random.uniform(-3.14, 3.14) - qvel=JntArrayVel(q,qdot) - qdot_solved=JntArray(self.chain.getNrOfJoints()) + qvel = JntArrayVel(q, qdot) + qdot_solved = JntArray(self.chain.getNrOfJoints()) cart = FrameVel() - self.assertTrue(0==self.fksolvervel.JntToCart(qvel,cart)) - self.assertTrue(0==self.iksolvervel.CartToJnt(qvel.q,cart.deriv(),qdot_solved)) + self.assertTrue(0 == self.fksolvervel.JntToCart(qvel, cart)) + self.assertTrue(0 == self.iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved)) - self.assertEqual(qvel.qdot,qdot_solved); + self.assertEqual(qvel.qdot, qdot_solved) def testFkPosAndIkPos(self): - q=JntArray(self.chain.getNrOfJoints()) + q = JntArray(self.chain.getNrOfJoints()) for i in range(q.rows()): - q[i]=random.uniform(-3.14,3.14) + q[i] = random.uniform(-3.14, 3.14) - q_init=JntArray(self.chain.getNrOfJoints()) + q_init = JntArray(self.chain.getNrOfJoints()) for i in range(q_init.rows()): - q_init[i]=q[i]+0.1*random.random() + q_init[i] = q[i]+0.1*random.random() - q_solved=JntArray(q.rows()) + q_solved = JntArray(q.rows()) - F1=Frame.Identity() - F2=Frame.Identity() + F1 = Frame.Identity() + F2 = Frame.Identity() - self.assertTrue(0==self.fksolverpos.JntToCart(q,F1)) - self.assertTrue(0==self.iksolverpos.CartToJnt(q_init,F1,q_solved)) - self.assertTrue(0==self.fksolverpos.JntToCart(q_solved,F2)) + self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1)) + self.assertTrue(0 == self.iksolverpos.CartToJnt(q_init, F1, q_solved)) + self.assertTrue(0 == self.fksolverpos.JntToCart(q_solved, F2)) - self.assertEqual(F1,F2) - self.assertEqual(q,q_solved) + self.assertEqual(F1, F2) + self.assertEqual(q, q_solved) class KinfamTestTree(unittest.TestCase): From 00b87170097ec2d1f8cb5b068ee08253b8ba5c04 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 14 Mar 2020 10:13:17 +0100 Subject: [PATCH 028/136] (PyKDL) Update tests --- python_orocos_kdl/tests/framestest.py | 84 ++++++++++++++++++--------- 1 file changed, 56 insertions(+), 28 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 993ecab14..3ad65aeea 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -26,7 +26,41 @@ class FramesTestFunctions(unittest.TestCase): - def testVector2(self, v): + def testVector(self): + v = Vector(3, 4, 5) + self.testVectorImpl(v) + v = Vector() + self.testVectorImpl(v) + v = Vector.Zero() + self.testVectorImpl(v) + + v = Vector(3, 4, 5) + self.assertFalse(v == -v) # Doesn't work for zero vector + self.assertTrue(v != -v) # Doesn't work for zero vector + self.assertEqual(v.x(), 3) + v.x(1) + self.assertEqual(v, Vector(1, 4, 5)) + self.assertEqual(v.y(), 4) + v.y(1) + self.assertEqual(v, Vector(1, 1, 5)) + self.assertEqual(v.z(), 5) + v.z(1) + self.assertEqual(v, Vector(1, 1, 1)) + self.assertEqual(v[1], 1) + self.assertEqual(v[2], 1) + v[0] = 3 + self.assertEqual(v[0], 3) + v[1] = 4 + self.assertEqual(v[1], 4) + v[2] = 5 + self.assertEqual(v[2], 5) + + SetToZero(v) + self.assertEqual(v, Vector(0, 0, 0)) + + def testVectorImpl(self, v): + self.assertTrue(v == v) + self.assertFalse(v != v) self.assertEqual(2*v-v, v) self.assertEqual(v*2-v, v) self.assertEqual(v+v+v-2*v, v) @@ -39,13 +73,15 @@ def testVector2(self, v): v2.ReverseSign() self.assertEqual(v, -v2) - def testVector(self): - v = Vector(3, 4, 5) - self.testVector2(v) - v = Vector.Zero() - self.testVector2(v) + def testTwist(self): + t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) + self.testTwistImpl(t) + t = Twist.Zero() + self.testTwistImpl(t) + t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) + self.testTwistImpl(t) - def testTwist2(self, t): + def testTwistImpl(self, t): self.assertEqual(2*t-t, t) self.assertEqual(t*2-t, t) self.assertEqual(t+t+t-2*t, t) @@ -58,14 +94,15 @@ def testTwist2(self, t): t.ReverseSign() self.assertEqual(t, -t2) - def testTwist(self): - t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) - self.testTwist2(t) - t = Twist.Zero() - self.testTwist2(t) - t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) + def testWrench(self): + w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) + self.testWrenchImpl(w) + w = Wrench.Zero() + self.testWrenchImpl(w) + w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1)) + self.testWrenchImpl(w) - def testWrench2(self, w): + def testWrenchImpl(self, w): self.assertEqual(2*w-w, w) self.assertEqual(w*2-w, w) self.assertEqual(w+w+w-2*w, w) @@ -78,15 +115,10 @@ def testWrench2(self, w): w.ReverseSign() self.assertEqual(w, -w2) - def testWrench(self): - w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) - self.testWrench2(w) - w = Wrench.Zero() - self.testWrench2(w) - w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1)) - self.testWrench2(w) + def testRotation(self): + self.testRotationImpl(Vector(3, 4, 5), radians(10), radians(20), radians(30)) - def testRotation2(self, v, a, b, c): + def testRotationImpl(self, v, a, b, c): w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) R = Rotation.RPY(a, b, c) @@ -129,14 +161,11 @@ def testRotation2(self, v, a, b, c): (angle, v2) = R.GetRotAngle() R2 = Rotation.Rot(v2, angle) self.assertEqual(R2, R) - R2 = Rotation.Rot(v2*1e20, angle) + R2 = Rotation.Rot(v2*1E20, angle) self.assertEqual(R, R2) v2 = Vector(6, 2, 4) self.assertAlmostEqual(v2.Norm(), sqrt(dot(v2, v2)), 14) - def testRotation(self): - self.testRotation2(Vector(3, 4, 5), radians(10), radians(20), radians(30)) - def testFrame(self): v = Vector(3, 4, 5) w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) @@ -160,9 +189,8 @@ def testFrame(self): self.assertEqual(F.Inverse()*v, F.Inverse(v)) def testPickle(self): - # import pickle import cPickle as pickle - data = {} + data = dict() data['v'] = Vector(1, 2, 3) data['rot'] = Rotation().EulerZYZ(1, 2, 3) data['fr'] = Frame(data['rot'], data['v']) From b0217ad9f6629e2839c668ba7b68dc7bcf9e7c19 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 09:33:08 +0100 Subject: [PATCH 029/136] Revert "(PyKDL) add comment to use properties in Vector" This reverts commit 3c9f22df5c8db5fe5e63d06d26145ff7adad646c. --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index c4f3ee1dc..99a6f0174 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -38,12 +38,12 @@ void init_frames(py::module &m) vector.def(py::init<>()); vector.def(py::init()); vector.def(py::init()); - vector.def("x", (void (Vector::*)(double)) &Vector::x, "Consider using the property 'x'"); - vector.def("y", (void (Vector::*)(double)) &Vector::y, "Consider using the property 'y'"); - vector.def("z", (void (Vector::*)(double)) &Vector::z, "Consider using the property 'z'"); - vector.def("x", (double (Vector::*)(void) const) &Vector::x, "Consider using the property 'x'"); - vector.def("y", (double (Vector::*)(void) const) &Vector::y, "Consider using the property 'y'"); - vector.def("z", (double (Vector::*)(void) const) &Vector::z, "Consider using the property 'z'"); + vector.def("x", (void (Vector::*)(double)) &Vector::x); + vector.def("y", (void (Vector::*)(double)) &Vector::y); + vector.def("z", (void (Vector::*)(double)) &Vector::z); + vector.def("x", (double (Vector::*)(void) const) &Vector::x); + vector.def("y", (double (Vector::*)(void) const) &Vector::y); + vector.def("z", (double (Vector::*)(void) const) &Vector::z); vector.def_property("x", (double (Vector::*)(void) const) &Vector::x, (void (Vector::*)(double)) &Vector::x); vector.def_property("y", (double (Vector::*)(void) const) &Vector::y, (void (Vector::*)(double)) &Vector::y); vector.def_property("z", (double (Vector::*)(void) const) &Vector::z, (void (Vector::*)(double)) &Vector::z); From 8b60991cad92cfdfa1fa0eb31a14fc36b8f80120 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 09:33:11 +0100 Subject: [PATCH 030/136] Revert "(PyKDL) Add properties to Vector" This reverts commit 9968bcbdb7dee36b0b5c16663dbde747f19f2126. --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 99a6f0174..848da49f0 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -44,9 +44,6 @@ void init_frames(py::module &m) vector.def("x", (double (Vector::*)(void) const) &Vector::x); vector.def("y", (double (Vector::*)(void) const) &Vector::y); vector.def("z", (double (Vector::*)(void) const) &Vector::z); - vector.def_property("x", (double (Vector::*)(void) const) &Vector::x, (void (Vector::*)(double)) &Vector::x); - vector.def_property("y", (double (Vector::*)(void) const) &Vector::y, (void (Vector::*)(double)) &Vector::y); - vector.def_property("z", (double (Vector::*)(void) const) &Vector::z, (void (Vector::*)(double)) &Vector::z); vector.def("__getitem__", [](const Vector &v, int i) { if (i < 0 || i > 2) From 009e2eeef9eb9d9a88d7e03ca0f097dd8f38b1f4 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 10:17:38 +0100 Subject: [PATCH 031/136] (PyKDL) extend Vector tests --- python_orocos_kdl/tests/framestest.py | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 3ad65aeea..d42253818 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -31,12 +31,13 @@ def testVector(self): self.testVectorImpl(v) v = Vector() self.testVectorImpl(v) - v = Vector.Zero() - self.testVectorImpl(v) v = Vector(3, 4, 5) self.assertFalse(v == -v) # Doesn't work for zero vector + self.assertFalse(Equal(v, -v)) # Doesn't work for zero vector self.assertTrue(v != -v) # Doesn't work for zero vector + self.assertTrue(not Equal(v, -v)) # Doesn't work for zero vector + # Test member get and set functions self.assertEqual(v.x(), 3) v.x(1) self.assertEqual(v, Vector(1, 4, 5)) @@ -46,21 +47,35 @@ def testVector(self): self.assertEqual(v.z(), 5) v.z(1) self.assertEqual(v, Vector(1, 1, 1)) + # Test __getitem__ + self.assertEqual(v[0], 1) self.assertEqual(v[1], 1) self.assertEqual(v[2], 1) + with self.assertRaises(IndexError): + _ = v[-1] + with self.assertRaises(IndexError): + _ = v[3] + # Test __setitem__ v[0] = 3 self.assertEqual(v[0], 3) v[1] = 4 self.assertEqual(v[1], 4) v[2] = 5 self.assertEqual(v[2], 5) + with self.assertRaises(IndexError): + v[-1] = 1 + with self.assertRaises(IndexError): + v[3] = 1 SetToZero(v) self.assertEqual(v, Vector(0, 0, 0)) + self.assertEqual(Vector.Zero(), Vector(0, 0, 0)) def testVectorImpl(self, v): self.assertTrue(v == v) + self.assertTrue(Equal(v, v)) self.assertFalse(v != v) + self.assertFalse(not Equal(v, v)) self.assertEqual(2*v-v, v) self.assertEqual(v*2-v, v) self.assertEqual(v+v+v-2*v, v) @@ -72,6 +87,11 @@ def testVectorImpl(self, v): self.assertEqual(v, v2) v2.ReverseSign() self.assertEqual(v, -v2) + for v2 in [Vector(v), -Vector(v)]: + self.assertAlmostEqual(v2.Norm()**2, sum(map(lambda i: i * i, v2)), delta=1e-10) + self.assertEqual(v2.Norm(), v2.Normalize()) # Norm before Normalize, so taking norm of un-normalized vector + + self.assertEqual(dot(v, v), sum(map(lambda i: i * i, v))) def testTwist(self): t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) From 8754311514c93aa3d9156014a730dbb4e10b1938 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 10:49:42 +0100 Subject: [PATCH 032/136] (PyKDL) extend Twist tests --- python_orocos_kdl/tests/framestest.py | 49 +++++++++++++++++++++++++-- 1 file changed, 47 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index d42253818..2264a4bcd 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -96,12 +96,48 @@ def testVectorImpl(self, v): def testTwist(self): t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) self.testTwistImpl(t) - t = Twist.Zero() + t = Twist() self.testTwistImpl(t) t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) self.testTwistImpl(t) + t = Twist(Vector(1, 2, 3), Vector(1, 2, 3)) + self.assertFalse(t == -t) # Doesn't work for zero twist + self.assertFalse(Equal(t, -t)) # Doesn't work for zero twist + self.assertTrue(t != -t) # Doesn't work for zero twist + self.assertTrue(not Equal(t, -t)) # Doesn't work for zero twist + + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + t = Twist(v1, v2) + self.assertEqual(t.vel, v1) + self.assertEqual(t.rot, v2) + # Test __getitem__ + for i in range(6): + self.assertEqual(t[i], i+1) + with self.assertRaises(IndexError): + _ = t[-1] + with self.assertRaises(IndexError): + _ = t[6] + # Test __setitem__ + for i in range(6): + t[i] = i + for i in range(6): + self.assertEqual(t[i], i) + with self.assertRaises(IndexError): + t[-1] = 1 + with self.assertRaises(IndexError): + t[6] = 1 + + SetToZero(t) + self.assertEqual(t, Twist()) + self.assertEqual(Twist.Zero(), Twist()) + def testTwistImpl(self, t): + self.assertTrue(t == t) + self.assertTrue(Equal(t, t)) + self.assertFalse(t != t) + self.assertFalse(not Equal(t, t)) self.assertEqual(2*t-t, t) self.assertEqual(t*2-t, t) self.assertEqual(t+t+t-2*t, t) @@ -111,8 +147,17 @@ def testTwistImpl(self, t): self.assertEqual(2*t, t2) t2 -= t self.assertEqual(t, t2) - t.ReverseSign() + t2.ReverseSign() self.assertEqual(t, -t2) + v = Vector(1, 2, 3) + t2 = t.RefPoint(v) + self.assertEqual(t2.vel, t.vel + t.rot*v) + self.assertEqual(t2.rot, t.rot) + + w = Wrench(v, v) + dot_result = dot(t.vel, w.force) + dot(t.rot, w.torque) + self.assertEqual(dot(t, w), dot_result) + self.assertEqual(dot(w, t), dot_result) def testWrench(self): w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) From 4272586264f7113bca534310b3ccf9513171b96c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 10:59:25 +0100 Subject: [PATCH 033/136] (PyKDL) extend Wrench tests --- python_orocos_kdl/tests/framestest.py | 42 +++++++++++++++++++++++++-- 1 file changed, 40 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 2264a4bcd..0b320f948 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -154,6 +154,7 @@ def testTwistImpl(self, t): self.assertEqual(t2.vel, t.vel + t.rot*v) self.assertEqual(t2.rot, t.rot) + # No need to test the dot functions again for Wrench w = Wrench(v, v) dot_result = dot(t.vel, w.force) + dot(t.rot, w.torque) self.assertEqual(dot(t, w), dot_result) @@ -162,11 +163,43 @@ def testTwistImpl(self, t): def testWrench(self): w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) self.testWrenchImpl(w) - w = Wrench.Zero() + w = Wrench() self.testWrenchImpl(w) w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1)) self.testWrenchImpl(w) + w = Wrench(Vector(1, 2, 3), Vector(1, 2, 3)) + self.assertFalse(w == -w) # Doesn't work for zero wrench + self.assertFalse(Equal(w, -w)) # Doesn't work for zero wrench + self.assertTrue(w != -w) # Doesn't work for zero wrench + self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench + + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + w = Wrench(v1, v2) + self.assertEqual(w.force, v1) + self.assertEqual(w.torque, v2) + # Test __getitem__ + for i in range(6): + self.assertEqual(w[i], i+1) + with self.assertRaises(IndexError): + _ = w[-1] + with self.assertRaises(IndexError): + _ = w[6] + # Test __setitem__ + for i in range(6): + w[i] = i + for i in range(6): + self.assertEqual(w[i], i) + with self.assertRaises(IndexError): + w[-1] = 1 + with self.assertRaises(IndexError): + w[6] = 1 + + SetToZero(w) + self.assertEqual(w, Wrench()) + self.assertEqual(Wrench.Zero(), Wrench()) + def testWrenchImpl(self, w): self.assertEqual(2*w-w, w) self.assertEqual(w*2-w, w) @@ -177,9 +210,14 @@ def testWrenchImpl(self, w): self.assertEqual(2*w, w2) w2 -= w self.assertEqual(w, w2) - w.ReverseSign() + w2.ReverseSign() self.assertEqual(w, -w2) + v = Vector(1, 2, 3) + w2 = w.RefPoint(v) + self.assertEqual(w2.force, w.force) + self.assertEqual(w2.torque, w.torque + w.force*v) + def testRotation(self): self.testRotationImpl(Vector(3, 4, 5), radians(10), radians(20), radians(30)) From 81574eebdc8f9cadf2a08b74b60a429f1c764999 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 11:49:56 +0100 Subject: [PATCH 034/136] (PyKDL)(sip) fix getitem/setitem code methodecode shouldn't return this causes the raising of error to fail --- python_orocos_kdl/PyKDL/sip/dynamics.sip | 6 ++- python_orocos_kdl/PyKDL/sip/frames.sip | 66 +++++++++++++++--------- python_orocos_kdl/PyKDL/sip/kinfam.sip | 36 ++++++++----- 3 files changed, 71 insertions(+), 37 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index 43bc6dc92..36d6a569b 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -40,9 +40,11 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Inertia index out of range"); - return NULL; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(i,j); } - sipRes=(*sipCpp)(i,j); %End //double operator()(unsigned int i,unsigned int j)const; //double& operator()(unsigned int i,unsigned int j); diff --git a/python_orocos_kdl/PyKDL/sip/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip index f437479e3..a97a7364f 100644 --- a/python_orocos_kdl/PyKDL/sip/frames.sip +++ b/python_orocos_kdl/PyKDL/sip/frames.sip @@ -46,18 +46,22 @@ public: %MethodCode if (a0 < 0 || a0 > 2) { PyErr_SetString(PyExc_IndexError, "Vector index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End void __setitem__(int index, double value); %MethodCode if (a0 < 0 || a0 > 2) { PyErr_SetString(PyExc_IndexError, "Vector index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End const std::string* __repr__() const; @@ -118,9 +122,11 @@ public: PyArg_ParseTuple(a0, "ii", &i, &j); if (i < 0 || j < 0 || i > 2 || j > 2) { PyErr_SetString(PyExc_IndexError, "Rotation index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=((const Rotation)(*sipCpp))(i,j); } - sipRes=((const Rotation)(*sipCpp))(i,j); %End void __setitem__(SIP_PYTUPLE,double value); @@ -129,9 +135,11 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 2 || j > 2) { PyErr_SetString(PyExc_IndexError, "Rotation index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(i,j)=a1; } - (*sipCpp)(i,j)=a1; %End const std::string* __repr__() const; @@ -219,9 +227,11 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 2 || j > 3) { PyErr_SetString(PyExc_IndexError, "Frame index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(i,j); } - sipRes=(*sipCpp)(i,j); %End void __setitem__(SIP_PYTUPLE,double value); @@ -230,12 +240,14 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 2 || j > 3) { PyErr_SetString(PyExc_IndexError, "Frame index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + if(j==3) + (*sipCpp).p(i)=a1; + else + (*sipCpp).M(i,j)=a1; } - if(j==3) - (*sipCpp).p(i)=a1; - else - (*sipCpp).M(i,j)=a1; %End const std::string* __repr__() const; @@ -296,18 +308,22 @@ public: %MethodCode if (a0 < 0 || a0 > 5) { PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End void __setitem__(int i, double value); %MethodCode if (a0 < 0 || a0 > 5) { PyErr_SetString(PyExc_IndexError, "Twist index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End const std::string* __repr__() const; @@ -369,18 +385,22 @@ public: %MethodCode if (a0 < 0 || a0 > 5) { PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End void __setitem__(int i, double value); %MethodCode if (a0 < 0 || a0 > 5) { PyErr_SetString(PyExc_IndexError, "Wrench index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End const std::string* __repr__() const; diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index cd080d50f..90f5eacf6 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -163,18 +163,22 @@ public: %MethodCode if (a0 < 0 || a0 >= 9) { PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp).data[a0]; } - sipRes=(*sipCpp).data[a0]; %End void __setitem__(int i, double value); %MethodCode if (a0 < 0 || a0 >= 9) { PyErr_SetString(PyExc_IndexError, "RotationalInertia index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp).data[a0]=a1; } - (*sipCpp).data[a0]=a1; %End }; @@ -292,18 +296,22 @@ public: %MethodCode if (a0 < 0 || a0 >= (int)sipCpp->rows()) { PyErr_SetString(PyExc_IndexError, "JntArray index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(a0); } - sipRes=(*sipCpp)(a0); %End void __setitem__(int index, double value); %MethodCode if (a0 < 0 || a0 >= (int)sipCpp->rows()) { PyErr_SetString(PyExc_IndexError, "JntArray index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(a0)=a1; } - (*sipCpp)(a0)=a1; %End const std::string* __repr__(); @@ -374,9 +382,11 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Jacobian index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + sipRes=(*sipCpp)(i,j); } - sipRes=(*sipCpp)(i,j); %End void __setitem__(SIP_PYTUPLE,double value); @@ -385,9 +395,11 @@ public: PyArg_ParseTuple(a0,"ii",&i,&j); if (i < 0 || j < 0 || i > 5 || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Jacobian index out of range"); - return 0; + sipError = sipErrorFail; + } + else { + (*sipCpp)(i,j)=a1; } - (*sipCpp)(i,j)=a1; %End const std::string* __repr__(); From d0d0fd4d451277da35af04271afb41a4ecfe9b1d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 12:39:18 +0100 Subject: [PATCH 035/136] (PyKDL) fix setWeightTS and setWeightJS; --- python_orocos_kdl/PyKDL/sip/kinfam.sip | 89 ++++++++++++++++---------- 1 file changed, 56 insertions(+), 33 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index 90f5eacf6..78a9b738e 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -567,28 +567,41 @@ public: temp1=PyList_GetItem(list,0); numCols=PyList_Size(temp1); if (numRows!=numCols) { - sipIsErr=1; //todo: raise exception + std::ostringstream oss; + oss << "Number of rows(" << numRows << ") and columns(" << numCols << ") don't match" << std::endl; + PyErr_SetString(PyExc_ValueError, oss.str().c_str()); + sipError = sipErrorFail; } - if (numRows!=6) { - sipIsErr=1; //todo: raise exception + if (sipError==sipErrorNone && numRows!=6) { + std::ostringstream oss; + oss << "Number of rows != 6, but " << numRows << std::endl; + PyErr_SetString(PyExc_ValueError, oss.str().c_str()); + sipError = sipErrorFail; } - Eigen::MatrixXd Mx; - Mx=Eigen::MatrixXd::Identity(numRows,numCols); - - for (Py_ssize_t r=0;rsetWeightTS(Mx); } - sipCpp->setWeightTS(Mx); %End void setWeightJS(SIP_PYLIST); @@ -604,24 +617,34 @@ public: temp1=PyList_GetItem(list,0); numCols=PyList_Size(temp1); if (numRows!=numCols) { - sipIsErr=1; //todo: raise exception + std::ostringstream oss; + oss << "Number of rows(" << numRows << ") and columns(" << numCols << ") don't match" << std::endl; + PyErr_SetString(PyExc_ValueError, oss.str().c_str()); + sipError = sipErrorFail; } - Eigen::MatrixXd Mq; - Mq=Eigen::MatrixXd::Identity(numRows,numCols); - for (Py_ssize_t r=0;rsetWeightJS(Mq); } - sipCpp->setWeightJS(Mq); %End void setLambda(const double& lambda); From 52349c6142156492fc2a139f5d9ba0749591fa3b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 19:58:54 +0100 Subject: [PATCH 036/136] (PyKDL) extend Rotation tests --- python_orocos_kdl/tests/framestest.py | 98 ++++++++++++++++++--------- 1 file changed, 65 insertions(+), 33 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 0b320f948..6e59a4db4 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -220,52 +220,84 @@ def testWrenchImpl(self, w): def testRotation(self): self.testRotationImpl(Vector(3, 4, 5), radians(10), radians(20), radians(30)) + self.testRotationImpl(Vector(), radians(10), radians(20), radians(30)) + self.testRotationImpl(Vector(3, 4, 5), radians(0), radians(0), radians(0)) + self.testRotationImpl(Vector(), radians(0), radians(0), radians(0)) + + r = Rotation(*range(1, 10)) + # Test __getitem__ + for i in range(3): + for j in range(3): + self.assertEqual(r[i, j], 3*i+j+1) + with self.assertRaises(IndexError): + _ = r[-1, 0] + with self.assertRaises(IndexError): + _ = r[0, -1] + with self.assertRaises(IndexError): + _ = r[3, 2] + with self.assertRaises(IndexError): + _ = r[2, 3] + # Test __setitem__ + for i in range(3): + for j in range(3): + r[i, j] = 3*i+j + for i in range(3): + for j in range(3): + self.assertEqual(r[i, j], 3*i+j) + with self.assertRaises(IndexError): + r[-1, 0] = 1 + with self.assertRaises(IndexError): + r[0, -1] = 1 + with self.assertRaises(IndexError): + r[3, 2] = 1 + with self.assertRaises(IndexError): + r[2, 3] = 1 def testRotationImpl(self, v, a, b, c): w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) - R = Rotation.RPY(a, b, c) - - self.assertAlmostEqual(dot(R.UnitX(), R.UnitX()), 1.0, 15) - self.assertEqual(dot(R.UnitY(), R.UnitY()), 1.0) - self.assertEqual(dot(R.UnitZ(), R.UnitZ()), 1.0) - self.assertAlmostEqual(dot(R.UnitX(), R.UnitY()), 0.0, 15) - self.assertAlmostEqual(dot(R.UnitX(), R.UnitZ()), 0.0, 15) - self.assertEqual(dot(R.UnitY(), R.UnitZ()), 0.0) - R2 = Rotation(R) - self.assertEqual(R, R2) - self.assertAlmostEqual((R*v).Norm(), v.Norm(), 14) - self.assertEqual(R.Inverse(R*v), v) - self.assertEqual(R.Inverse(R*t), t) - self.assertEqual(R.Inverse(R*w), w) - self.assertEqual(R*R.Inverse(v), v) - self.assertEqual(R*Rotation.Identity(), R) - self.assertEqual(Rotation.Identity()*R, R) - self.assertEqual(R*(R*(R*v)), (R*R*R)*v) - self.assertEqual(R*(R*(R*t)), (R*R*R)*t) - self.assertEqual(R*(R*(R*w)), (R*R*R)*w) - self.assertEqual(R*R.Inverse(), Rotation.Identity()) - self.assertEqual(R.Inverse()*R, Rotation.Identity()) - self.assertEqual(R.Inverse()*v, R.Inverse(v)) - (ra, rb, rc) = R.GetRPY() + r = Rotation.RPY(a, b, c) + + self.assertAlmostEqual(dot(r.UnitX(), r.UnitX()), 1.0, 15) + self.assertEqual(dot(r.UnitY(), r.UnitY()), 1.0) + self.assertEqual(dot(r.UnitZ(), r.UnitZ()), 1.0) + self.assertAlmostEqual(dot(r.UnitX(), r.UnitY()), 0.0, 15) + self.assertAlmostEqual(dot(r.UnitX(), r.UnitZ()), 0.0, 15) + self.assertEqual(dot(r.UnitY(), r.UnitZ()), 0.0) + r2 = Rotation(r) + self.assertEqual(r, r2) + self.assertAlmostEqual((r*v).Norm(), v.Norm(), 14) + self.assertEqual(r.Inverse(r*v), v) + self.assertEqual(r.Inverse(r*t), t) + self.assertEqual(r.Inverse(r*w), w) + self.assertEqual(r*r.Inverse(v), v) + self.assertEqual(r*Rotation.Identity(), r) + self.assertEqual(Rotation.Identity()*r, r) + self.assertEqual(r*(r*(r*v)), (r*r*r)*v) + self.assertEqual(r*(r*(r*t)), (r*r*r)*t) + self.assertEqual(r*(r*(r*w)), (r*r*r)*w) + self.assertEqual(r*r.Inverse(), Rotation.Identity()) + self.assertEqual(r.Inverse()*r, Rotation.Identity()) + self.assertEqual(r.Inverse()*v, r.Inverse(v)) + (ra, rb, rc) = r.GetRPY() self.assertEqual(ra, a) self.assertEqual(rb, b) self.assertEqual(rc, c) - R = Rotation.EulerZYX(a, b, c) - (ra, rb, rc) = R.GetEulerZYX() + r = Rotation.EulerZYX(a, b, c) + (ra, rb, rc) = r.GetEulerZYX() self.assertEqual(ra, a) self.assertEqual(rb, b) self.assertEqual(rc, c) - R = Rotation.EulerZYZ(a, b, c) - (ra, rb, rc) = R.GetEulerZYZ() + r = Rotation.EulerZYZ(a, b, c) + (ra, rb, rc) = r.GetEulerZYZ() self.assertEqual(ra, a) self.assertEqual(rb, b) self.assertAlmostEqual(rc, c, 15) - (angle, v2) = R.GetRotAngle() - R2 = Rotation.Rot(v2, angle) - self.assertEqual(R2, R) - R2 = Rotation.Rot(v2*1E20, angle) - self.assertEqual(R, R2) + (angle, v2) = r.GetRotAngle() + r2 = Rotation.Rot(v2, angle) + self.assertEqual(r2, r) + r2 = Rotation.Rot(v2*1E20, angle) + self.assertEqual(r, r2) v2 = Vector(6, 2, 4) self.assertAlmostEqual(v2.Norm(), sqrt(dot(v2, v2)), 14) From 449f9bb47b04aa78e47aeb7d6a3fc114ff7ccaab Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 22:09:46 +0100 Subject: [PATCH 037/136] (PyKDL) extend Frame tests --- python_orocos_kdl/tests/framestest.py | 77 +++++++++++++++++++++------ 1 file changed, 60 insertions(+), 17 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 6e59a4db4..a2a448537 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -305,23 +305,66 @@ def testFrame(self): v = Vector(3, 4, 5) w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) - F = Frame(Rotation.EulerZYX(radians(10), radians(20), radians(-10)), Vector(4, -2, 1)) - F2 = Frame(F) - self.assertEqual(F, F2) - self.assertEqual(F.Inverse(F*v), v) - self.assertEqual(F.Inverse(F*t), t) - self.assertEqual(F.Inverse(F*w), w) - self.assertEqual(F*F.Inverse(v), v) - self.assertEqual(F*F.Inverse(t), t) - self.assertEqual(F*F.Inverse(w), w) - self.assertEqual(F*Frame.Identity(), F) - self.assertEqual(Frame.Identity()*F, F) - self.assertEqual(F*(F*(F*v)), (F*F*F)*v) - self.assertEqual(F*(F*(F*t)), (F*F*F)*t) - self.assertEqual(F*(F*(F*w)), (F*F*F)*w) - self.assertEqual(F*F.Inverse(), Frame.Identity()) - self.assertEqual(F.Inverse()*F, Frame.Identity()) - self.assertEqual(F.Inverse()*v, F.Inverse(v)) + f = Frame() + self.testFrameImpl(v, w, t, f) + r = Rotation.EulerZYX(radians(10), radians(20), radians(-10)) + v2 = Vector(4, -2, 1) + f = Frame(r, v2) + self.testFrameImpl(v, w, t, f) + + f2 = Frame(f) + self.assertEqual(f, f2) + + self.assertEqual(f.M, r) + self.assertEqual(f.p, v2) + + f = Frame(Rotation(1, 2, 3, + 5, 6, 7, + 9, 10, 11), + Vector(4, 8, 12)) + # Test __getitem__ + for i in range(3): + for j in range(4): + self.assertEqual(f[i, j], 4*i+j+1) + with self.assertRaises(IndexError): + _ = f[-1, 0] + with self.assertRaises(IndexError): + _ = f[0, -1] + with self.assertRaises(IndexError): + _ = f[3, 3] + with self.assertRaises(IndexError): + _ = f[2, 4] + # Test __setitem__ + for i in range(3): + for j in range(4): + f[i, j] = 4*i+j + for i in range(3): + for j in range(4): + self.assertEqual(f[i, j], 4*i+j) + with self.assertRaises(IndexError): + f[-1, 0] = 1 + with self.assertRaises(IndexError): + f[0, -1] = 1 + with self.assertRaises(IndexError): + f[3, 3] = 1 + with self.assertRaises(IndexError): + f[2, 4] = 1 + + def testFrameImpl(self, v, w, t, f): + self.assertEqual(f.Inverse(f*v), v) + self.assertEqual(f.Inverse(f*t), t) + self.assertEqual(f.Inverse(f*w), w) + self.assertEqual(f*f.Inverse(v), v) + self.assertEqual(f*f.Inverse(t), t) + self.assertEqual(f*f.Inverse(w), w) + self.assertEqual(f*Frame.Identity(), f) + self.assertEqual(Frame.Identity()*f, f) + self.assertEqual(f*(f*(f*v)), (f*f*f)*v) + self.assertEqual(f*(f*(f*t)), (f*f*f)*t) + self.assertEqual(f*(f*(f*w)), (f*f*f)*w) + self.assertEqual(f*f.Inverse(), Frame.Identity()) + self.assertEqual(f.Inverse()*f, Frame.Identity()) + self.assertEqual(f.Inverse()*v, f.Inverse(v)) def testPickle(self): import cPickle as pickle From bb2b70070b3f4aa4ddaaaf3ca14d722a8bfe2c94 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 22:11:17 +0100 Subject: [PATCH 038/136] (PyKDL) Add test for member of unassgined instances This was a bug in sip --- python_orocos_kdl/tests/framestest.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index a2a448537..d85aa80f1 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -111,7 +111,9 @@ def testTwist(self): v2 = Vector(4, 5, 6) t = Twist(v1, v2) self.assertEqual(t.vel, v1) + self.assertEqual(Twist(t).vel, v1) self.assertEqual(t.rot, v2) + self.assertEqual(Twist(t).rot, v2) # Test __getitem__ for i in range(6): self.assertEqual(t[i], i+1) @@ -178,7 +180,9 @@ def testWrench(self): v2 = Vector(4, 5, 6) w = Wrench(v1, v2) self.assertEqual(w.force, v1) + self.assertEqual(Wrench(w).force, v1) self.assertEqual(w.torque, v2) + self.assertEqual(Wrench(w).torque, v2) # Test __getitem__ for i in range(6): self.assertEqual(w[i], i+1) @@ -316,7 +320,9 @@ def testFrame(self): self.assertEqual(f, f2) self.assertEqual(f.M, r) + self.assertEqual(Frame(f).M, r) self.assertEqual(f.p, v2) + self.assertEqual(Frame(f).p, v2) f = Frame(Rotation(1, 2, 3, 5, 6, 7, From 65b22c188f991a844e1b24de24d7797ad514826e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 15 Mar 2020 23:22:26 +0100 Subject: [PATCH 039/136] (travis) add allow_failures --- .travis.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.travis.yml b/.travis.yml index 2558356da..b4db1b143 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,12 +5,19 @@ dist: xenial compiler: - gcc - clang + env: - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=OFF - OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=ON - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=ON +jobs: + fast_finish: true + allow_failures: + - env: OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=OFF + - env: OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF + before_script: - sudo apt-get -qq update - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python-psutil From 525cb28a2ebff469c003707c0b319c763f23af4b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 09:47:40 +0100 Subject: [PATCH 040/136] (PyKDL) docstring to comment --- python_orocos_kdl/tests/kinfamtest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 51f4dddfa..60c9611b8 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -154,7 +154,7 @@ def setUp(self): Frame(Vector(0.0, 0.0, 0.0))), "bar") def testTreeGetChainMemLeak(self): - """ test for the memory leak in Tree.getChain described in issue #211 """ + # test for the memory leak in Tree.getChain described in issue #211 process = psutil.Process() self.tree.getChain("foo", "bar") gc.collect() From 32b4e9a43ea2ba4e033e05669ae9be604c0d1373 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 09:55:40 +0100 Subject: [PATCH 041/136] (PyKDL) use context manager in pickle test --- python_orocos_kdl/tests/framestest.py | 10 ++++------ python_orocos_kdl/tests/frameveltest.py | 10 ++++------ 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index d85aa80f1..c731aec76 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -381,13 +381,11 @@ def testPickle(self): data['tw'] = Twist(data['v'], Vector(4, 5, 6)) data['wr'] = Wrench(Vector(0.1, 0.2, 0.3), data['v']) - f = open('/tmp/pickle_test', 'w') - pickle.dump(data, f, 2) - f.close() + with open('/tmp/pickle_test', 'w') as f: + pickle.dump(data, f, 2) - f = open('/tmp/pickle_test', 'r') - data1 = pickle.load(f) - f.close() + with open('/tmp/pickle_test', 'r') as f: + data1 = pickle.load(f) self.assertEqual(data, data1) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 73d651b62..5dc7f4a81 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -92,13 +92,11 @@ def testPickle(self): data['fv'] = FrameVel(data['rv'], data['vv']) data['tv'] = TwistVel(data['vv'], data['vv']) - f = open('/tmp/pickle_test_kdl_framevel', 'w') - pickle.dump(data, f, 2) - f.close() + with open('/tmp/pickle_test_kdl_framevel', 'w') as f: + pickle.dump(data, f, 2) - f = open('/tmp/pickle_test_kdl_framevel', 'r') - data1 = pickle.load(f) - f.close() + with open('/tmp/pickle_test_kdl_framevel', 'r') as f: + data1 = pickle.load(f) self.assertEqual(data['vv'].p, data1['vv'].p) self.assertEqual(data['vv'].v, data1['vv'].v) From aa72abcc6d5bdc33f0ea85503870e839f051946a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 10:25:56 +0100 Subject: [PATCH 042/136] (PyKDL) add value and grad to doubleVel --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 2 ++ python_orocos_kdl/PyKDL/sip/framevel.sip | 2 ++ 2 files changed, 4 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index 5109e0c32..520b5d902 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -38,6 +38,8 @@ void init_framevel(pybind11::module &m) double_vel.def(py::init<>()); double_vel.def_readwrite("t", &doubleVel::t); double_vel.def_readwrite("grad", &doubleVel::grad); + double_vel.def("value", &doubleVel::value); + double_vel.def("deriv", &doubleVel::deriv); m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, py::arg("a"), py::arg("b"), py::arg("dt")=1.0); diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 953b9b14f..96de3bcef 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -30,6 +30,8 @@ public: typedef Rall1d doubleVel; double t; double grad; + double value(); + double deriv(); }; doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0); From cd383caf743e3e9c19aa04699730fa68f735fc5b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 10:26:17 +0100 Subject: [PATCH 043/136] (PyKDL) remove typedef of doubleVel --- python_orocos_kdl/PyKDL/sip/framevel.sip | 1 - 1 file changed, 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 96de3bcef..5ed6340e1 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -27,7 +27,6 @@ class doubleVel using namespace KDL; %End public: - typedef Rall1d doubleVel; double t; double grad; double value(); From 1f0cb350be73f06bf7c88fe60a22a6d575d660d3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 10:30:13 +0100 Subject: [PATCH 044/136] (PyKDL) add doubleVel test --- python_orocos_kdl/tests/frameveltest.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 5dc7f4a81..bea5c4d64 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -26,6 +26,11 @@ class FrameVelTestFunctions(unittest.TestCase): + def testdoubleVel(self): + d = doubleVel() + self.assertEqual(d.t, d.value()) + self.assertEqual(d.grad, d.deriv()) + def testVectorVel(self): v = VectorVel(Vector(3, -4, 5), Vector(6, 3, -5)) vt = Vector(-4, -6, -8) @@ -114,6 +119,7 @@ def testPickle(self): def suite(): suite = unittest.TestSuite() + suite.addTest(FrameVelTestFunctions('testdoubleVel')) suite.addTest(FrameVelTestFunctions('testVectorVel')) suite.addTest(FrameVelTestFunctions('testRotationVel')) suite.addTest(FrameVelTestFunctions('testFrameVel')) From 5166249b17deb9f2d68d06fcb27191b52bd1a55d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 10:35:10 +0100 Subject: [PATCH 045/136] (kdl) remove inlcude of yourself --- orocos_kdl/src/framevel_io.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/orocos_kdl/src/framevel_io.hpp b/orocos_kdl/src/framevel_io.hpp index 2124c13c4..a3dcc0743 100644 --- a/orocos_kdl/src/framevel_io.hpp +++ b/orocos_kdl/src/framevel_io.hpp @@ -21,7 +21,6 @@ #include "utilities/utility_io.h" #include "utilities/rall1d_io.h" -#include "framevel_io.hpp" #include "frames_io.hpp" namespace KDL { From cb346868103f9dde2de1fcb12fd95fc4de19eb46 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 11:28:00 +0100 Subject: [PATCH 046/136] (PyKDL) framevel pickle tests use Equal func --- python_orocos_kdl/tests/frameveltest.py | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index bea5c4d64..8ba4d1918 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -103,18 +103,10 @@ def testPickle(self): with open('/tmp/pickle_test_kdl_framevel', 'r') as f: data1 = pickle.load(f) - self.assertEqual(data['vv'].p, data1['vv'].p) - self.assertEqual(data['vv'].v, data1['vv'].v) - self.assertEqual(data['rv'].R, data1['rv'].R) - self.assertEqual(data['rv'].w, data1['rv'].w) - self.assertEqual(data['fv'].M.R, data1['fv'].M.R) - self.assertEqual(data['fv'].M.w, data1['fv'].M.w) - self.assertEqual(data['fv'].p.p, data1['fv'].p.p) - self.assertEqual(data['fv'].p.v, data1['fv'].p.v) - self.assertEqual(data['tv'].vel.p, data1['tv'].vel.p) - self.assertEqual(data['tv'].vel.v, data1['tv'].vel.v) - self.assertEqual(data['tv'].rot.p, data1['tv'].rot.p) - self.assertEqual(data['tv'].rot.v, data1['tv'].rot.v) + self.assertTrue(Equal(data['vv'], data1['vv'])) + self.assertTrue(Equal(data['rv'], data1['rv'])) + self.assertTrue(Equal(data['fv'], data1['fv'])) + self.assertTrue(Equal(data['tv'], data1['tv'])) def suite(): From aebdab9ca3c35bff238aeca3ce5534c2460981d9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 11:32:11 +0100 Subject: [PATCH 047/136] (kdl) include types in Rall1d oss --- orocos_kdl/src/utilities/rall1d_io.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/src/utilities/rall1d_io.h b/orocos_kdl/src/utilities/rall1d_io.h index d69b26177..584595051 100644 --- a/orocos_kdl/src/utilities/rall1d_io.h +++ b/orocos_kdl/src/utilities/rall1d_io.h @@ -18,6 +18,7 @@ #ifndef Rall_IO_H #define Rall_IO_H +#include #include "utility_io.h" #include "rall1d.h" @@ -26,7 +27,7 @@ namespace KDL { template inline std::ostream& operator << (std::ostream& os,const Rall1d& r) { - os << "Rall1d(" << r.t <<"," << r.grad <<")"; + os << "Rall1d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ", " <<"(" << r.t <<"," << r.grad <<")"; return os; } From a1470db790339a32f4d0b71d8a8326b8c306a984 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 11:32:58 +0100 Subject: [PATCH 048/136] (PyKDL)(sip) add __repr__ for framevel classes --- python_orocos_kdl/PyKDL/sip/framevel.sip | 45 ++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 5ed6340e1..7b4353890 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -24,6 +24,7 @@ class doubleVel %TypeHeaderCode #include +#include using namespace KDL; %End public: @@ -31,6 +32,13 @@ public: double grad; double value(); double deriv(); + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End }; doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0); @@ -43,6 +51,7 @@ class VectorVel { %TypeHeaderCode #include +#include using namespace KDL; %End public: @@ -55,6 +64,14 @@ public: Vector value() const; Vector deriv() const; + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + VectorVel& operator += (const VectorVel& arg); VectorVel& operator -= (const VectorVel& arg); static VectorVel Zero(); @@ -100,6 +117,7 @@ class RotationVel { %TypeHeaderCode #include +#include using namespace KDL; %End @@ -113,6 +131,14 @@ public: Rotation value() const; Vector deriv() const; + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + VectorVel UnitX() const; VectorVel UnitY() const; @@ -159,6 +185,7 @@ class FrameVel %TypeHeaderCode #include +#include using namespace KDL; %End @@ -174,6 +201,14 @@ public: Frame value() const; Twist deriv() const; + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + static FrameVel Identity(); FrameVel Inverse() const; @@ -207,6 +242,7 @@ class TwistVel { %TypeHeaderCode #include +#include using namespace KDL; %End @@ -221,6 +257,15 @@ public: Twist value() const; Twist deriv() const; + + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + TwistVel& operator-=(const TwistVel& arg); TwistVel& operator+=(const TwistVel& arg); From 9d9de4e85572187bc9249f98917011c64699b54a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 12:05:51 +0100 Subject: [PATCH 049/136] (PyKDL) add __repr__ for framevel classes --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index 520b5d902..d463d16d9 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -40,6 +40,12 @@ void init_framevel(pybind11::module &m) double_vel.def_readwrite("grad", &doubleVel::grad); double_vel.def("value", &doubleVel::value); double_vel.def("deriv", &doubleVel::deriv); + double_vel.def("__repr__", [](const doubleVel &d) + { + std::ostringstream oss; + oss << d; + return oss.str(); + }); m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, py::arg("a"), py::arg("b"), py::arg("dt")=1.0); @@ -61,6 +67,12 @@ void init_framevel(pybind11::module &m) vector_vel.def(py::init()); vector_vel.def("value", &VectorVel::value); vector_vel.def("deriv", &VectorVel::deriv); + vector_vel.def("__repr__", [](const VectorVel &vv) + { + std::ostringstream oss; + oss << vv; + return oss.str(); + }); vector_vel.def_static("Zero", &VectorVel::Zero); vector_vel.def("ReverseSign", &VectorVel::ReverseSign); vector_vel.def("Norm", &VectorVel::Norm); @@ -128,6 +140,12 @@ void init_framevel(pybind11::module &m) twist_vel.def(py::init()); twist_vel.def("value", &TwistVel::value); twist_vel.def("deriv", &TwistVel::deriv); + twist_vel.def("__repr__", [](const TwistVel &tv) + { + std::ostringstream oss; + oss << tv; + return oss.str(); + }); twist_vel.def_static("Zero", &TwistVel::Zero); twist_vel.def("ReverseSign", &TwistVel::ReverseSign); twist_vel.def("RefPoint", &TwistVel::RefPoint); @@ -187,6 +205,12 @@ void init_framevel(pybind11::module &m) rotation_vel.def(py::init()); rotation_vel.def("value", &RotationVel::value); rotation_vel.def("deriv", &RotationVel::deriv); + rotation_vel.def("__repr__", [](const RotationVel &rv) + { + std::ostringstream oss; + oss << rv; + return oss.str(); + }); rotation_vel.def("UnitX", &RotationVel::UnitX); rotation_vel.def("UnitY", &RotationVel::UnitY); rotation_vel.def("UnitZ", &RotationVel::UnitZ); @@ -250,6 +274,12 @@ void init_framevel(pybind11::module &m) frame_vel.def(py::init()); frame_vel.def("value", &FrameVel::value); frame_vel.def("deriv", &FrameVel::deriv); + frame_vel.def("__repr__", [](const FrameVel &fv) + { + std::ostringstream oss; + oss << fv; + return oss.str(); + }); frame_vel.def_static("Identity", &FrameVel::Identity); frame_vel.def("Inverse", (FrameVel (FrameVel::*)() const) &FrameVel::Inverse); frame_vel.def("Inverse", (VectorVel (FrameVel::*)(const VectorVel&) const) &FrameVel::Inverse); From a03ee0d6836ef200d7e27e5adc50cbb0b10aeda0 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 12:08:53 +0100 Subject: [PATCH 050/136] (PyKDL) add copy constructors for framevel classes --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index d463d16d9..60703a90a 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -36,6 +36,7 @@ void init_framevel(pybind11::module &m) // -------------------- py::class_ double_vel(m, "doubleVel"); double_vel.def(py::init<>()); + double_vel.def(py::init()); double_vel.def_readwrite("t", &doubleVel::t); double_vel.def_readwrite("grad", &doubleVel::grad); double_vel.def("value", &doubleVel::value); @@ -138,6 +139,7 @@ void init_framevel(pybind11::module &m) twist_vel.def(py::init()); twist_vel.def(py::init()); twist_vel.def(py::init()); + twist_vel.def(py::init()); twist_vel.def("value", &TwistVel::value); twist_vel.def("deriv", &TwistVel::deriv); twist_vel.def("__repr__", [](const TwistVel &tv) From 410ae3519996f03f73cbda6d8b16e91d2c618c72 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 12:12:41 +0100 Subject: [PATCH 051/136] (PyKDL) pickle test python3 compatible --- python_orocos_kdl/tests/framestest.py | 6 +++++- python_orocos_kdl/tests/frameveltest.py | 9 ++++++--- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index c731aec76..d564e9cdc 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -373,7 +373,11 @@ def testFrameImpl(self, v, w, t, f): self.assertEqual(f.Inverse()*v, f.Inverse(v)) def testPickle(self): - import cPickle as pickle + import sys + if sys.version_info < (3, 0): + import cPickle as pickle + else: + import pickle data = dict() data['v'] = Vector(1, 2, 3) data['rot'] = Rotation().EulerZYZ(1, 2, 3) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 8ba4d1918..4fab03b4b 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -89,11 +89,14 @@ def testFrameVel(self): self.assertTrue(Equal(F.Inverse()*vt, F.Inverse(vt))) def testPickle(self): - rot = Rotation.RotX(1.3) - import cPickle as pickle + import sys + if sys.version_info < (3, 0): + import cPickle as pickle + else: + import pickle data = {} data['vv'] = VectorVel(Vector(1, 2, 3), Vector(4, 5, 6)) - data['rv'] = RotationVel(rot, Vector(4.1, 5.1, 6.1)) + data['rv'] = RotationVel(Rotation.RotX(1.3), Vector(4.1, 5.1, 6.1)) data['fv'] = FrameVel(data['rv'], data['vv']) data['tv'] = TwistVel(data['vv'], data['vv']) From 20bb4566fe0fba66d57c40698578f04d29a5db72 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 12:23:01 +0100 Subject: [PATCH 052/136] (PyKDL) fix import and test results --- python_orocos_kdl/tests/framestest.py | 13 +++++++++---- python_orocos_kdl/tests/frameveltest.py | 13 +++++++++---- python_orocos_kdl/tests/kinfamtest.py | 8 +++++++- 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index d564e9cdc..ab816f03f 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -20,9 +20,10 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -import unittest +from math import radians, sqrt from PyKDL import * -from math import * +import sys +import unittest class FramesTestFunctions(unittest.TestCase): @@ -373,7 +374,6 @@ def testFrameImpl(self, v, w, t, f): self.assertEqual(f.Inverse()*v, f.Inverse(v)) def testPickle(self): - import sys if sys.version_info < (3, 0): import cPickle as pickle else: @@ -407,4 +407,9 @@ def suite(): if __name__ == '__main__': suite = suite() - unittest.TextTestRunner(verbosity=3).run(suite) + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 4fab03b4b..2615ceb74 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -20,9 +20,10 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -import unittest +from math import radians from PyKDL import * -from math import * +import sys +import unittest class FrameVelTestFunctions(unittest.TestCase): @@ -89,7 +90,6 @@ def testFrameVel(self): self.assertTrue(Equal(F.Inverse()*vt, F.Inverse(vt))) def testPickle(self): - import sys if sys.version_info < (3, 0): import cPickle as pickle else: @@ -124,4 +124,9 @@ def suite(): if __name__ == '__main__': suite = suite() - unittest.TextTestRunner(verbosity=3).run(suite) + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 60c9611b8..5d1664bac 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -24,6 +24,7 @@ import psutil from PyKDL import * import random +import sys import unittest @@ -180,4 +181,9 @@ def suite(): if __name__ == '__main__': suite = suite() - unittest.TextTestRunner(verbosity=3).run(suite) + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) From 5b023545d96e41655b03ddc3b27896cb8ad3d04e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 12:38:05 +0100 Subject: [PATCH 053/136] (kdl) rename config to cmake --- orocos_kdl/CMakeLists.txt | 6 +++--- orocos_kdl/{config => cmake}/CheckSTLContainers.cmake | 0 orocos_kdl/{config => cmake}/DependentOption.cmake | 0 orocos_kdl/{config => cmake}/FindEigen3.cmake | 0 orocos_kdl/{config => cmake}/FindPkgConfig.cmake | 0 5 files changed, 3 insertions(+), 3 deletions(-) rename orocos_kdl/{config => cmake}/CheckSTLContainers.cmake (100%) rename orocos_kdl/{config => cmake}/DependentOption.cmake (100%) rename orocos_kdl/{config => cmake}/FindEigen3.cmake (100%) rename orocos_kdl/{config => cmake}/FindPkgConfig.cmake (100%) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index 7bb236658..e535c3f43 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -47,14 +47,14 @@ SET( KDL_CFLAGS "") find_package(Eigen 3 QUIET) if(NOT Eigen_FOUND) - include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake) + include(${PROJ_SOURCE_DIR}/cmake/FindEigen3.cmake) set(Eigen_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}") endif() include_directories(${Eigen_INCLUDE_DIR}) SET(KDL_CFLAGS "${KDL_CFLAGS} -I${Eigen_INCLUDE_DIR}") # Check the platform STL containers capabilities -include(config/CheckSTLContainers.cmake) +include(cmake/CheckSTLContainers.cmake) CHECK_STL_CONTAINERS() # Set the default option appropriately @@ -75,7 +75,7 @@ if(KDL_USE_NEW_TREE_INTERFACE) set(KDL_INCLUDE_DIRS ${Boost_INCLUDE_DIRS}) endif(KDL_USE_NEW_TREE_INTERFACE) -INCLUDE (${PROJ_SOURCE_DIR}/config/DependentOption.cmake) +INCLUDE (${PROJ_SOURCE_DIR}/cmake/DependentOption.cmake) OPTION(ENABLE_TESTS OFF "Enable building of tests") IF( ENABLE_TESTS ) diff --git a/orocos_kdl/config/CheckSTLContainers.cmake b/orocos_kdl/cmake/CheckSTLContainers.cmake similarity index 100% rename from orocos_kdl/config/CheckSTLContainers.cmake rename to orocos_kdl/cmake/CheckSTLContainers.cmake diff --git a/orocos_kdl/config/DependentOption.cmake b/orocos_kdl/cmake/DependentOption.cmake similarity index 100% rename from orocos_kdl/config/DependentOption.cmake rename to orocos_kdl/cmake/DependentOption.cmake diff --git a/orocos_kdl/config/FindEigen3.cmake b/orocos_kdl/cmake/FindEigen3.cmake similarity index 100% rename from orocos_kdl/config/FindEigen3.cmake rename to orocos_kdl/cmake/FindEigen3.cmake diff --git a/orocos_kdl/config/FindPkgConfig.cmake b/orocos_kdl/cmake/FindPkgConfig.cmake similarity index 100% rename from orocos_kdl/config/FindPkgConfig.cmake rename to orocos_kdl/cmake/FindPkgConfig.cmake From aa54b5b528d34442dacdb7ff6380c5b1fb46c7c2 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 12:44:14 +0100 Subject: [PATCH 054/136] (kdl) cleanup of repetetive cmake code --- orocos_kdl/CMakeLists.txt | 6 +++++ orocos_kdl/src/CMakeLists.txt | 46 ----------------------------------- 2 files changed, 6 insertions(+), 46 deletions(-) diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index e535c3f43..af5acba1c 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -67,6 +67,12 @@ endif(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) # Allow the user to select the Tree API version to use set(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") +#Sanity check, inform the user +if(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) + message(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " + "incomplete types, this configuration is likely invalid") +endif() + # The new interface requires the use of shared pointers if(KDL_USE_NEW_TREE_INTERFACE) # We need shared_ptr from boost since not all compilers are c++11 capable diff --git a/orocos_kdl/src/CMakeLists.txt b/orocos_kdl/src/CMakeLists.txt index 079ca8ac1..7557069bb 100644 --- a/orocos_kdl/src/CMakeLists.txt +++ b/orocos_kdl/src/CMakeLists.txt @@ -3,52 +3,6 @@ FILE( GLOB KDL_HPPS [^.]*.hpp [^.]*.inl) FILE( GLOB UTIL_HPPS utilities/[^.]*.h utilities/[^.]*.hpp) -INCLUDE(CheckCXXSourceCompiles) -SET(CMAKE_REQUIRED_FLAGS) -CHECK_CXX_SOURCE_COMPILES(" - #include - #include - #include - - class TreeElement; - typedef std::map SegmentMap; - - class TreeElement - { - TreeElement(const std::string& name): number(0) {} - - public: - int number; - SegmentMap::const_iterator parent; - std::vector children; - - static TreeElement Root(std::string& name) - { - return TreeElement(name); - } - }; - - int main() - { - return 0; - } - " - HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - -if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off) -ELSE(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On) -ENDIF(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) - -SET(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") - -#Sanity check, inform the user -IF(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) - MESSAGE(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " - "incomplete types, this configuration is likely invalid") -ENDIF() - #In Windows (Visual Studio) it is necessary to specify the postfix #of the debug library name and no symbols are exported by kdl, #so it is necessary to compile it as a static library From 4dacfa6a1845b55fc429fd23431e6074dc4930c1 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 17:43:22 +0100 Subject: [PATCH 055/136] (kdl) remove unused dependentoption --- orocos_kdl/CMakeLists.txt | 2 -- orocos_kdl/cmake/DependentOption.cmake | 37 -------------------------- 2 files changed, 39 deletions(-) delete mode 100644 orocos_kdl/cmake/DependentOption.cmake diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index af5acba1c..2bbc61436 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -81,8 +81,6 @@ if(KDL_USE_NEW_TREE_INTERFACE) set(KDL_INCLUDE_DIRS ${Boost_INCLUDE_DIRS}) endif(KDL_USE_NEW_TREE_INTERFACE) -INCLUDE (${PROJ_SOURCE_DIR}/cmake/DependentOption.cmake) - OPTION(ENABLE_TESTS OFF "Enable building of tests") IF( ENABLE_TESTS ) # If not in standard paths, set CMAKE_xxx_PATH's in environment, eg. diff --git a/orocos_kdl/cmake/DependentOption.cmake b/orocos_kdl/cmake/DependentOption.cmake deleted file mode 100644 index 4ec4a6e1b..000000000 --- a/orocos_kdl/cmake/DependentOption.cmake +++ /dev/null @@ -1,37 +0,0 @@ -# - Macro to provide an option dependent on other options. -# This macro presents an option to the user only if a set of other -# conditions are true. When the option is not presented a default -# value is used, but any value set by the user is preserved for when -# the option is presented again. -# Example invocation: -# DEPENDENT_OPTION(USE_FOO "Use Foo" ON -# "USE_BAR;NOT USE_ZOT" OFF) -# If USE_BAR is true and USE_ZOT is false, this provides an option called -# USE_FOO that defaults to ON. Otherwise, it sets USE_FOO to OFF. If -# the status of USE_BAR or USE_ZOT ever changes, any value for the -# USE_FOO option is saved so that when the option is re-enabled it -# retains its old value. -MACRO(DEPENDENT_OPTION option doc default depends force) - IF(${option}_ISSET MATCHES "^${option}_ISSET$") - SET(${option}_AVAILABLE 1) - FOREACH(d ${depends}) - STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}") - IF(${CMAKE_DEPENDENT_OPTION_DEP}) - ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) - SET(${option}_AVAILABLE 0) - ENDIF(${CMAKE_DEPENDENT_OPTION_DEP}) - ENDFOREACH(d) - IF(${option}_AVAILABLE) - OPTION(${option} "${doc}" "${default}") - SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE) - ELSE(${option}_AVAILABLE) - IF(${option} MATCHES "^${option}$") - ELSE(${option} MATCHES "^${option}$") - SET(${option} "${${option}}" CACHE INTERNAL "${doc}") - ENDIF(${option} MATCHES "^${option}$") - SET(${option} ${force}) - ENDIF(${option}_AVAILABLE) - ELSE(${option}_ISSET MATCHES "^${option}_ISSET$") - SET(${option} "${${option}_ISSET}") - ENDIF(${option}_ISSET MATCHES "^${option}_ISSET$") -ENDMACRO(DEPENDENT_OPTION) From 52a57b9340557ec8507a58bc6eeed122d2a81a3c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 18:00:04 +0100 Subject: [PATCH 056/136] (kdl) remove outdated test targets --- orocos_kdl/tests/CMakeLists.txt | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/orocos_kdl/tests/CMakeLists.txt b/orocos_kdl/tests/CMakeLists.txt index 6f91b036f..80cd20ba1 100644 --- a/orocos_kdl/tests/CMakeLists.txt +++ b/orocos_kdl/tests/CMakeLists.txt @@ -66,28 +66,6 @@ IF(ENABLE_TESTS) # ADD_EXECUTABLE(rallnumbertest rallnumbertest.cpp) # TARGET_LINK_LIBRARIES(rallnumbertest orocos-kdl) # ADD_TEST(rallnumbertest rallnumbertest) -# -# -# IF(OROCOS_PLUGIN) -# ADD_EXECUTABLE(toolkittest toolkittest.cpp) -# LINK_DIRECTORIES(${OROCOS_RTT_LINK_DIRS}) -# TARGET_LINK_LIBRARIES(toolkittest orocos-kdltk orocos-kdl ${OROCOS_RTT_LIBS}) -# ADD_TEST(toolkittest toolkittest) -# ENDIF(OROCOS_PLUGIN) -# -# IF(PYTHON_BINDINGS) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/framestest.py -# ${CMAKE_CURRENT_BINARY_DIR}/framestest.py COPY_ONLY) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/kinfamtest.py -# ${CMAKE_CURRENT_BINARY_DIR}/kinfamtest.py COPY_ONLY) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/frameveltest.py -# ${CMAKE_CURRENT_BINARY_DIR}/frameveltest.py COPY_ONLY) -# CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/PyKDLtest.py -# ${CMAKE_CURRENT_BINARY_DIR}/PyKDLtest.py COPY_ONLY) -# ADD_TEST(PyKDLtest PyKDLtest.py) -# -# -# ENDIF(PYTHON_BINDINGS) ADD_CUSTOM_TARGET(check ctest -V WORKING_DIRECTORY ${PROJ_BINARY_DIR}/tests) ENDIF(ENABLE_TESTS) From 33936db2609c2fecb1809625fd2bf3dd6297f967 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 20:39:22 +0100 Subject: [PATCH 057/136] (kdl) cleanup commented includes --- orocos_kdl/src/frames.hpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp index 1069afcd5..73f3db19c 100644 --- a/orocos_kdl/src/frames.hpp +++ b/orocos_kdl/src/frames.hpp @@ -1252,19 +1252,9 @@ IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); #ifdef KDL_INLINE -// #include "vector.inl" -// #include "wrench.inl" - //#include "rotation.inl" - //#include "frame.inl" - //#include "twist.inl" - //#include "vector2.inl" - //#include "rotation2.inl" - //#include "frame2.inl" #include "frames.inl" #endif - - } From a3afe45fe9cab4ed76a3c1491012837826470d32 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 22:28:46 +0100 Subject: [PATCH 058/136] (kdl) add more equality operators --- orocos_kdl/src/framevel.hpp | 29 ++++++ orocos_kdl/src/framevel.inl | 149 +++++++++++++++++++++++++++--- orocos_kdl/src/utilities/rall1d.h | 16 ++++ orocos_kdl/src/utilities/rall2d.h | 18 ++++ 4 files changed, 200 insertions(+), 12 deletions(-) diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp index 243898a60..ba119aa0b 100644 --- a/orocos_kdl/src/framevel.hpp +++ b/orocos_kdl/src/framevel.hpp @@ -128,6 +128,14 @@ class VectorVel IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps); IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps); + + IMETHOD friend bool operator==(const VectorVel& r1,const VectorVel& r2); + IMETHOD friend bool operator!=(const VectorVel& r1,const VectorVel& r2); + IMETHOD friend bool operator==(const Vector& r1,const VectorVel& r2); + IMETHOD friend bool operator!=(const Vector& r1,const VectorVel& r2); + IMETHOD friend bool operator==(const VectorVel& r1,const Vector& r2); + IMETHOD friend bool operator!=(const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator - (const VectorVel& r); IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs); @@ -185,6 +193,13 @@ class RotationVel IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps); IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps); + IMETHOD friend bool operator==(const RotationVel& r1,const RotationVel& r2); + IMETHOD friend bool operator!=(const RotationVel& r1,const RotationVel& r2); + IMETHOD friend bool operator==(const Rotation& r1,const RotationVel& r2); + IMETHOD friend bool operator!=(const Rotation& r1,const RotationVel& r2); + IMETHOD friend bool operator==(const RotationVel& r1,const Rotation& r2); + IMETHOD friend bool operator!=(const RotationVel& r1,const Rotation& r2); + IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; @@ -239,6 +254,13 @@ class FrameVel IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps); IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps); + IMETHOD friend bool operator==(const FrameVel& a,const FrameVel& b); + IMETHOD friend bool operator!=(const FrameVel& a,const FrameVel& b); + IMETHOD friend bool operator==(const Frame& a,const FrameVel& b); + IMETHOD friend bool operator!=(const Frame& a,const FrameVel& b); + IMETHOD friend bool operator==(const FrameVel& a,const Frame& b); + IMETHOD friend bool operator!=(const FrameVel& a,const Frame& b); + IMETHOD TwistVel Inverse(const TwistVel& arg) const; IMETHOD TwistVel Inverse(const Twist& arg) const; IMETHOD TwistVel operator * (const TwistVel& arg) const; @@ -311,6 +333,13 @@ class TwistVel IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps); IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps); + IMETHOD friend bool operator==(const TwistVel& a,const TwistVel& b); + IMETHOD friend bool operator!=(const TwistVel& a,const TwistVel& b); + IMETHOD friend bool operator==(const Twist& a,const TwistVel& b); + IMETHOD friend bool operator!=(const Twist& a,const TwistVel& b); + IMETHOD friend bool operator==(const TwistVel& a,const Twist& b); + IMETHOD friend bool operator!=(const TwistVel& a,const Twist& b); + // = Conversion to other entities IMETHOD Twist GetTwist() const; IMETHOD Twist GetTwistDot() const; diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl index 61a43fdc2..d4490ebb1 100644 --- a/orocos_kdl/src/framevel.inl +++ b/orocos_kdl/src/framevel.inl @@ -81,6 +81,41 @@ bool Equal(const Frame& r1,const FrameVel& r2,double eps) { bool Equal(const FrameVel& r1,const Frame& r2,double eps) { return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); } +bool operator==(const FrameVel& r1,const FrameVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const FrameVel& r1,const FrameVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Frame& r1,const FrameVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const Frame& r1,const FrameVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const FrameVel& r1,const Frame& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.M == r2.M ); +#endif +} +bool operator!=(const FrameVel& r1,const Frame& r2) { + return !operator==(r1,r2); +} + + Frame FrameVel::GetFrame() const { return Frame(M.R,p.p); @@ -329,6 +364,40 @@ bool Equal(const Vector& r1,const VectorVel& r2,double eps) { bool Equal(const VectorVel& r1,const Vector& r2,double eps) { return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps)); } +bool operator==(const VectorVel& r1,const VectorVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2.p && + r1.v == r2.v ); +#endif +} +bool operator!=(const VectorVel& r1,const VectorVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Vector& r1,const VectorVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1 == r2.p && + Vector::Zero() == r2.v); +#endif +} +bool operator!=(const Vector& r1,const VectorVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const VectorVel& r1,const Vector& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.p == r2 && + r1.v == Vector::Zero() ); +#endif +} +bool operator!=(const VectorVel& r1,const Vector& r2) { + return !operator==(r1,r2); +} + bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) { return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps)); @@ -339,6 +408,41 @@ bool Equal(const Rotation& r1,const RotationVel& r2,double eps) { bool Equal(const RotationVel& r1,const Rotation& r2,double eps) { return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps)); } +bool operator==(const RotationVel& r1,const RotationVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.w == r2.w && + r1.R == r2.R ); +#endif +} +bool operator!=(const RotationVel& r1,const RotationVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const Rotation& r1,const RotationVel& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (Vector::Zero() == r2.w && + r1 == r2.R); +#endif +} +bool operator!=(const Rotation& r1,const RotationVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const RotationVel& r1,const Rotation& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (r1.w == Vector::Zero() && + r1.R == r2); +#endif +} +bool operator!=(const RotationVel& r1,const Rotation& r2) { + return !operator==(r1,r2); +} + + bool Equal(const TwistVel& a,const TwistVel& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); @@ -351,7 +455,39 @@ bool Equal(const TwistVel& a,const Twist& b,double eps) { return (Equal(a.rot,b.rot,eps)&& Equal(a.vel,b.vel,eps) ); } - +bool operator==(const TwistVel& a,const TwistVel& b) { +#ifdef KDL_USE_EQUAL + return Equal(a, b); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const TwistVel& a,const TwistVel& b) { + return !operator==(a,b); +} +bool operator==(const Twist& a,const TwistVel& b) { +#ifdef KDL_USE_EQUAL + return Equal(a, b); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const Twist& r1,const TwistVel& r2) { + return !operator==(r1,r2); +} +bool operator==(const TwistVel& r1,const Twist& r2) { +#ifdef KDL_USE_EQUAL + return Equal(r1, r2); +#else + return (a.rot == b.rot && + a.vel == b.vel ); +#endif +} +bool operator!=(const TwistVel& r1,const Twist& r2) { + return !operator==(r1,r2); +} IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) { @@ -364,17 +500,6 @@ IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) { return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v)); } - - - - - - - - - - - TwistVel TwistVel::Zero() { return TwistVel(VectorVel::Zero(),VectorVel::Zero()); diff --git a/orocos_kdl/src/utilities/rall1d.h b/orocos_kdl/src/utilities/rall1d.h index 4b3ba1ac1..cea245a92 100644 --- a/orocos_kdl/src/utilities/rall1d.h +++ b/orocos_kdl/src/utilities/rall1d.h @@ -471,6 +471,22 @@ INLINE bool Equal(const Rall1d& y,const Rall1d& x,double eps=epsi return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps)); } +template +INLINE bool operator==(const Rall1d& y,const Rall1d& x) +{ +#ifdef KDL_USE_EQUAL + return Equal(y, x); +#else + return (x.t == y.t && x.grad == y.grad); +#endif +} + +template +INLINE bool operator!=(const Rall1d& y,const Rall1d& x) +{ + return !operator==(y, x); +} + } diff --git a/orocos_kdl/src/utilities/rall2d.h b/orocos_kdl/src/utilities/rall2d.h index 811bad553..17072ffb7 100644 --- a/orocos_kdl/src/utilities/rall2d.h +++ b/orocos_kdl/src/utilities/rall2d.h @@ -531,6 +531,24 @@ INLINE bool Equal(const Rall2d& y,const Rall2d& x,double eps=epsi ); } +template +INLINE bool operator==(const Rall2d& y,const Rall2d& x) +{ +#ifdef KDL_USE_EQUAL + return Equal(y, x); +#else + return (x.t == y.t && + x.d == y.d && + x.dd == y.dd); +#endif + +} + +template +INLINE bool operator!=(const Rall2d& y,const Rall2d& x) +{ + return !operator==(y, x); +} } From 0a848e186c2a9667710c062b106b7b0cdf6cb636 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 22:46:24 +0100 Subject: [PATCH 059/136] (PyKDL) add equality operators --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index 60703a90a..729b7c843 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -48,6 +48,9 @@ void init_framevel(pybind11::module &m) return oss.str(); }); + double_vel.def(py::self == py::self); + double_vel.def(py::self != py::self); + m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, py::arg("a"), py::arg("b"), py::arg("dt")=1.0); m.def("addDelta", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::addDelta, @@ -97,6 +100,13 @@ void init_framevel(pybind11::module &m) vector_vel.def(py::self / double()); vector_vel.def(py::self / doubleVel()); + + vector_vel.def(py::self == py::self); + vector_vel.def(py::self != py::self); + vector_vel.def(Vector() == py::self); + vector_vel.def(Vector() != py::self); + vector_vel.def(py::self == Vector()); + vector_vel.def(py::self != Vector()); vector_vel.def("__neg__", [](const VectorVel &a) { return operator-(a); @@ -166,6 +176,13 @@ void init_framevel(pybind11::module &m) twist_vel.def(py::self + py::self); twist_vel.def(py::self - py::self); + + twist_vel.def(py::self == py::self); + twist_vel.def(py::self != py::self); + twist_vel.def(Twist() == py::self); + twist_vel.def(Twist() != py::self); + twist_vel.def(py::self == Twist()); + twist_vel.def(py::self != Twist()); twist_vel.def("__neg__", [](const TwistVel &a) { return operator-(a); @@ -239,6 +256,13 @@ void init_framevel(pybind11::module &m) rotation_vel.def(py::self * py::self); rotation_vel.def(Rotation() * py::self); rotation_vel.def(py::self * Rotation()); + + rotation_vel.def(py::self == py::self); + rotation_vel.def(py::self != py::self); + rotation_vel.def(Rotation() == py::self); + rotation_vel.def(Rotation() != py::self); + rotation_vel.def(py::self == Rotation()); + rotation_vel.def(py::self != Rotation()); rotation_vel.def(py::pickle( [](const RotationVel &rv) { // __getstate__ @@ -297,6 +321,13 @@ void init_framevel(pybind11::module &m) frame_vel.def(py::self * py::self); frame_vel.def(Frame() * py::self); frame_vel.def(py::self * Frame()); + + frame_vel.def(py::self == py::self); + frame_vel.def(py::self != py::self); + frame_vel.def(Frame() == py::self); + frame_vel.def(Frame() != py::self); + frame_vel.def(py::self == Frame()); + frame_vel.def(py::self != Frame()); frame_vel.def(py::pickle( [](const FrameVel &fv) { // __getstate__ From dd426022fb2918bbc9d2f3679ebcb72affae9555 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 22:58:08 +0100 Subject: [PATCH 060/136] (PyKDL)(sip) add equality operators --- python_orocos_kdl/PyKDL/sip/framevel.sip | 37 ++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 7b4353890..78e4522a8 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -43,9 +43,12 @@ public: doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0); doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0); + bool Equal(const doubleVel& r1,const doubleVel& r2,double eps=epsilon); -//bool Equal(const double& r1,const doubleVel& r2,double eps=epsilon); -//bool Equal(const doubleVel& r1,const double& r2,double eps=epsilon); + +bool operator==(const doubleVel& r1,const doubleVel& r2); +bool operator!=(const doubleVel& r1,const doubleVel& r2); + class VectorVel { @@ -108,11 +111,19 @@ bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); +bool operator==(const VectorVel& r1,const VectorVel& r2); +bool operator!=(const VectorVel& r1,const VectorVel& r2); +bool operator==(const Vector& r1,const VectorVel& r2); +bool operator!=(const Vector& r1,const VectorVel& r2); +bool operator==(const VectorVel& r1,const Vector& r2); +bool operator!=(const VectorVel& r1,const Vector& r2); + VectorVel operator - (const VectorVel& r); doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); doubleVel dot(const VectorVel& lhs,const Vector& rhs); doubleVel dot(const Vector& lhs,const VectorVel& rhs); + class RotationVel { %TypeHeaderCode @@ -177,7 +188,12 @@ bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon); bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon); bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); - +bool operator==(const RotationVel& r1,const RotationVel& r2); +bool operator!=(const RotationVel& r1,const RotationVel& r2); +bool operator==(const Rotation& r1,const RotationVel& r2); +bool operator!=(const Rotation& r1,const RotationVel& r2); +bool operator==(const RotationVel& r1,const Rotation& r2); +bool operator!=(const RotationVel& r1,const Rotation& r2); class FrameVel @@ -238,6 +254,14 @@ bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon); bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon); bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon); +bool operator==(const FrameVel& r1,const FrameVel& r2); +bool operator!=(const FrameVel& r1,const FrameVel& r2); +bool operator==(const Frame& r1,const FrameVel& r2); +bool operator!=(const Frame& r1,const FrameVel& r2); +bool operator==(const FrameVel& r1,const Frame& r2); +bool operator!=(const FrameVel& r1,const Frame& r2); + + class TwistVel { %TypeHeaderCode @@ -298,6 +322,13 @@ bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); +bool operator==(const TwistVel& r1,const TwistVel& r2); +bool operator!=(const TwistVel& r1,const TwistVel& r2); +bool operator==(const Twist& r1,const TwistVel& r2); +bool operator!=(const Twist& r1,const TwistVel& r2); +bool operator==(const TwistVel& r1,const Twist& r2); +bool operator!=(const TwistVel& r1,const Twist& r2); + VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0); VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0); From 09bd8cef7b0b6d6339ee73cad118e916f5bf8aa0 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 16 Mar 2020 23:05:47 +0100 Subject: [PATCH 061/136] (PyKDL) pickle test use assertEqual after adding equality operators --- python_orocos_kdl/tests/frameveltest.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 2615ceb74..0ffc8b2a5 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -106,10 +106,7 @@ def testPickle(self): with open('/tmp/pickle_test_kdl_framevel', 'r') as f: data1 = pickle.load(f) - self.assertTrue(Equal(data['vv'], data1['vv'])) - self.assertTrue(Equal(data['rv'], data1['rv'])) - self.assertTrue(Equal(data['fv'], data1['fv'])) - self.assertTrue(Equal(data['tv'], data1['tv'])) + self.assertEqual(data, data1) def suite(): From 564ea708e828a6fa2907367f12957da381ab08b3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 17 Mar 2020 19:34:56 +0100 Subject: [PATCH 062/136] (kdl) initialize members for Rall1d/Rall2d --- orocos_kdl/src/utilities/rall1d.h | 2 +- orocos_kdl/src/utilities/rall2d.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/utilities/rall1d.h b/orocos_kdl/src/utilities/rall1d.h index cea245a92..812c69fe3 100644 --- a/orocos_kdl/src/utilities/rall1d.h +++ b/orocos_kdl/src/utilities/rall1d.h @@ -56,7 +56,7 @@ class Rall1d T t; //!< value V grad; //!< gradient public : - INLINE Rall1d() {} + INLINE Rall1d():t(),grad() {}; T value() const { return t; diff --git a/orocos_kdl/src/utilities/rall2d.h b/orocos_kdl/src/utilities/rall2d.h index 17072ffb7..fd6f6f965 100644 --- a/orocos_kdl/src/utilities/rall2d.h +++ b/orocos_kdl/src/utilities/rall2d.h @@ -59,7 +59,7 @@ class Rall2d V dd; //!< 2nd derivative public : // = Constructors - INLINE Rall2d() {} + INLINE Rall2d():t(),d(),dd() {}; explicit INLINE Rall2d(typename TI::Arg c) {t=c;SetToZero(d);SetToZero(dd);} From ab11a578f5c3b1be0e0af0f424c361d6dcec035c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 20:42:18 +0100 Subject: [PATCH 063/136] (PyKDL) improve member tests --- python_orocos_kdl/tests/framestest.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index ab816f03f..dc20c740f 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -112,9 +112,9 @@ def testTwist(self): v2 = Vector(4, 5, 6) t = Twist(v1, v2) self.assertEqual(t.vel, v1) - self.assertEqual(Twist(t).vel, v1) self.assertEqual(t.rot, v2) - self.assertEqual(Twist(t).rot, v2) + self.assertEqual(Twist(t).vel, t.vel) + self.assertEqual(Twist(t).rot, t.rot) # Test __getitem__ for i in range(6): self.assertEqual(t[i], i+1) @@ -181,9 +181,9 @@ def testWrench(self): v2 = Vector(4, 5, 6) w = Wrench(v1, v2) self.assertEqual(w.force, v1) - self.assertEqual(Wrench(w).force, v1) self.assertEqual(w.torque, v2) - self.assertEqual(Wrench(w).torque, v2) + self.assertEqual(Wrench(w).force, w.force) + self.assertEqual(Wrench(w).torque, w.torque) # Test __getitem__ for i in range(6): self.assertEqual(w[i], i+1) @@ -321,9 +321,9 @@ def testFrame(self): self.assertEqual(f, f2) self.assertEqual(f.M, r) - self.assertEqual(Frame(f).M, r) self.assertEqual(f.p, v2) - self.assertEqual(Frame(f).p, v2) + self.assertEqual(Frame(f).M, f.M) + self.assertEqual(Frame(f).p, f.p) f = Frame(Rotation(1, 2, 3, 5, 6, 7, From b0b45826f3400d4440e6adbec095795f96f47c0b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 20:42:49 +0100 Subject: [PATCH 064/136] (PyKDL) add setToZero for VectorVel --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index 729b7c843..e09d16d7c 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -127,6 +127,7 @@ void init_framevel(pybind11::module &m) return vv; })); + m.def("SetToZero", (void (*)(VectorVel&)) &KDL::SetToZero); m.def("Equal", (bool (*)(const VectorVel&, const VectorVel&, double)) &KDL::Equal, py::arg("r1"), py::arg("r2"), py::arg("eps")=epsilon); m.def("Equal", (bool (*)(const Vector&, const VectorVel&, double)) &KDL::Equal, From 85177f0a24f45688495591069ef085acd34ed1fb Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 20:43:22 +0100 Subject: [PATCH 065/136] (PyKDL) assertEqual to assertAlmostEqual --- python_orocos_kdl/tests/framestest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index dc20c740f..e4b08c727 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -268,7 +268,7 @@ def testRotationImpl(self, v, a, b, c): self.assertEqual(dot(r.UnitZ(), r.UnitZ()), 1.0) self.assertAlmostEqual(dot(r.UnitX(), r.UnitY()), 0.0, 15) self.assertAlmostEqual(dot(r.UnitX(), r.UnitZ()), 0.0, 15) - self.assertEqual(dot(r.UnitY(), r.UnitZ()), 0.0) + self.assertAlmostEqual(dot(r.UnitY(), r.UnitZ()), 0.0, 15) r2 = Rotation(r) self.assertEqual(r, r2) self.assertAlmostEqual((r*v).Norm(), v.Norm(), 14) From 38d7f050e9687cdf7dcc0fd7f9a9a7c724003be5 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 22:00:13 +0100 Subject: [PATCH 066/136] (PyKDL) add constructor of doubleVel --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index e09d16d7c..e8c66ce42 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -36,6 +36,7 @@ void init_framevel(pybind11::module &m) // -------------------- py::class_ double_vel(m, "doubleVel"); double_vel.def(py::init<>()); + double_vel.def(py::init()); double_vel.def(py::init()); double_vel.def_readwrite("t", &doubleVel::t); double_vel.def_readwrite("grad", &doubleVel::grad); From b7b13109db8e1607dbcb3e93b9fa4e7d0eb3118a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 22:01:29 +0100 Subject: [PATCH 067/136] (PyKDL) update VectorVel tests --- python_orocos_kdl/tests/frameveltest.py | 40 +++++++++++++++++++++++-- 1 file changed, 37 insertions(+), 3 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 0ffc8b2a5..e0518c588 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -29,12 +29,46 @@ class FrameVelTestFunctions(unittest.TestCase): def testdoubleVel(self): d = doubleVel() - self.assertEqual(d.t, d.value()) - self.assertEqual(d.grad, d.deriv()) + self.assertEqual(doubleVel(d), d) + self.assertEqual(d.t, 0) + self.assertEqual(d.grad, 0) + self.assertEqual(d.value(), 0) + self.assertEqual(d.deriv(), 0) def testVectorVel(self): - v = VectorVel(Vector(3, -4, 5), Vector(6, 3, -5)) + v = VectorVel() + vt = Vector() + self.testVectorVelImpl(v, vt) vt = Vector(-4, -6, -8) + self.testVectorVelImpl(v, vt) + v1 = Vector(3, -4, 5) + v2 = Vector(6, 3, -5) + v = VectorVel(v1, v2) + self.testVectorVelImpl(v, vt) + + self.assertEqual(VectorVel(v).p, v.p) + self.assertEqual(VectorVel(v).v, v.v) + self.assertFalse(v == -v) # Doesn't work for zero VectorVel + self.assertFalse(Equal(v, -v)) # Doesn't work for zero VectorVel + self.assertTrue(v != -v) # Doesn't work for zero VectorVel + self.assertTrue(not Equal(v, -v)) # Doesn't work for zero VectorVel + + v = VectorVel(v1) + self.assertEqual(v, v1) + self.assertEqual(v1, v) + + v = VectorVel(v1, v2) + SetToZero(v) + self.assertEqual(v, VectorVel()) + self.assertEqual(VectorVel.Zero(), VectorVel()) + + v = VectorVel(v1, v2) + self.assertEqual(dot(v, v), doubleVel(dot(v.p, v.p), dot(v.p, v.v)+dot(v.v, v.p))) + dot_result = doubleVel(dot(v.p, v1), dot(v.v, v1)) + self.assertEqual(dot(v, v1), dot_result) + self.assertEqual(dot(v1, v), dot_result) + + def testVectorVelImpl(self, v, vt): self.assertTrue(Equal(2*v-v, v)) self.assertTrue(Equal(v*2-v, v)) self.assertTrue(Equal(v+v+v-2*v, v)) From a031317309f7c8aa55d3b807efa3a4ee88340868 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 23:13:13 +0100 Subject: [PATCH 068/136] (kdl) fix Rall1d io --- orocos_kdl/src/utilities/rall1d_io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/utilities/rall1d_io.h b/orocos_kdl/src/utilities/rall1d_io.h index 584595051..ac2115904 100644 --- a/orocos_kdl/src/utilities/rall1d_io.h +++ b/orocos_kdl/src/utilities/rall1d_io.h @@ -27,7 +27,7 @@ namespace KDL { template inline std::ostream& operator << (std::ostream& os,const Rall1d& r) { - os << "Rall1d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ", " <<"(" << r.t <<"," << r.grad <<")"; + os << "Rall1d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ">(" << r.t <<"," << r.grad <<")"; return os; } From 0ca4f2ef6cb80cc027760a981dc2e27cf57369e4 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 23:20:19 +0100 Subject: [PATCH 069/136] (PyKDL) add constructor of doubleVel --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index e8c66ce42..f56a2084f 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -36,6 +36,7 @@ void init_framevel(pybind11::module &m) // -------------------- py::class_ double_vel(m, "doubleVel"); double_vel.def(py::init<>()); + double_vel.def(py::init()); double_vel.def(py::init()); double_vel.def(py::init()); double_vel.def_readwrite("t", &doubleVel::t); From f9bd3116ed09b152f79e57a44f4053b120daf878 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 23:20:47 +0100 Subject: [PATCH 070/136] (PyKDL) add unary operator of doubleVel --- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index f56a2084f..9ea711aef 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -53,6 +53,11 @@ void init_framevel(pybind11::module &m) double_vel.def(py::self == py::self); double_vel.def(py::self != py::self); + double_vel.def("__neg__", [](const doubleVel &a) + { + return operator-(a); + }, py::is_operator()); + m.def("diff", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::diff, py::arg("a"), py::arg("b"), py::arg("dt")=1.0); m.def("addDelta", (doubleVel (*)(const doubleVel&, const doubleVel&, double)) &KDL::addDelta, From 4a2cfc4351925ecc9aebc22014498e053fc715d3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 23:21:55 +0100 Subject: [PATCH 071/136] (PyKDL) add member(function) tests for VectorVel --- python_orocos_kdl/tests/frameveltest.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index e0518c588..05401d16f 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -46,6 +46,11 @@ def testVectorVel(self): v = VectorVel(v1, v2) self.testVectorVelImpl(v, vt) + self.assertEqual(v.p, v1) + self.assertEqual(v.v, v2) + self.assertEqual(v.value(), v1) + self.assertEqual(v.deriv(), v2) + self.assertEqual(VectorVel(v).p, v.p) self.assertEqual(VectorVel(v).v, v.v) self.assertFalse(v == -v) # Doesn't work for zero VectorVel From 8592a2352a28c1f2b065d4be5b1be914e8e097b5 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 23:46:52 +0100 Subject: [PATCH 072/136] (PyKDL) add TwistVel tests --- python_orocos_kdl/tests/frameveltest.py | 58 +++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 05401d16f..6315a2901 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -89,6 +89,63 @@ def testVectorVelImpl(self, v, vt): v2 = VectorVel(Vector(-5, -6, -3), Vector(3, 4, 5)) self.assertTrue(Equal(v*v2, -v2*v)) + def testTwistVel(self): + t = TwistVel() + self.testTwistVelImpl(t) + v1 = Vector(1, 2, 3) + v2 = Vector(4, 5, 6) + vv1 = VectorVel(v1, v2) + vv2 = VectorVel(v2, v1) + t = TwistVel(vv1, vv2) + self.testTwistVelImpl(t) + + # Alternative constructor + tw1 = Twist(v1, v2) + tw2 = Twist(v2, v1) + t2 = TwistVel(tw1, tw2) + self.assertEqual(t, t2) + + # Members + self.assertEqual(t.vel, vv1) + self.assertEqual(t.rot, vv2) + self.assertEqual(t2.value(), tw1) + self.assertEqual(t2.deriv(), tw2) + self.assertEqual(t2.GetTwist(), tw1) + self.assertEqual(t2.GetTwistDot(), tw2) + + # Equality + self.assertEqual(TwistVel(t).vel, t.vel) + self.assertEqual(TwistVel(t).rot, t.rot) + self.assertFalse(t == -t) # Doesn't work for zero TwistVel + self.assertFalse(Equal(t, -t)) # Doesn't work for zero TwistVel + self.assertTrue(t != -t) # Doesn't work for zero TwistVel + self.assertTrue(not Equal(t, -t)) # Doesn't work for zero TwistVel + + t = TwistVel(VectorVel(v1), VectorVel(v2)) + t2 = Twist(v1, v2) + self.assertEqual(t, t2) + self.assertEqual(t2, t) + + # Zero + SetToZero(t) + self.assertEqual(t, TwistVel()) + self.assertEqual(TwistVel.Zero(), TwistVel()) + + def testTwistVelImpl(self, t): + self.assertTrue(Equal(2*t-t, t)) + self.assertTrue(Equal(t*2-t, t)) + self.assertTrue(Equal(t+t+t-2*t, t)) + t2 = TwistVel(t) + self.assertTrue(Equal(t, t2)) + t2 += t + self.assertTrue(Equal(2*t, t2)) + t2 -= t + self.assertTrue(Equal(t, t2)) + t2.ReverseSign() + self.assertTrue(t, -t2) + self.assertTrue(t*doubleVel(), doubleVel()*t) + self.assertTrue(t*doubleVel(5), doubleVel(5)*t) + def testRotationVel(self): v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2)) vt = Vector(2, 3, 4) @@ -152,6 +209,7 @@ def suite(): suite = unittest.TestSuite() suite.addTest(FrameVelTestFunctions('testdoubleVel')) suite.addTest(FrameVelTestFunctions('testVectorVel')) + suite.addTest(FrameVelTestFunctions('testTwistVel')) suite.addTest(FrameVelTestFunctions('testRotationVel')) suite.addTest(FrameVelTestFunctions('testFrameVel')) suite.addTest(FrameVelTestFunctions('testPickle')) From 23e43d22e912936faf65886e1006100eab830a6d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 20 Mar 2020 23:54:03 +0100 Subject: [PATCH 073/136] (PyKDL) add comments to tests --- python_orocos_kdl/tests/framestest.py | 40 ++++++++++++++++++------- python_orocos_kdl/tests/frameveltest.py | 4 +++ 2 files changed, 34 insertions(+), 10 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index e4b08c727..463411b2c 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -33,11 +33,13 @@ def testVector(self): v = Vector() self.testVectorImpl(v) + # Equality v = Vector(3, 4, 5) self.assertFalse(v == -v) # Doesn't work for zero vector self.assertFalse(Equal(v, -v)) # Doesn't work for zero vector self.assertTrue(v != -v) # Doesn't work for zero vector self.assertTrue(not Equal(v, -v)) # Doesn't work for zero vector + # Test member get and set functions self.assertEqual(v.x(), 3) v.x(1) @@ -48,7 +50,8 @@ def testVector(self): self.assertEqual(v.z(), 5) v.z(1) self.assertEqual(v, Vector(1, 1, 1)) - # Test __getitem__ + + # __getitem__ self.assertEqual(v[0], 1) self.assertEqual(v[1], 1) self.assertEqual(v[2], 1) @@ -56,7 +59,8 @@ def testVector(self): _ = v[-1] with self.assertRaises(IndexError): _ = v[3] - # Test __setitem__ + + # __setitem__ v[0] = 3 self.assertEqual(v[0], 3) v[1] = 4 @@ -68,6 +72,7 @@ def testVector(self): with self.assertRaises(IndexError): v[3] = 1 + # Zero SetToZero(v) self.assertEqual(v, Vector(0, 0, 0)) self.assertEqual(Vector.Zero(), Vector(0, 0, 0)) @@ -102,12 +107,14 @@ def testTwist(self): t = Twist(Vector(0, -9, -3), Vector(1, -2, -4)) self.testTwistImpl(t) + # Equality t = Twist(Vector(1, 2, 3), Vector(1, 2, 3)) self.assertFalse(t == -t) # Doesn't work for zero twist self.assertFalse(Equal(t, -t)) # Doesn't work for zero twist self.assertTrue(t != -t) # Doesn't work for zero twist self.assertTrue(not Equal(t, -t)) # Doesn't work for zero twist + # Members v1 = Vector(1, 2, 3) v2 = Vector(4, 5, 6) t = Twist(v1, v2) @@ -115,14 +122,16 @@ def testTwist(self): self.assertEqual(t.rot, v2) self.assertEqual(Twist(t).vel, t.vel) self.assertEqual(Twist(t).rot, t.rot) - # Test __getitem__ + + # __getitem__ for i in range(6): self.assertEqual(t[i], i+1) with self.assertRaises(IndexError): _ = t[-1] with self.assertRaises(IndexError): _ = t[6] - # Test __setitem__ + + # __setitem__ for i in range(6): t[i] = i for i in range(6): @@ -132,6 +141,7 @@ def testTwist(self): with self.assertRaises(IndexError): t[6] = 1 + # Zero SetToZero(t) self.assertEqual(t, Twist()) self.assertEqual(Twist.Zero(), Twist()) @@ -171,12 +181,14 @@ def testWrench(self): w = Wrench(Vector(2, 1, 4), Vector(5, 3, 1)) self.testWrenchImpl(w) + # Equality w = Wrench(Vector(1, 2, 3), Vector(1, 2, 3)) self.assertFalse(w == -w) # Doesn't work for zero wrench self.assertFalse(Equal(w, -w)) # Doesn't work for zero wrench self.assertTrue(w != -w) # Doesn't work for zero wrench self.assertTrue(not Equal(w, -w)) # Doesn't work for zero wrench + # Members v1 = Vector(1, 2, 3) v2 = Vector(4, 5, 6) w = Wrench(v1, v2) @@ -184,14 +196,16 @@ def testWrench(self): self.assertEqual(w.torque, v2) self.assertEqual(Wrench(w).force, w.force) self.assertEqual(Wrench(w).torque, w.torque) - # Test __getitem__ + + # __getitem__ for i in range(6): self.assertEqual(w[i], i+1) with self.assertRaises(IndexError): _ = w[-1] with self.assertRaises(IndexError): _ = w[6] - # Test __setitem__ + + # __setitem__ for i in range(6): w[i] = i for i in range(6): @@ -201,6 +215,7 @@ def testWrench(self): with self.assertRaises(IndexError): w[6] = 1 + # Zero SetToZero(w) self.assertEqual(w, Wrench()) self.assertEqual(Wrench.Zero(), Wrench()) @@ -230,7 +245,8 @@ def testRotation(self): self.testRotationImpl(Vector(), radians(0), radians(0), radians(0)) r = Rotation(*range(1, 10)) - # Test __getitem__ + + # __getitem__ for i in range(3): for j in range(3): self.assertEqual(r[i, j], 3*i+j+1) @@ -242,7 +258,8 @@ def testRotation(self): _ = r[3, 2] with self.assertRaises(IndexError): _ = r[2, 3] - # Test __setitem__ + + # __setitem__ for i in range(3): for j in range(3): r[i, j] = 3*i+j @@ -317,9 +334,11 @@ def testFrame(self): f = Frame(r, v2) self.testFrameImpl(v, w, t, f) + # Equality f2 = Frame(f) self.assertEqual(f, f2) + # Members self.assertEqual(f.M, r) self.assertEqual(f.p, v2) self.assertEqual(Frame(f).M, f.M) @@ -329,7 +348,7 @@ def testFrame(self): 5, 6, 7, 9, 10, 11), Vector(4, 8, 12)) - # Test __getitem__ + # __getitem__ for i in range(3): for j in range(4): self.assertEqual(f[i, j], 4*i+j+1) @@ -341,7 +360,8 @@ def testFrame(self): _ = f[3, 3] with self.assertRaises(IndexError): _ = f[2, 4] - # Test __setitem__ + + # __setitem__ for i in range(3): for j in range(4): f[i, j] = 4*i+j diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 6315a2901..9e5254f73 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -46,11 +46,13 @@ def testVectorVel(self): v = VectorVel(v1, v2) self.testVectorVelImpl(v, vt) + # Members self.assertEqual(v.p, v1) self.assertEqual(v.v, v2) self.assertEqual(v.value(), v1) self.assertEqual(v.deriv(), v2) + # Equality self.assertEqual(VectorVel(v).p, v.p) self.assertEqual(VectorVel(v).v, v.v) self.assertFalse(v == -v) # Doesn't work for zero VectorVel @@ -62,11 +64,13 @@ def testVectorVel(self): self.assertEqual(v, v1) self.assertEqual(v1, v) + # Zero v = VectorVel(v1, v2) SetToZero(v) self.assertEqual(v, VectorVel()) self.assertEqual(VectorVel.Zero(), VectorVel()) + # dot functions v = VectorVel(v1, v2) self.assertEqual(dot(v, v), doubleVel(dot(v.p, v.p), dot(v.p, v.v)+dot(v.v, v.p))) dot_result = doubleVel(dot(v.p, v1), dot(v.v, v1)) From fc983828fb40a01d965427e22432153b842118c6 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 21 Mar 2020 00:00:04 +0100 Subject: [PATCH 074/136] (PyKDL)(sip) add doubleVel constructors --- python_orocos_kdl/PyKDL/sip/framevel.sip | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 78e4522a8..67dfa214b 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -28,6 +28,10 @@ class doubleVel using namespace KDL; %End public: + doubleVel(); + doubleVel(const double c); + doubleVel(const double tn, const double afg); + doubleVel(const doubleVel& r); double t; double grad; double value(); From f36daccc71e1a25e4d2079e319dbf7b3b2e1ba2f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 21 Mar 2020 00:00:38 +0100 Subject: [PATCH 075/136] (PyKDL)(sip) add vectorVel setToZero --- python_orocos_kdl/PyKDL/sip/framevel.sip | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 67dfa214b..cccab64ac 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -110,7 +110,7 @@ VectorVel operator*(const Rotation& R,const VectorVel& x); VectorVel operator / (const VectorVel& r1,double r2); VectorVel operator / (const VectorVel& r2,const doubleVel& r1); - +void SetToZero(VectorVel& v); bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); From 0859e4324b4e3e9be09e00508d5fe5f6a4567a0a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 21 Mar 2020 08:33:58 +0100 Subject: [PATCH 076/136] (kdl) add type info to Rall2d io --- orocos_kdl/src/utilities/rall2d_io.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/orocos_kdl/src/utilities/rall2d_io.h b/orocos_kdl/src/utilities/rall2d_io.h index 8870243a5..5ea145cd2 100644 --- a/orocos_kdl/src/utilities/rall2d_io.h +++ b/orocos_kdl/src/utilities/rall2d_io.h @@ -19,7 +19,7 @@ #define Rall2d_IO_H - +#include #include "utility_io.h" #include "rall2d.h" @@ -28,7 +28,7 @@ namespace KDL { template std::ostream& operator << (std::ostream& os,const Rall2d& r) { - os << "Rall2d(" << r.t <<"," << r.d <<","<(" << r.t <<"," << r.d <<","< Date: Sat, 21 Mar 2020 23:29:15 +0100 Subject: [PATCH 077/136] (PyKDL) add unary test for doubleVel --- python_orocos_kdl/tests/frameveltest.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 9e5254f73..904af228b 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -34,6 +34,9 @@ def testdoubleVel(self): self.assertEqual(d.grad, 0) self.assertEqual(d.value(), 0) self.assertEqual(d.deriv(), 0) + d2 = -d + self.assertEqual(d2.t, -d.t) + self.assertEqual(d2.grad, -d.grad) def testVectorVel(self): v = VectorVel() From 70ba2bde1275c7d0f82f5e9e2b2cba25b43dbdef Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 22 Mar 2020 10:18:24 +0100 Subject: [PATCH 078/136] (PyKDL) use assertEqual in VectorVel test --- python_orocos_kdl/tests/frameveltest.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 904af228b..326b58671 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -81,20 +81,20 @@ def testVectorVel(self): self.assertEqual(dot(v1, v), dot_result) def testVectorVelImpl(self, v, vt): - self.assertTrue(Equal(2*v-v, v)) - self.assertTrue(Equal(v*2-v, v)) - self.assertTrue(Equal(v+v+v-2*v, v)) + self.assertEqual(2*v-v, v) + self.assertEqual(v*2-v, v) + self.assertEqual(v+v+v-2*v, v) v2 = VectorVel(v) - self.assertTrue(Equal(v, v2)) + self.assertEqual(v, v2) v2 += v - self.assertTrue(Equal(2*v, v2)) + self.assertEqual(2*v, v2) v2 -= v - self.assertTrue(Equal(v, v2)) + self.assertEqual(v, v2) v2.ReverseSign() - self.assertTrue(Equal(v, -v2)) - self.assertTrue(Equal(v*vt, -vt*v)) + self.assertEqual(v, -v2) + self.assertEqual(v*vt, -vt*v) v2 = VectorVel(Vector(-5, -6, -3), Vector(3, 4, 5)) - self.assertTrue(Equal(v*v2, -v2*v)) + self.assertEqual(v*v2, -v2*v) def testTwistVel(self): t = TwistVel() From 033a8e64757217ea7160218e89da6ab8544f4c9c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 22 Mar 2020 10:18:37 +0100 Subject: [PATCH 079/136] (PyKDL) use assertEqual in TwistVel test --- python_orocos_kdl/tests/frameveltest.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 326b58671..e57f77fb7 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -139,15 +139,15 @@ def testTwistVel(self): self.assertEqual(TwistVel.Zero(), TwistVel()) def testTwistVelImpl(self, t): - self.assertTrue(Equal(2*t-t, t)) - self.assertTrue(Equal(t*2-t, t)) - self.assertTrue(Equal(t+t+t-2*t, t)) + self.assertEqual(2*t-t, t) + self.assertEqual(t*2-t, t) + self.assertEqual(t+t+t-2*t, t) t2 = TwistVel(t) - self.assertTrue(Equal(t, t2)) + self.assertEqual(t, t2) t2 += t - self.assertTrue(Equal(2*t, t2)) + self.assertEqual(2*t, t2) t2 -= t - self.assertTrue(Equal(t, t2)) + self.assertEqual(t, t2) t2.ReverseSign() self.assertTrue(t, -t2) self.assertTrue(t*doubleVel(), doubleVel()*t) From 8d2937556145fbce434adc5770d47a9744fa9a95 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 22 Mar 2020 10:19:38 +0100 Subject: [PATCH 080/136] (PyKDL) extend TwistVel test --- python_orocos_kdl/tests/frameveltest.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index e57f77fb7..ea2f184c0 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -152,6 +152,7 @@ def testTwistVelImpl(self, t): self.assertTrue(t, -t2) self.assertTrue(t*doubleVel(), doubleVel()*t) self.assertTrue(t*doubleVel(5), doubleVel(5)*t) + self.assertTrue(t * doubleVel(3, 5), doubleVel(3, 5) * t) def testRotationVel(self): v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2)) From 725fcd53518be6def3440b08331b18fa687100db Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 22 Mar 2020 10:45:17 +0100 Subject: [PATCH 081/136] (PyKDL)(sip) add unary operator to doubleVel --- python_orocos_kdl/PyKDL/sip/framevel.sip | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index cccab64ac..1ca1ee5a5 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -45,6 +45,8 @@ public: %End }; +doubleVel operator - (const doubleVel& r); + doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0); doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0); From 57dab43472181cd7b0fd9e38e85dcfbd3da9ae7a Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 22 Mar 2020 10:45:45 +0100 Subject: [PATCH 082/136] (PyKDL)(sip) styling in VectorVel --- python_orocos_kdl/PyKDL/sip/framevel.sip | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 1ca1ee5a5..57f327480 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -108,7 +108,7 @@ VectorVel operator * (const VectorVel& r1,double r2); VectorVel operator * (double r1,const VectorVel& r2); VectorVel operator * (const doubleVel& r1,const VectorVel& r2); VectorVel operator * (const VectorVel& r2,const doubleVel& r1); -VectorVel operator*(const Rotation& R,const VectorVel& x); +VectorVel operator * (const Rotation& R,const VectorVel& x); VectorVel operator / (const VectorVel& r1,double r2); VectorVel operator / (const VectorVel& r2,const doubleVel& r1); From 45d8666c7d42b543ac88bb3573795a6032734d85 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 22 Mar 2020 11:36:14 +0100 Subject: [PATCH 083/136] (PyKDL) fix TwistVel test --- python_orocos_kdl/tests/frameveltest.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index ea2f184c0..82425a3da 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -149,10 +149,10 @@ def testTwistVelImpl(self, t): t2 -= t self.assertEqual(t, t2) t2.ReverseSign() - self.assertTrue(t, -t2) - self.assertTrue(t*doubleVel(), doubleVel()*t) - self.assertTrue(t*doubleVel(5), doubleVel(5)*t) - self.assertTrue(t * doubleVel(3, 5), doubleVel(3, 5) * t) + self.assertEqual(t, -t2) + self.assertEqual(t*doubleVel(), doubleVel()*t) + self.assertEqual(t*doubleVel(5), doubleVel(5)*t) + self.assertEqual(t * doubleVel(3, 5), doubleVel(3, 5) * t) def testRotationVel(self): v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2)) From 5074de10fb491a440ba1a8d5876a2d7de25b30d0 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 22 Mar 2020 17:17:18 +0100 Subject: [PATCH 084/136] (kdl) don't divide by zero Norm of derivative of VectorVel should be zero incase the p vector norm is zero. --- orocos_kdl/src/framevel.inl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl index d4490ebb1..c955905cb 100644 --- a/orocos_kdl/src/framevel.inl +++ b/orocos_kdl/src/framevel.inl @@ -352,7 +352,7 @@ void VectorVel::ReverseSign() { } doubleVel VectorVel::Norm() const { double n = p.Norm(); - return doubleVel(n,dot(p,v)/n); + return doubleVel(n,n ? dot(p,v)/n : 0); // Setting norm of p to 0 in case norm of v is 0 } bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) { From 19895c72caf07cb1ace44237c9fb9b1c94aa40bf Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 23 Mar 2020 22:31:16 +0100 Subject: [PATCH 085/136] (PyKDL) Improve Rotation test --- python_orocos_kdl/tests/framestest.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 463411b2c..add19d0a1 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -239,10 +239,10 @@ def testWrenchImpl(self, w): self.assertEqual(w2.torque, w.torque + w.force*v) def testRotation(self): - self.testRotationImpl(Vector(3, 4, 5), radians(10), radians(20), radians(30)) - self.testRotationImpl(Vector(), radians(10), radians(20), radians(30)) - self.testRotationImpl(Vector(3, 4, 5), radians(0), radians(0), radians(0)) - self.testRotationImpl(Vector(), radians(0), radians(0), radians(0)) + self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector(3, 4, 5)) + self.testRotationImpl(Rotation.RPY(radians(10), radians(20), radians(30)), Vector()) + self.testRotationImpl(Rotation(), Vector(3, 4, 5)) + self.testRotationImpl(Rotation(), Vector()) r = Rotation(*range(1, 10)) @@ -275,10 +275,10 @@ def testRotation(self): with self.assertRaises(IndexError): r[2, 3] = 1 - def testRotationImpl(self, v, a, b, c): + def testRotationImpl(self, r, v): w = Wrench(Vector(7, -1, 3), Vector(2, -3, 3)) t = Twist(Vector(6, 3, 5), Vector(4, -2, 7)) - r = Rotation.RPY(a, b, c) + a, b, c = r.GetRPY() self.assertAlmostEqual(dot(r.UnitX(), r.UnitX()), 1.0, 15) self.assertEqual(dot(r.UnitY(), r.UnitY()), 1.0) From b337902ded49329c6c2a04de64cdb076738f8d95 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 10:00:35 +0100 Subject: [PATCH 086/136] (PyKDL) add JntToCart functions to ChainFkSolvers --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index 7cab3cbf8..76b9b7da0 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -318,6 +318,8 @@ void init_kinfam(pybind11::module &m) py::class_ chain_fk_solver_pos(m, "ChainFkSolverPos"); chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, Frame&, int)) &ChainFkSolverPos::JntToCart, py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + chain_fk_solver_pos.def("JntToCart", (int (ChainFkSolverPos::*)(const JntArray&, std::vector&, int)) &ChainFkSolverPos::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); // -------------------- @@ -326,6 +328,8 @@ void init_kinfam(pybind11::module &m) py::class_ chain_fk_solver_vel(m, "ChainFkSolverVel"); chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart, py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); + chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector&, int)) &ChainFkSolverVel::JntToCart, + py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); // ------------------------------ From cb6fb98817674c23c568377f3e8a59d2f3fd031f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 10:44:28 +0100 Subject: [PATCH 087/136] (PyKDL)(sip) add JntToCart functions to ChainFkSolvers --- python_orocos_kdl/PyKDL/sip/kinfam.sip | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index 78a9b738e..92e7017a8 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -440,7 +440,8 @@ class ChainFkSolverPos : SolverI #include using namespace KDL; %End - virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0; + virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1)=0; + virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1)=0; }; class ChainFkSolverVel : SolverI @@ -449,8 +450,8 @@ class ChainFkSolverVel : SolverI #include using namespace KDL; %End - virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out,int - segmentNr=-1)=0; + virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out, int segmentNr=-1)=0; + virtual int JntToCart(const JntArrayVel& q_in, std::vector& p_out, int segmentNr=-1)=0; }; class ChainFkSolverPos_recursive : ChainFkSolverPos @@ -462,7 +463,8 @@ using namespace KDL; public: ChainFkSolverPos_recursive(const Chain& chain); - virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1); + virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1); + virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1); virtual void updateInternalDataStructures(); }; @@ -474,8 +476,8 @@ using namespace KDL; %End public: ChainFkSolverVel_recursive(const Chain& chain); - virtual int JntToCart(const JntArrayVel& q_in ,FrameVel& out,int - segmentNr=-1 ); + virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out, int segmentNr=-1); + virtual int JntToCart(const JntArrayVel& q_in, std::vector& out, int segmentNr=-1 ); virtual void updateInternalDataStructures(); }; From 0e2a8ae6ff37493a3a6321ca03a83ebd46b000f3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 11:55:46 +0100 Subject: [PATCH 088/136] (PyKDL)(sip) move std::vector support to seperate file --- python_orocos_kdl/PyKDL/sip/PyKDL.sip | 9 +- python_orocos_kdl/PyKDL/sip/dynamics.sip | 1 - python_orocos_kdl/PyKDL/sip/framevel.sip | 1 + python_orocos_kdl/PyKDL/sip/kinfam.sip | 85 ----- python_orocos_kdl/PyKDL/sip/std_vector.sip | 341 +++++++++++++++++++++ 5 files changed, 347 insertions(+), 90 deletions(-) create mode 100644 python_orocos_kdl/PyKDL/sip/std_vector.sip diff --git a/python_orocos_kdl/PyKDL/sip/PyKDL.sip b/python_orocos_kdl/PyKDL/sip/PyKDL.sip index 1de04528c..9910de61f 100644 --- a/python_orocos_kdl/PyKDL/sip/PyKDL.sip +++ b/python_orocos_kdl/PyKDL/sip/PyKDL.sip @@ -20,13 +20,14 @@ //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -%Module(name = PyKDL,version=0,keyword_arguments="All") +%Module(name = PyKDL, keyword_arguments="All") -%License(type="LGPL",licensee="Ruben Smits",signature="ruben@intermodalics.eu",timestamp="2014") +%License(type="LGPL", licensee="Ruben Smits", signature="ruben@intermodalics.eu", timestamp="2014") %Include std_string.sip +%Include std_vector.sip + %Include frames.sip -%Include kinfam.sip %Include framevel.sip - +%Include kinfam.sip %Include dynamics.sip diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index 36d6a569b..f9b228094 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -19,7 +19,6 @@ //License along with this library; if not, write to the Free Software //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -%Include std_string.sip class JntSpaceInertiaMatrix { %TypeHeaderCode diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 57f327480..5ee1438f9 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -19,6 +19,7 @@ //License along with this library; if not, write to the Free Software //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + class doubleVel { diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index 92e7017a8..1bc8ecd2a 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -20,91 +20,6 @@ //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -template -%MappedType std::vector -{ -%TypeHeaderCode -#include -%End - -%ConvertFromTypeCode - PyObject *l = PyList_New(sipCpp -> size()); - - // Create the Python list of the correct length. - if (!l) - return NULL; - - // Go through each element in the C++ instance and convert it to a - // wrapped P2d. - for (int i = 0; i < (int)sipCpp->size(); ++i) { - TYPE *cpp = new TYPE(sipCpp->at(i)); - PyObject *pobj = sipConvertFromInstance(cpp, sipClass_TYPE, sipTransferObj); - - // Get the Python wrapper for the Type instance, creating a new - // one if necessary, and handle any ownership transfer. - if (!pobj) { - // There was an error so garbage collect the Python list. - Py_DECREF(l); - return NULL; - } - - // Add the wrapper to the list. - PyList_SET_ITEM(l, i, pobj); - } - - // Return the Python list. - return l; -%End - -%ConvertToTypeCode - // Check if type is compatible - if (!sipIsErr) { - // Must be any iterable - PyObject *i = PyObject_GetIter(sipPy); - bool iterable = (i != NULL); - Py_XDECREF(i); - return iterable; - } - - // Iterate over the object - PyObject *iterator = PyObject_GetIter(sipPy); - PyObject *item; - - std::vector *V = new std::vector(); - - while ((item = PyIter_Next(iterator))) - { - if (!sipCanConvertToInstance(item, sipClass_TYPE, SIP_NOT_NONE)) { - PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to TYPE"); - *sipIsErr = 1; - break; - } - - int state; - TYPE* p = reinterpret_cast( - sipConvertToInstance(item, sipClass_TYPE, 0, SIP_NOT_NONE, &state, sipIsErr)); - - if (!*sipIsErr) - V->push_back(*p); - - sipReleaseInstance(p, sipClass_TYPE, state); - Py_DECREF(item); - } - - Py_DECREF(iterator); - - if (*sipIsErr) { - delete V; - return 0; - } - - *sipCppPtr = V; - return sipGetState(sipTransferObj); -%End -}; - - - class Joint{ %TypeHeaderCode diff --git a/python_orocos_kdl/PyKDL/sip/std_vector.sip b/python_orocos_kdl/PyKDL/sip/std_vector.sip new file mode 100644 index 000000000..7563d2b91 --- /dev/null +++ b/python_orocos_kdl/PyKDL/sip/std_vector.sip @@ -0,0 +1,341 @@ +// SIP support for std::vector +// by Giovanni Bajo develer.com> +// Public domain + +// **************************************************** +// SIP generic implementation for std::vector<> +// **************************************************** +// ALas, this template-based generic implementation is valid only +// if the element type is a SIP-wrapped type. For basic types (int, double, etc.) +// we are forced to cut & paste to provide a specialization. + +template +%MappedType std::vector +{ +%TypeHeaderCode +#include +%End + +%ConvertFromTypeCode + PyObject *l = PyList_New(sipCpp -> size()); + + // Create the Python list of the correct length. + if (!l) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped P2d. + for (int i = 0; i < (int)sipCpp->size(); ++i) { + TYPE *cpp = new TYPE(sipCpp->at(i)); + PyObject *pobj = sipConvertFromInstance(cpp, sipClass_TYPE, sipTransferObj); + + // Get the Python wrapper for the Type instance, creating a new + // one if necessary, and handle any ownership transfer. + if (!pobj) { + // There was an error so garbage collect the Python list. + Py_DECREF(l); + return NULL; + } + + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, pobj); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (!sipIsErr) { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + std::vector *V = new std::vector(); + + while ((item = PyIter_Next(iterator))) + { + if (!sipCanConvertToInstance(item, sipClass_TYPE, SIP_NOT_NONE)) { + PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to TYPE"); + *sipIsErr = 1; + break; + } + + int state; + TYPE* p = reinterpret_cast( + sipConvertToInstance(item, sipClass_TYPE, 0, SIP_NOT_NONE, &state, sipIsErr)); + + if (!*sipIsErr) + V->push_back(*p); + + sipReleaseInstance(p, sipClass_TYPE, state); + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) { + delete V; + return 0; + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; + +// **************************************************** +// Specialization for std::vector +// **************************************************** + +%MappedType std::vector +{ +%TypeHeaderCode +#include +%End + +%ConvertFromTypeCode + PyObject *l; + + // Create the Python list of the correct length. + if ((l = PyList_New(sipCpp -> size())) == NULL) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped object. + for (int i = 0; i < (int)sipCpp -> size(); ++i) + { + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, PyFloat_FromDouble(sipCpp -> at(i))); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (sipIsErr == NULL) + { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + // Maximum number of elements + int len = PyObject_Size(sipPy); + std::vector *V = new std::vector(); + V->reserve(len); + + if (len) + { + while ((item = PyIter_Next(iterator))) + { + if (!PyNumber_Check(item)) + { + PyErr_Format(PyExc_TypeError, "object in iterable is not a number"); + *sipIsErr = 1; + break; + } + + PyObject *f = PyNumber_Float(item); + V->push_back(PyFloat_AsDouble(f)); + + Py_DECREF(f); + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) + { + delete V; + return 0; + } + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; + +// **************************************************** +// Specialization for std::vector +// **************************************************** + +%MappedType std::vector +{ +%TypeHeaderCode +#include +%End + +%ConvertFromTypeCode + PyObject *l; + + // Create the Python list of the correct length. + if ((l = PyList_New((SIP_SSIZE_T)sipCpp -> size())) == NULL) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped object. + for (int i = 0; i < (int)sipCpp -> size(); ++i) + { + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, PyInt_FromLong(sipCpp -> at(i))); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (sipIsErr == NULL) + { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + // Maximum number of elements + int len = PyObject_Size(sipPy); + std::vector *V = new std::vector(); + V->reserve(len); + + if (len) + { + while ((item = PyIter_Next(iterator))) + { + if (!PyInt_Check(item)) + { + PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float"); + *sipIsErr = 1; + break; + } + + int val = PyInt_AsLong(item); + V->push_back(val); + + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) + { + delete V; + return 0; + } + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; + + +// **************************************************** +// Specialization for std::vector +// **************************************************** + +%MappedType std::vector +{ +%TypeHeaderCode +#include +%End + +%ConvertFromTypeCode + PyObject *l; + + // Create the Python list of the correct length. + if ((l = PyList_New(sipCpp -> size())) == NULL) + return NULL; + + // Go through each element in the C++ instance and convert it to a + // wrapped object. + for (int i = 0; i < (int)sipCpp -> size(); ++i) + { + // Add the wrapper to the list. + PyList_SET_ITEM(l, i, PyInt_FromLong(sipCpp -> at(i))); + } + + // Return the Python list. + return l; +%End + +%ConvertToTypeCode + // Check if type is compatible + if (sipIsErr == NULL) + { + // Must be any iterable + PyObject *i = PyObject_GetIter(sipPy); + bool iterable = (i != NULL); + Py_XDECREF(i); + return iterable; + } + + // Iterate over the object + PyObject *iterator = PyObject_GetIter(sipPy); + PyObject *item; + + // Maximum number of elements + Py_ssize_t size = PyObject_Size(sipPy); + if (size == -1) { + Py_DECREF(iterator); + return 0; + } + + unsigned int len = size; + std::vector *V = new std::vector(); + V->reserve(len); + + if (len) + { + while ((item = PyIter_Next(iterator))) + { + if (!PyInt_Check(item)) + { + PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float"); + *sipIsErr = 1; + break; + } + + unsigned int val = PyInt_AsLong(item); + V->push_back(val); + + Py_DECREF(item); + } + + Py_DECREF(iterator); + + if (*sipIsErr) + { + delete V; + return 0; + } + } + + *sipCppPtr = V; + return sipGetState(sipTransferObj); +%End +}; From de2d11441dc63a7d87715d919014e788a0c457a9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 17:50:51 +0100 Subject: [PATCH 089/136] (PyKDL)(sip) add __setitem__ to JntSpaceInertiaMatrix --- python_orocos_kdl/PyKDL/sip/dynamics.sip | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index f9b228094..31bcf744d 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -32,7 +32,6 @@ public: void resize(unsigned int newSize); unsigned int rows()const; unsigned int columns()const; - //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg); double __getitem__(SIP_PYTUPLE); %MethodCode int i,j; @@ -45,6 +44,19 @@ public: sipRes=(*sipCpp)(i,j); } %End + void __setitem__(SIP_PYTUPLE,double value); +%MethodCode + int i,j; + PyArg_ParseTuple(a0,"ii",&i,&j); + if (i < 0 || j < 0 || i > (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { + PyErr_SetString(PyExc_IndexError, "Inertia index out of range"); + sipError = sipErrorFail; + } + else { + (*sipCpp)(i,j)=a1; + } +%End + //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg); //double operator()(unsigned int i,unsigned int j)const; //double& operator()(unsigned int i,unsigned int j); //bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); From ea317a8f1d8769630e71eef7de5735428366f5cf Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 17:51:57 +0100 Subject: [PATCH 090/136] (PyKDL) update RotationVel tests --- python_orocos_kdl/tests/frameveltest.py | 50 +++++++++++++++++-------- 1 file changed, 34 insertions(+), 16 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 82425a3da..fa9ac65bf 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -155,24 +155,42 @@ def testTwistVelImpl(self, t): self.assertEqual(t * doubleVel(3, 5), doubleVel(3, 5) * t) def testRotationVel(self): + v = VectorVel() + vt = Vector() + r = RotationVel() + self.testRotationVelImpl(r, v, vt) v = VectorVel(Vector(9, 4, -2), Vector(-5, 6, -2)) vt = Vector(2, 3, 4) - a = radians(-15) - b = radians(20) - c = radians(-80) - R = RotationVel(Rotation.RPY(a, b, c), Vector(2, 4, 1)) - R2 = RotationVel(R) - self.assertTrue(Equal(R, R2)) - self.assertTrue(Equal((R*v).Norm(), (v.Norm()))) - self.assertTrue(Equal(R.Inverse(R*v), v)) - self.assertTrue(Equal(R*R.Inverse(v), v)) - self.assertTrue(Equal(R*Rotation.Identity(), R)) - self.assertTrue(Equal(Rotation.Identity()*R, R)) - self.assertTrue(Equal(R*(R*(R*v)), (R*R*R)*v)) - self.assertTrue(Equal(R*(R*(R*vt)), (R*R*R)*vt)) - self.assertTrue(Equal(R*R.Inverse(), RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*R, RotationVel.Identity())) - self.assertTrue(Equal(R.Inverse()*v, R.Inverse(v))) + rot = Rotation.RPY(radians(-15), radians(20), radians(-80)) + vec = Vector(2, 4, 1) + r = RotationVel(rot, vec) + self.testRotationVelImpl(r, v, vt) + + # Members + self.assertEqual(r.R, rot) + self.assertEqual(r.w, vec) + self.assertEqual(r.value(), rot) + self.assertEqual(r.deriv(), vec) + + # Equality + self.assertEqual(RotationVel(r).R, r.R) + self.assertEqual(RotationVel(r).w, r.w) + self.assertEqual(RotationVel(rot), rot) + self.assertEqual(rot, RotationVel(rot)) + + def testRotationVelImpl(self, r, v, vt): + r2 = RotationVel(r) + self.assertEqual(r, r2) + self.assertEqual((r*v).Norm(), v.Norm()) + self.assertEqual(r.Inverse(r*v), v) + self.assertEqual(r*r.Inverse(v), v) + self.assertEqual(r*Rotation.Identity(), r) + self.assertEqual(Rotation.Identity()*r, r) + self.assertEqual(r*(r*(r*v)), (r*r*r)*v) + self.assertEqual(r*(r*(r*vt)), (r*r*r)*vt) + self.assertEqual(r*r.Inverse(), RotationVel.Identity()) + self.assertEqual(r.Inverse()*r, RotationVel.Identity()) + self.assertEqual(r.Inverse()*v, r.Inverse(v)) def testFrameVel(self): v = VectorVel(Vector(3, 4, 5), Vector(-2, -4, -1)) From f842b4ada1d69200332317a771948adbde3ad150 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 20:47:30 +0100 Subject: [PATCH 091/136] (PyKDL) update FrameVel tests --- python_orocos_kdl/tests/frameveltest.py | 63 +++++++++++++++++++------ 1 file changed, 48 insertions(+), 15 deletions(-) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index fa9ac65bf..ab2e5bab8 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -193,23 +193,56 @@ def testRotationVelImpl(self, r, v, vt): self.assertEqual(r.Inverse()*v, r.Inverse(v)) def testFrameVel(self): + v = VectorVel() + vt = Vector() + f = FrameVel() + self.testFrameVelImpl(f, v, vt) + fr_m = Rotation.EulerZYX(radians(10), radians(20), radians(-10)) + fr_p = Vector(4, -2, 1) + tw_vel = Vector(2, -2, -2) + tw_rot = Vector(-5, -3, -2) + fr = Frame(fr_m, fr_p) + tw = Twist(tw_vel, tw_rot) + f = FrameVel(fr, tw) + self.testFrameVelImpl(f, v, vt) v = VectorVel(Vector(3, 4, 5), Vector(-2, -4, -1)) vt = Vector(-1, 0, -10) - F = FrameVel(Frame(Rotation.EulerZYX(radians(10), radians(20), radians(-10)), Vector(4, -2, 1)), - Twist(Vector(2, -2, -2), Vector(-5, -3, -2))) - F2 = FrameVel(F) - self.assertTrue(Equal(F, F2)) - self.assertTrue(Equal(F.Inverse(F*v), v)) - self.assertTrue(Equal(F.Inverse(F*vt), vt)) - self.assertTrue(Equal(F*F.Inverse(v), v)) - self.assertTrue(Equal(F*F.Inverse(vt), vt)) - self.assertTrue(Equal(F*Frame.Identity(), F)) - self.assertTrue(Equal(Frame.Identity()*F, F)) - self.assertTrue(Equal(F*(F*(F*v)), (F*F*F)*v)) - self.assertTrue(Equal(F*(F*(F*vt)), (F*F*F)*vt)) - self.assertTrue(Equal(F*F.Inverse(), FrameVel.Identity())) - self.assertTrue(Equal(F.Inverse()*F, Frame.Identity())) - self.assertTrue(Equal(F.Inverse()*vt, F.Inverse(vt))) + self.testFrameVelImpl(f, v, vt) + + # Alternative constructor + rv = RotationVel(fr_m, tw_rot) + vv = VectorVel(fr_p, tw_vel) + f2 = FrameVel(rv, vv) + self.assertEqual(f, f2) + + # Members + self.assertEqual(f.M, rv) + self.assertEqual(f.p, vv) + self.assertEqual(f2.value(), fr) + self.assertEqual(f2.deriv(), tw) + + # Equality + self.assertEqual(FrameVel(f).M, f.M) + self.assertEqual(FrameVel(f).p, f.p) + + f = FrameVel(fr) + self.assertEqual(f, fr) + self.assertEqual(fr, f) + + def testFrameVelImpl(self, f, v, vt): + f2 = FrameVel(f) + self.assertEqual(f, f2) + self.assertEqual(f.Inverse(f*v), v) + self.assertEqual(f.Inverse(f*vt), vt) + self.assertEqual(f*f.Inverse(v), v) + self.assertEqual(f*f.Inverse(vt), vt) + self.assertEqual(f*Frame.Identity(), f) + self.assertEqual(Frame.Identity()*f, f) + self.assertEqual(f*(f*(f*v)), (f*f*f)*v) + self.assertEqual(f*(f*(f*vt)), (f*f*f)*vt) + self.assertEqual(f*f.Inverse(), FrameVel.Identity()) + self.assertEqual(f.Inverse()*f, Frame.Identity()) + self.assertEqual(f.Inverse()*vt, f.Inverse(vt)) def testPickle(self): if sys.version_info < (3, 0): From 5f07a904e4359e55dd4797d61e698a70e9a39eac Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 22:54:22 +0100 Subject: [PATCH 092/136] (PyKDL)(sip) add extra __repr__ of kinfam --- python_orocos_kdl/PyKDL/sip/dynamics.sip | 8 ++++ python_orocos_kdl/PyKDL/sip/kinfam.sip | 61 ++++++++++++++++-------- 2 files changed, 48 insertions(+), 21 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index 31bcf744d..125008796 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -23,6 +23,7 @@ class JntSpaceInertiaMatrix { %TypeHeaderCode #include +#include using namespace KDL; %End public: @@ -55,6 +56,13 @@ public: else { (*sipCpp)(i,j)=a1; } +%End + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; %End //JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg); //double operator()(unsigned int i,unsigned int j)const; diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index 1bc8ecd2a..4d4df1c7d 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -53,20 +53,19 @@ public: JointType getType() const; std::string getTypeName() const; - const std::string* __repr__(); - %MethodCode - std::ostringstream oss; - oss<<(*sipCpp); - std::string s(oss.str()); - sipRes=&s; - %End + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End }; class RotationalInertia { %TypeHeaderCode #include -#include using namespace KDL; %End public: @@ -106,7 +105,6 @@ class RigidBodyInertia { %TypeHeaderCode #include -#include using namespace KDL; %End public: @@ -136,11 +134,11 @@ public: Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); - const std::string* __repr__(); + const std::string* __repr__() const; %MethodCode - std::stringstream ss; - ss<<(*sipCpp); - std::string s(ss.str()); + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); sipRes=&s; %End @@ -158,6 +156,7 @@ class Chain %TypeHeaderCode #include +#include using namespace KDL; %End @@ -171,6 +170,14 @@ public: unsigned int getNrOfJoints()const; unsigned int getNrOfSegments()const; + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + const Segment& getSegment(unsigned int nr)const /Factory/; }; @@ -178,6 +185,7 @@ public: class Tree { %TypeHeaderCode #include +#include using namespace KDL; %End public: @@ -185,6 +193,15 @@ public: bool addSegment(const Segment& segment, const std::string& hook_name); unsigned int getNrOfJoints()const; unsigned int getNrOfSegments()const; + + const std::string* __repr__() const; +%MethodCode + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); + sipRes=&s; +%End + Chain* getChain(const std::string& chain_root, const std::string& chain_tip)const /Factory/; %MethodCode Chain* chain = new Chain(); @@ -197,6 +214,7 @@ class JntArray{ %TypeHeaderCode #include +#include using namespace KDL; %End @@ -229,11 +247,11 @@ public: } %End - const std::string* __repr__(); + const std::string* __repr__() const; %MethodCode - std::stringstream ss; - ss<data; - std::string s(ss.str()); + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); sipRes=&s; %End }; @@ -282,6 +300,7 @@ class Jacobian { %TypeHeaderCode #include +#include using namespace KDL; %End public: @@ -317,11 +336,11 @@ public: } %End - const std::string* __repr__(); + const std::string* __repr__() const; %MethodCode - std::stringstream ss; - ss<data; - std::string s(ss.str()); + std::ostringstream oss; + oss<<(*sipCpp); + std::string s(oss.str()); sipRes=&s; %End Twist getColumn(unsigned int i) const /Factory/; From a1d75a3b199405944599e8b0f62055172bde5318 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 22:56:42 +0100 Subject: [PATCH 093/136] (PyKDL) add extra __repr__ of kinfam --- python_orocos_kdl/PyKDL/pybind11/dynamics.cpp | 10 ++-------- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp index fef190194..ed6fa695a 100644 --- a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "PyKDL.h" namespace py = pybind11; @@ -64,14 +65,7 @@ void init_dynamics(pybind11::module &m) jnt_space_inertia_matrix.def("__repr__", [](const JntSpaceInertiaMatrix &jm) { std::ostringstream oss; - for (size_t r = 0; r < jm.rows(); r++) - { - for (size_t c = 0; c < jm.columns(); c++) - { - oss << std::setw(KDL_FRAME_WIDTH) << jm(r, c); - } - oss << std::endl; - } + oss << jm; return oss.str(); }); jnt_space_inertia_matrix.def(py::self == py::self); diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index 76b9b7da0..f6bcb064a 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -172,6 +172,12 @@ void init_kinfam(pybind11::module &m) chain.def("getNrOfSegments", &Chain::getNrOfSegments); chain.def("getSegment", (Segment& (Chain::*)(unsigned int)) &Chain::getSegment); chain.def("getSegment", (const Segment& (Chain::*)(unsigned int) const) &Chain::getSegment); + chain.def("__repr__", [](const Chain &c) + { + std::ostringstream oss; + oss << c; + return oss.str(); + }); // -------------------- @@ -190,6 +196,12 @@ void init_kinfam(pybind11::module &m) tree.getChain(chain_root, chain_tip, *chain); return chain; }); + tree.def("__repr__", [](const Tree &t) + { + std::ostringstream oss; + oss << t; + return oss.str(); + }); // -------------------- From f23b898333b627862deba4a4e2dfe27be777f66e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 23:57:22 +0100 Subject: [PATCH 094/136] (PyKDL) fix __getitem__ and __setitem__ of Jacobian --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index f6bcb064a..dbc2486a3 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -222,7 +222,7 @@ void init_kinfam(pybind11::module &m) { int i = std::get<0>(idx); int j = std::get<1>(idx); - if (i < 0 || i > 5 || j < 0 || j > jac.columns()) + if (i < 0 || i > 5 || j < 0 || j >= jac.columns()) throw py::index_error("Jacobian index out of range"); return jac((unsigned int)i, (unsigned int)j); }); @@ -230,7 +230,7 @@ void init_kinfam(pybind11::module &m) { int i = std::get<0>(idx); int j = std::get<1>(idx); - if (i < 0 || i > 5 || j < 0 || j > jac.columns()) + if (i < 0 || i > 5 || j < 0 || j >= jac.columns()) throw py::index_error("Jacobian index out of range"); jac((unsigned int)i, (unsigned int)j) = value; From 3cea7a4ec5f5785f50c33088e19c6f8db49a9002 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 28 Mar 2020 23:57:54 +0100 Subject: [PATCH 095/136] (PyKDL)(sip) fix include --- python_orocos_kdl/PyKDL/sip/kinfam.sip | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index 4d4df1c7d..cf5c4d9cd 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -299,7 +299,7 @@ bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); class Jacobian { %TypeHeaderCode -#include +#include #include using namespace KDL; %End From a1ff3496a263b3868468c9afde6503edf48e253f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:20:21 +0100 Subject: [PATCH 096/136] (kdl) initialize jacobian with zeros --- orocos_kdl/src/jacobian.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/orocos_kdl/src/jacobian.cpp b/orocos_kdl/src/jacobian.cpp index 839ad0011..c6a5f0d9a 100644 --- a/orocos_kdl/src/jacobian.cpp +++ b/orocos_kdl/src/jacobian.cpp @@ -33,6 +33,7 @@ namespace KDL Jacobian::Jacobian(unsigned int nr_of_columns): data(6,nr_of_columns) { + data.setZero(); } Jacobian::Jacobian(const Jacobian& arg): From a7744074aca6ef391085c1dd854827d7dd973f25 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:22:53 +0100 Subject: [PATCH 097/136] (PyKDL) add empty constructors to Jacobian and JntArray --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index dbc2486a3..bf21deed0 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -208,6 +208,7 @@ void init_kinfam(pybind11::module &m) // Jacobian // -------------------- py::class_ jacobian(m, "Jacobian"); + jacobian.def(py::init<>()); jacobian.def(py::init()); jacobian.def(py::init()); jacobian.def("rows", &Jacobian::rows); @@ -252,6 +253,7 @@ void init_kinfam(pybind11::module &m) // JntArray // -------------------- py::class_ jnt_array(m, "JntArray"); + jnt_array.def(py::init<>()); jnt_array.def(py::init()); jnt_array.def(py::init()); jnt_array.def("rows", &JntArray::rows); From 751ea67ad4317948da6c6c754c276cff07ec713d Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:23:09 +0100 Subject: [PATCH 098/136] (PyKDL)(sip) add empty constructors to Jacobian and JntArray --- python_orocos_kdl/PyKDL/sip/kinfam.sip | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index cf5c4d9cd..670d3026e 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -219,6 +219,7 @@ using namespace KDL; %End public: + JntArray(); JntArray(unsigned int size); JntArray(const JntArray& arg); unsigned int rows()const; @@ -304,6 +305,7 @@ class Jacobian using namespace KDL; %End public: + Jacobian(); Jacobian(unsigned int size); Jacobian(const Jacobian& arg); unsigned int rows()const; From 37f64d94e583ee27689da83802b2e1a0cb1dabcc Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:23:42 +0100 Subject: [PATCH 099/136] (PyKDL) fix __getitem__ and __setitem__ of JntArray --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index bf21deed0..10032b836 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -261,14 +261,14 @@ void init_kinfam(pybind11::module &m) jnt_array.def("resize", &JntArray::resize); jnt_array.def("__getitem__", [](const JntArray &ja, int i) { - if (i < 0 || i > ja.rows()) + if (i < 0 || i >= ja.rows()) throw py::index_error("JntArray index out of range"); return ja(i); }); jnt_array.def("__setitem__", [](JntArray &ja, int i, double value) { - if (i < 0 || i > ja.rows()) + if (i < 0 || i >= ja.rows()) throw py::index_error("JntArray index out of range"); ja(i) = value; From a7a7bead1a85a1e3ada935f48377e6cc407decc8 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:24:17 +0100 Subject: [PATCH 100/136] (PyKDL) add __getitem__ and __setitem__ tests --- python_orocos_kdl/tests/kinfamtest.py | 79 +++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 5d1664bac..a58ca4514 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -56,6 +56,82 @@ def setUp(self): self.iksolvervel = ChainIkSolverVel_pinv(self.chain) self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel) + def testRotationalInertia(self): + ri = RotationalInertia(1, 2, 3, 4, 7, 5) + # __getitem__ + for i in range(3): + for j in range(3): + self.assertEqual(ri[3*i+j], 2*abs(i-j) + max(i, j) + 1) + with self.assertRaises(IndexError): + _ = ri[-1] + with self.assertRaises(IndexError): + _ = ri[9] + + # __setitem__ + for i in range(3): + for j in range(3): + ri[i] = i + for i in range(3): + for j in range(3): + self.assertEqual(ri[i], i) + with self.assertRaises(IndexError): + ri[-1] = 1 + with self.assertRaises(IndexError): + ri[9] = 1 + + def testJacobian(self): + jac = Jacobian(3) + for i in range(jac.columns()): + jac.setColumn(i, Twist(Vector(6*i+1, 6*i+2, 6*i+3), Vector(6*i+4, 6*i+5, 6*i+6))) + # __getitem__ + for i in range(6): + for j in range(3): + self.assertEqual(jac[i, j], 6*j+i+1) + with self.assertRaises(IndexError): + _ = jac[-1, 0] + with self.assertRaises(IndexError): + _ = jac[6, 0] + with self.assertRaises(IndexError): + _ = jac[5, -1] + with self.assertRaises(IndexError): + _ = jac[5, 3] + + # __setitem__ + for i in range(6): + for j in range(3): + jac[i, j] = 3*i+j + for i in range(6): + for j in range(3): + self.assertEqual(jac[i, j], 3*i+j) + with self.assertRaises(IndexError): + jac[-1, 0] = 1 + with self.assertRaises(IndexError): + jac[6, 0] = 1 + with self.assertRaises(IndexError): + jac[5, -1] = 1 + with self.assertRaises(IndexError): + jac[5, 3] = 1 + + def testJntArray(self): + ja = JntArray(3) + # __getitem__ + for i in range(3): + self.assertEqual(ja[i], 0) + with self.assertRaises(IndexError): + _ = ja[-1] + with self.assertRaises(IndexError): + _ = ja[3] + + # __setitem__ + for i in range(3): + ja[i] = 2*i + for i in range(3): + self.assertEqual(ja[i], 2*i) + with self.assertRaises(IndexError): + ja[-1] = 1 + with self.assertRaises(IndexError): + ja[3] = 1 + def testFkPosAndJac(self): deltaq = 1E-4 epsJ = 1E-4 @@ -171,6 +247,9 @@ def testTreeGetChainMemLeak(self): def suite(): suite = unittest.TestSuite() + suite.addTest(KinfamTestFunctions('testRotationalInertia')) + suite.addTest(KinfamTestFunctions('testJacobian')) + suite.addTest(KinfamTestFunctions('testJntArray')) suite.addTest(KinfamTestFunctions('testFkPosAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndJac')) suite.addTest(KinfamTestFunctions('testFkVelAndIkVel')) From 4b7eced32a6d5836e5b1076e082ba672369f1430 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:33:58 +0100 Subject: [PATCH 101/136] (PyKDL) fix __getitem__ and __setitem__ of JntSpaceInertiaMatrix --- python_orocos_kdl/PyKDL/pybind11/dynamics.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp index ed6fa695a..89b88b197 100644 --- a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp @@ -48,7 +48,7 @@ void init_dynamics(pybind11::module &m) { int i = std::get<0>(idx); int j = std::get<1>(idx); - if (i < 0 || i > jm.rows() || j < 0 || j > jm.columns()) + if (i < 0 || i >= jm.rows() || j < 0 || j >= jm.columns()) throw py::index_error("Inertia index out of range"); return jm((unsigned int)i, (unsigned int)j); @@ -57,7 +57,7 @@ void init_dynamics(pybind11::module &m) { int i = std::get<0>(idx); int j = std::get<1>(idx); - if (i < 0 || i > jm.rows() || j < 0 || j > jm.columns()) + if (i < 0 || i >= jm.rows() || j < 0 || j >= jm.columns()) throw py::index_error("Inertia index out of range"); jm((unsigned int)i, (unsigned int)j) = value; From eca64f7ed492dd3f56eb586b3036c73518cacb38 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:34:13 +0100 Subject: [PATCH 102/136] (PyKDL)(sip) fix __getitem__ and __setitem__ of JntSpaceInertiaMatrix --- python_orocos_kdl/PyKDL/sip/dynamics.sip | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index 125008796..0b3302f94 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -37,7 +37,7 @@ public: %MethodCode int i,j; PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { + if (i < 0 || j < 0 || i >= (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Inertia index out of range"); sipError = sipErrorFail; } @@ -49,7 +49,7 @@ public: %MethodCode int i,j; PyArg_ParseTuple(a0,"ii",&i,&j); - if (i < 0 || j < 0 || i > (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { + if (i < 0 || j < 0 || i >= (int)sipCpp->rows() || j >= (int)sipCpp->columns()) { PyErr_SetString(PyExc_IndexError, "Inertia index out of range"); sipError = sipErrorFail; } From 130c049c9461972f857741924bac38c6ee5c813c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 00:34:43 +0100 Subject: [PATCH 103/136] (PyKDL) add dynamics tests --- python_orocos_kdl/tests/PyKDLtest.py | 2 + python_orocos_kdl/tests/dynamicstest.py | 74 +++++++++++++++++++++++++ 2 files changed, 76 insertions(+) create mode 100644 python_orocos_kdl/tests/dynamicstest.py diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index bd555b2eb..603a34872 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -22,6 +22,7 @@ import unittest +import dynamicstest import kinfamtest import framestest import frameveltest @@ -29,6 +30,7 @@ import sys suite = unittest.TestSuite() +suite.addTest(dynamicstest.suite()) suite.addTest(framestest.suite()) suite.addTest(frameveltest.suite()) suite.addTest(kinfamtest.suite()) diff --git a/python_orocos_kdl/tests/dynamicstest.py b/python_orocos_kdl/tests/dynamicstest.py new file mode 100644 index 000000000..3f5d73e22 --- /dev/null +++ b/python_orocos_kdl/tests/dynamicstest.py @@ -0,0 +1,74 @@ +# Copyright (C) 2007 Ruben Smits + +# Version: 1.0 +# Author: Ruben Smits +# Maintainer: Ruben Smits +# URL: http://www.orocos.org/kdl + +# This library 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 2.1 of the License, or (at your option) any later version. + +# This library 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 +# Lesser General Public License for more details. + +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +from PyKDL import * +import unittest + + +class DynamicsTestFunctions(unittest.TestCase): + def testJntSpaceInertiaMatrix(self): + ll = 3 + jm = JntSpaceInertiaMatrix(3) + # __getitem__ + for i in range(ll): + for j in range(ll): + self.assertEqual(jm[i, j], 0) + with self.assertRaises(IndexError): + _ = jm[-1, 0] + with self.assertRaises(IndexError): + _ = jm[3, 0] + with self.assertRaises(IndexError): + _ = jm[2, -1] + with self.assertRaises(IndexError): + _ = jm[2, 3] + + # __setitem__ + for i in range(ll): + for j in range(ll): + jm[i, j] = 3 * i + j + for i in range(ll): + for j in range(ll): + self.assertEqual(jm[i, j], 3 * i + j) + with self.assertRaises(IndexError): + jm[-1, 0] = 1 + with self.assertRaises(IndexError): + jm[3, 0] = 1 + with self.assertRaises(IndexError): + jm[2, -1] = 1 + with self.assertRaises(IndexError): + jm[2, 3] = 1 + + +def suite(): + suite = unittest.TestSuite() + suite.addTest(DynamicsTestFunctions('testJntSpaceInertiaMatrix')) + return suite + + +if __name__ == '__main__': + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) \ No newline at end of file From 733dc66270cf5c981592606bba7aaea68ddfeb05 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 11:32:10 +0200 Subject: [PATCH 104/136] (PyKDL) remove functions using containers as arg by ref Requires more work to get it to work nicely in PyBind11 --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index 10032b836..3d571260e 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -342,8 +342,9 @@ void init_kinfam(pybind11::module &m) py::class_ chain_fk_solver_vel(m, "ChainFkSolverVel"); chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, FrameVel&, int)) &ChainFkSolverVel::JntToCart, py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); - chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector&, int)) &ChainFkSolverVel::JntToCart, - py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); +// Argument by reference doesn't work for container types +// chain_fk_solver_vel.def("JntToCart", (int (ChainFkSolverVel::*)(const JntArrayVel&, std::vector&, int)) &ChainFkSolverVel::JntToCart, +// py::arg("q_in"), py::arg("p_out"), py::arg("segmentNr")=-1); // ------------------------------ @@ -374,8 +375,9 @@ void init_kinfam(pybind11::module &m) py::class_ chain_ik_solver_vel(m, "ChainIkSolverVel"); chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const Twist&, JntArray&)) &ChainIkSolverVel::CartToJnt, py::arg("q_in"), py::arg("v_in"), py::arg("qdot_out")); - chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt, - py::arg("q_init"), py::arg("v_in"), py::arg("q_out")); +// Argument by reference doesn't work for container types +// chain_ik_solver_vel.def("CartToJnt", (int (ChainIkSolverVel::*)(const JntArray&, const FrameVel&, JntArrayVel&)) &ChainIkSolverVel::CartToJnt, +// py::arg("q_init"), py::arg("v_in"), py::arg("q_out")); // ---------------------- From f2206526eb19759fd0cae89af17cce17058647dd Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 29 Mar 2020 11:32:54 +0200 Subject: [PATCH 105/136] (PyKDL)(sip) remove functions using containers as arg by ref I don't know SIP supports this, doesn't work right now --- python_orocos_kdl/PyKDL/sip/kinfam.sip | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index 670d3026e..bb92a6cdd 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -377,7 +377,8 @@ class ChainFkSolverPos : SolverI using namespace KDL; %End virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1)=0; - virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1)=0; +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1)=0; }; class ChainFkSolverVel : SolverI @@ -387,7 +388,8 @@ class ChainFkSolverVel : SolverI using namespace KDL; %End virtual int JntToCart(const JntArrayVel& q_in, FrameVel& p_out, int segmentNr=-1)=0; - virtual int JntToCart(const JntArrayVel& q_in, std::vector& p_out, int segmentNr=-1)=0; +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArrayVel& q_in, std::vector& p_out, int segmentNr=-1)=0; }; class ChainFkSolverPos_recursive : ChainFkSolverPos @@ -400,7 +402,8 @@ using namespace KDL; public: ChainFkSolverPos_recursive(const Chain& chain); virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1); - virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1); +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1); virtual void updateInternalDataStructures(); }; @@ -413,7 +416,8 @@ using namespace KDL; public: ChainFkSolverVel_recursive(const Chain& chain); virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out, int segmentNr=-1); - virtual int JntToCart(const JntArrayVel& q_in, std::vector& out, int segmentNr=-1 ); +// Argument by reference doesn't work for container types +// virtual int JntToCart(const JntArrayVel& q_in, std::vector& out, int segmentNr=-1 ); virtual void updateInternalDataStructures(); }; From 6bbea2e0c837dfcded7caaec0a61f09b23c8ae13 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 1 Apr 2020 09:40:53 +0200 Subject: [PATCH 106/136] (travis) SIP is fixed, so remove allow_failures --- .travis.yml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index b4db1b143..f94a923ba 100644 --- a/.travis.yml +++ b/.travis.yml @@ -12,12 +12,6 @@ env: - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF - OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=ON -jobs: - fast_finish: true - allow_failures: - - env: OROCOS_KDL_BUILD_TYPE=Debug BUILD_PYKDL_PYBIND11=OFF - - env: OROCOS_KDL_BUILD_TYPE=Release BUILD_PYKDL_PYBIND11=OFF - before_script: - sudo apt-get -qq update - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python-psutil From 1540386dc52e2b7cb4a4e00c52b4665338ddfd63 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 16 Apr 2020 18:34:05 +0200 Subject: [PATCH 107/136] (travis) Also test python 3 --- .travis.yml | 38 ++++++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/.travis.yml b/.travis.yml index f94a923ba..0e9660b5a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,8 +14,8 @@ env: before_script: - sudo apt-get -qq update - - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python-psutil - #build orocos_kdl + - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python3-sip-dev python-psutil python3-psutil + # build orocos_kdl - cd orocos_kdl - mkdir build - cd build @@ -24,12 +24,27 @@ before_script: - make - sudo make install - cd ../.. - #build python bindings + + # build python bindings python 2 - cd python_orocos_kdl - - mkdir build - - cd build + - mkdir build2 + - cd build2 + - export ROS_PYTHON_VERSION=2 + - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./.. + # compile and install python bindings python 2 + - make + - sudo make install + - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib + - sudo ldconfig + - cd ../.. + + # build python bindings python 3 + - cd python_orocos_kdl + - mkdir build3 + - cd build3 + - export ROS_PYTHON_VERSION=3 - cmake -DCMAKE_CXX_FLAGS:STRING="-Wall" -DCMAKE_BUILD_TYPE=${OROCOS_KDL_BUILD_TYPE} -DBUILD_PYKDL_PYBIND11=${BUILD_PYKDL_PYBIND11} ./.. - #compile and install python bindings + # compile and install python bindings python 3 - make - sudo make install - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib @@ -37,10 +52,13 @@ before_script: - cd ../.. script: - #execute orocos_kdl tests + # execute orocos_kdl tests - cd orocos_kdl/build - make check - cd ../.. - #execute python bindings tests - - cd python_orocos_kdl/build - - python ../tests/PyKDLtest.py + # execute python bindings tests + - cd python_orocos_kdl + # python 2 + - python2 tests/PyKDLtest.py + # python 3 + - python3 tests/PyKDLtest.py From f4edb65964c4f2b1438929a7522cfbfc1b0af2f9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 16 Apr 2020 22:24:16 +0200 Subject: [PATCH 108/136] set PYTHON_MODULE_EXTENSION to '.so' --- python_orocos_kdl/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index 2f9b5926d..ae4c06ece 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -23,6 +23,7 @@ set(LIBRARY_NAME "PyKDL") option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" ON) if (BUILD_PYKDL_PYBIND11) + SET(PYTHON_MODULE_EXTENSION ".so") add_subdirectory(pybind11) pybind11_add_module(${LIBRARY_NAME} PyKDL/pybind11/PyKDL.h From 4634955afc93871d32e55bb9903f160bf5a46847 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 17 Apr 2020 20:29:13 +0200 Subject: [PATCH 109/136] (PyKDL) change install path --- python_orocos_kdl/CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index ae4c06ece..edf0e0006 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -17,8 +17,9 @@ endif() find_package(PythonInterp ${PYTHON_VERSION} REQUIRED) find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED) -execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) -set(PYTHON_SITE_PACKAGES_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITE_PACKAGES}) +# get_python_lib in python3 produces path which isn't in sys.path: https://bugs.launchpad.net/ubuntu/+source/python3-stdlib-extensions/+bug/1832215 +# execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(plat_specific=True, prefix=''))" OUTPUT_VARIABLE PYTHON_SITE_PACKAGES OUTPUT_STRIP_TRAILING_WHITESPACE) +set(PYTHON_SITE_PACKAGES_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/dist-packages") set(LIBRARY_NAME "PyKDL") option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" ON) From dce6742a379698bc6d50afedc8c0a4fc4657d493 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 17 Apr 2020 21:26:05 +0200 Subject: [PATCH 110/136] (kdl) Replace None by Fixed, None points towards Fixed --- orocos_kdl/src/chain.cpp | 2 +- orocos_kdl/src/chaindynparam.cpp | 6 +++--- orocos_kdl/src/chainfksolverpos_recursive.cpp | 6 +++--- orocos_kdl/src/chainfksolvervel_recursive.cpp | 6 +++--- .../src/chainidsolver_recursive_newton_euler.cpp | 4 ++-- orocos_kdl/src/chainidsolver_vereshchagin.cpp | 6 +++--- orocos_kdl/src/chainiksolverpos_lma.cpp | 4 ++-- orocos_kdl/src/chainjnttojacdotsolver.cpp | 2 +- orocos_kdl/src/chainjnttojacsolver.cpp | 4 ++-- orocos_kdl/src/joint.cpp | 6 +++--- orocos_kdl/src/joint.hpp | 16 ++++++++-------- orocos_kdl/src/segment.hpp | 8 ++++---- orocos_kdl/src/tree.cpp | 4 ++-- .../src/treeidsolver_recursive_newton_euler.cpp | 4 ++-- orocos_kdl/src/treejnttojacsolver.cpp | 2 +- 15 files changed, 40 insertions(+), 40 deletions(-) diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 2b6320b27..2fba4582a 100644 --- a/orocos_kdl/src/chain.cpp +++ b/orocos_kdl/src/chain.cpp @@ -55,7 +55,7 @@ namespace KDL { { segments.push_back(segment); nrOfSegments++; - if(segment.getJoint().getType()!=Joint::None) + if(segment.getJoint().getType()!=Joint::Fixed) nrOfJoints++; } diff --git a/orocos_kdl/src/chaindynparam.cpp b/orocos_kdl/src/chaindynparam.cpp index 61ed64227..980481412 100644 --- a/orocos_kdl/src/chaindynparam.cpp +++ b/orocos_kdl/src/chaindynparam.cpp @@ -71,7 +71,7 @@ namespace KDL { { //Collect RigidBodyInertia Ic[i]=chain.getSegment(i).getInertia(); - if(chain.getSegment(i).getJoint().getType()!=Joint::None) + if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { q_=q(k); k++; @@ -96,7 +96,7 @@ namespace KDL { } F=Ic[i]*S[i]; - if(chain.getSegment(i).getJoint().getType()!=Joint::None) + if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { H(k,k)=dot(S[i],F); H(k,k)+=chain.getSegment(i).getJoint().getInertia(); // add joint inertia @@ -108,7 +108,7 @@ namespace KDL { F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] l--; //go down a segment - if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint + if(chain.getSegment(l).getJoint().getType()!=Joint::Fixed) //if the joint connected to segment is not a fixed joint { j--; H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment diff --git a/orocos_kdl/src/chainfksolverpos_recursive.cpp b/orocos_kdl/src/chainfksolverpos_recursive.cpp index 99d7d4793..051a3afc7 100644 --- a/orocos_kdl/src/chainfksolverpos_recursive.cpp +++ b/orocos_kdl/src/chainfksolverpos_recursive.cpp @@ -46,7 +46,7 @@ namespace KDL { else{ int j=0; for(unsigned int i=0;i=0;i--){ - if(chain.getSegment(i).getJoint().getType()!=Joint::None){ + if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) { torques(j)=dot(S[i],f[i]); torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia --j; diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.cpp b/orocos_kdl/src/chainidsolver_vereshchagin.cpp index 880503a53..ddc86c535 100644 --- a/orocos_kdl/src/chainidsolver_vereshchagin.cpp +++ b/orocos_kdl/src/chainidsolver_vereshchagin.cpp @@ -124,7 +124,7 @@ void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const //external forces are taken into account through s.U. Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; - if (segment.getJoint().getType() != Joint::None) + if (segment.getJoint().getType() != Joint::Fixed) j++; } @@ -240,7 +240,7 @@ void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const Jnt vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data); s.EZ.noalias() = s.E.transpose() * vZ; - if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed) j--; } } @@ -325,7 +325,7 @@ void ChainIdSolver_Vereshchagin::final_upwards_sweep(JntArray &q_dotdot, JntArra // nullspace forces. q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed - if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed) j++; } } diff --git a/orocos_kdl/src/chainiksolverpos_lma.cpp b/orocos_kdl/src/chainiksolverpos_lma.cpp index ccfe8c907..dd17c345e 100644 --- a/orocos_kdl/src/chainiksolverpos_lma.cpp +++ b/orocos_kdl/src/chainiksolverpos_lma.cpp @@ -144,7 +144,7 @@ void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) { T_base_head = Frame::Identity(); // frame w.r.t. base of head for (unsigned int i=0;i retval; //insert new element - unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0; + unsigned int q_nr = segment.getJoint().getType() != Joint::Fixed ? nrOfJoints : 0; #ifdef KDL_USE_NEW_TREE_INTERFACE retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr)))); @@ -75,7 +75,7 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) { //increase number of segments nrOfSegments++; //increase number of joints - if (segment.getJoint().getType() != Joint::None) + if (segment.getJoint().getType() != Joint::Fixed) nrOfJoints++; return true; } diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp index b4757f6dd..dfc446e3a 100644 --- a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp @@ -83,7 +83,7 @@ namespace KDL{ //Do forward calculations involving velocity & acceleration of this segment double q_, qdot_, qdotdot_; unsigned int j = GetTreeElementQNr(segment->second); - if(seg.getJoint().getType()!=Joint::None){ + if(seg.getJoint().getType()!=Joint::Fixed) { q_ = q(j); qdot_ = q_dot(j); qdotdot_ = q_dotdot(j); @@ -127,7 +127,7 @@ namespace KDL{ //do backward calculations involving wrenches and joint efforts //If there is a moving joint, evaluate its effort - if(seg.getJoint().getType()!=Joint::None){ + if(seg.getJoint().getType()!=Joint::Fixed) { torques(j) = dot(S.at(segname), f.at(segname)); torques(j) += seg.getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia } diff --git a/orocos_kdl/src/treejnttojacsolver.cpp b/orocos_kdl/src/treejnttojacsolver.cpp index 9b5ff856a..0afb4d1c5 100644 --- a/orocos_kdl/src/treejnttojacsolver.cpp +++ b/orocos_kdl/src/treejnttojacsolver.cpp @@ -47,7 +47,7 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std: T_total = T_local * T_total; //get the twist of the segment: - if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) { + if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::Fixed) { Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0); //transform the endpoint of the local twist to the global endpoint: t_local = t_local.RefPoint(T_total.p - T_local.p); From acd62266abb27cfbfd4897800e3b4cfe7ec5ecef Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 17 Apr 2020 23:18:19 +0200 Subject: [PATCH 111/136] (PyKDL)(SIP) vector python3 compatible --- python_orocos_kdl/PyKDL/sip/std_vector.sip | 24 ++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/std_vector.sip b/python_orocos_kdl/PyKDL/sip/std_vector.sip index 7563d2b91..c1545f3bc 100644 --- a/python_orocos_kdl/PyKDL/sip/std_vector.sip +++ b/python_orocos_kdl/PyKDL/sip/std_vector.sip @@ -181,6 +181,12 @@ template { %TypeHeaderCode #include + +#if PY_VERSION_HEX < 0x03000000 +#define PyLong_AsLong PyInt_AsLong +#define PyLong_FromLong PyInt_FromLong +#define PyLong_Check PyInt_Check +#endif %End %ConvertFromTypeCode @@ -195,7 +201,7 @@ template for (int i = 0; i < (int)sipCpp -> size(); ++i) { // Add the wrapper to the list. - PyList_SET_ITEM(l, i, PyInt_FromLong(sipCpp -> at(i))); + PyList_SET_ITEM(l, i, PyLong_FromLong(sipCpp -> at(i))); } // Return the Python list. @@ -226,14 +232,14 @@ template { while ((item = PyIter_Next(iterator))) { - if (!PyInt_Check(item)) + if (!PyLong_Check(item)) { PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float"); *sipIsErr = 1; break; } - int val = PyInt_AsLong(item); + int val = PyLong_AsLong(item); V->push_back(val); Py_DECREF(item); @@ -262,6 +268,12 @@ template { %TypeHeaderCode #include + +#if PY_VERSION_HEX < 0x03000000 +#define PyLong_AsLong PyInt_AsLong +#define PyLong_FromLong PyInt_FromLong +#define PyLong_Check PyInt_Check +#endif %End %ConvertFromTypeCode @@ -276,7 +288,7 @@ template for (int i = 0; i < (int)sipCpp -> size(); ++i) { // Add the wrapper to the list. - PyList_SET_ITEM(l, i, PyInt_FromLong(sipCpp -> at(i))); + PyList_SET_ITEM(l, i, PyLong_FromLong(sipCpp -> at(i))); } // Return the Python list. @@ -313,14 +325,14 @@ template { while ((item = PyIter_Next(iterator))) { - if (!PyInt_Check(item)) + if (!PyLong_Check(item)) { PyErr_Format(PyExc_TypeError, "object in iterable cannot be converted to float"); *sipIsErr = 1; break; } - unsigned int val = PyInt_AsLong(item); + unsigned int val = PyLong_AsLong(item); V->push_back(val); Py_DECREF(item); From a06eb73985b2b70524f0f801d548f16b1651b779 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 17 Apr 2020 23:31:32 +0200 Subject: [PATCH 112/136] (PyKDL)(SIP) string python3 compatible --- python_orocos_kdl/PyKDL/sip/std_string.sip | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/std_string.sip b/python_orocos_kdl/PyKDL/sip/std_string.sip index d91bda972..33b24e0ba 100644 --- a/python_orocos_kdl/PyKDL/sip/std_string.sip +++ b/python_orocos_kdl/PyKDL/sip/std_string.sip @@ -15,6 +15,13 @@ //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +%ModuleHeaderCode +#if PY_VERSION_HEX < 0x03000000 +#define PyBytes_AS_STRING PyString_AS_STRING +#define PyBytes_FromString PyString_FromString +#define PyBytes_Check PyString_Check +#endif +%End %MappedType std::string { %TypeHeaderCode @@ -39,7 +46,7 @@ // If argument is a Python string, assume it's UTF-8 if (sipIsErr == NULL) #if PY_MAJOR_VERSION < 3 - return (PyString_Check(sipPy) || PyUnicode_Check(sipPy)); + return (PyBytes_Check(sipPy) || PyUnicode_Check(sipPy)); #else return PyUnicode_Check(sipPy); #endif @@ -50,12 +57,12 @@ #if PY_MAJOR_VERSION < 3 if (PyUnicode_Check(sipPy)) { PyObject* s = PyUnicode_AsUTF8String(sipPy); - *sipCppPtr = new std::string(PyString_AS_STRING(s)); + *sipCppPtr = new std::string(PyBytes_AS_STRING(s)); Py_DECREF(s); return 1; } - else if (PyString_Check(sipPy)) { - *sipCppPtr = new std::string(PyString_AS_STRING(sipPy)); + else if (PyBytes_Check(sipPy)) { + *sipCppPtr = new std::string(PyBytes_AS_STRING(sipPy)); return 1; } #else From 2d1ef03a4e2888b3a11da626efd0ed1f779e6a88 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 00:16:02 +0200 Subject: [PATCH 113/136] (PyKDL)(sip) no None in python3 --- python_orocos_kdl/CMakeLists.txt | 6 +++++- python_orocos_kdl/PyKDL/sip/kinfam.sip | 17 ++++++++++++----- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index edf0e0006..c67c98533 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -44,7 +44,11 @@ else (BUILD_PYKDL_PYBIND11) file(GLOB SIP_FILES "${CMAKE_CURRENT_SOURCE_DIR}/${LIBRARY_NAME}/sip/*.sip") set(SIP_INCLUDES ${SIP_FILES}) - set(SIP_EXTRA_OPTIONS "-o") + if (${PYTHON_VERSION_MAJOR} GREATER_EQUAL "3") + set(SIP_EXTRA_OPTIONS "-o;-x;PYTHON2") # '-x' disables the feature + else() + set(SIP_EXTRA_OPTIONS "-o") + endif() add_sip_python_module(${LIBRARY_NAME} ${LIBRARY_NAME}/sip/${LIBRARY_NAME}.sip ${orocos_kdl_LIBRARIES}) endif(BUILD_PYKDL_PYBIND11) diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index bb92a6cdd..e2b02f23c 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -20,6 +20,8 @@ //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +%Feature PYTHON2 + class Joint{ %TypeHeaderCode @@ -30,10 +32,15 @@ using namespace KDL; public: - enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,None}; - Joint(std::string name, JointType type=None,double scale=1,double offset=0, +%If (!PYTHON2) + enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed}; +%End +%If (PYTHON2) + enum JointType {RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed,None}; +%End + Joint(std::string name, JointType type=Fixed,double scale=1,double offset=0, double inertia=0,double damping=0,double stiffness=0); - Joint(JointType type=None,double scale=1,double offset=0, + Joint(JointType type=Fixed,double scale=1,double offset=0, double inertia=0,const double damping=0,double stiffness=0); Joint(std::string name, Vector origin, Vector axis, JointType type, double scale=1, double offset=0, double inertia=0, double damping=0, double stiffness=0); @@ -130,8 +137,8 @@ class Segment using namespace KDL; %End public: - Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); - Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + Segment(const std::string& name, const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + Segment(const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); const std::string* __repr__() const; From d518c8b86e2be19fc1389fda91c9f566c21c0b8c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 00:30:49 +0200 Subject: [PATCH 114/136] (PyKDL) remove None from Joint --- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index 3d571260e..560ece1d5 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -62,15 +62,18 @@ void init_kinfam(pybind11::module &m) joint_type.value("TransX", Joint::JointType::TransX); joint_type.value("TransY", Joint::JointType::TransY); joint_type.value("TransZ", Joint::JointType::TransZ); + joint_type.value("Fixed", Joint::JointType::Fixed); +#if PY_VERSION_HEX < 0x03000000 joint_type.value("None", Joint::JointType::None); +#endif joint_type.export_values(); joint.def(py::init<>()); joint.def(py::init(), - py::arg("name"), py::arg("type")=Joint::JointType::None, py::arg("scale")=1, py::arg("offset")=0, + py::arg("name"), py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0, py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); joint.def(py::init(), - py::arg("type")=Joint::JointType::None, py::arg("scale")=1, py::arg("offset")=0, + py::arg("type")=Joint::JointType::Fixed, py::arg("scale")=1, py::arg("offset")=0, py::arg("inertia")=0, py::arg("damping")=0, py::arg("stiffness")=0); joint.def(py::init(), py::arg("name"), py::arg("origin"), py::arg("axis"), py::arg("type"), py::arg("scale")=1, py::arg("offset")=0, From f2591a078b7af3e9f07d814179dbd04be509a005 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 09:02:10 +0200 Subject: [PATCH 115/136] (PyKDL) Fix None in tests --- python_orocos_kdl/tests/PyKDLtest.py | 4 ++ python_orocos_kdl/tests/jointtypetest.py | 48 ++++++++++++++++++++++++ python_orocos_kdl/tests/kinfamtest.py | 8 ++-- 3 files changed, 56 insertions(+), 4 deletions(-) create mode 100644 python_orocos_kdl/tests/jointtypetest.py diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 603a34872..4c3b4cbac 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -35,6 +35,10 @@ suite.addTest(frameveltest.suite()) suite.addTest(kinfamtest.suite()) +if sys.version_info < (3, 0): + import jointtypetest + suite.addTest(jointtypetest.suite()) + result = unittest.TextTestRunner(verbosity=3).run(suite) if result.wasSuccessful(): diff --git a/python_orocos_kdl/tests/jointtypetest.py b/python_orocos_kdl/tests/jointtypetest.py new file mode 100644 index 000000000..7081b962e --- /dev/null +++ b/python_orocos_kdl/tests/jointtypetest.py @@ -0,0 +1,48 @@ +# Copyright (C) 2020 Ruben Smits + +# Version: 1.0 +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits +# URL: http://www.orocos.org/kdl + +# This library 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 2.1 of the License, or (at your option) any later version. + +# This library 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 +# Lesser General Public License for more details. + +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +from PyKDL import Joint +import unittest + + +class JointTypeNoneTest(unittest.TestCase): + def testJointType(self): + self.assertEqual(Joint.Fixed, Joint.None) + self.assertEqual(str(Joint.Fixed), str(Joint.None)) + self.assertEqual(int(Joint.Fixed), int(Joint.None)) + + +def suite(): + suite = unittest.TestSuite() + suite.addTest(JointTypeNoneTest('testJointType')) + return suite + + +if __name__ == '__main__': + import sys + suite = suite() + result = unittest.TextTestRunner(verbosity=3).run(suite) + + if result.wasSuccessful(): + sys.exit(0) + else: + sys.exit(1) diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index a58ca4514..faa2ce316 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -35,11 +35,11 @@ def setUp(self): Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotX), Frame(Vector(0.0, 0.0, 0.9)))) - self.chain.addSegment(Segment(Joint(Joint.None), + self.chain.addSegment(Segment(Joint(Joint.Fixed), Frame(Vector(-0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.RotY), Frame(Vector(0.0, 0.0, 1.2)))) - self.chain.addSegment(Segment(Joint(Joint.None), + self.chain.addSegment(Segment(Joint(Joint.Fixed), Frame(Vector(0.4, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransZ), Frame(Vector(0.0, 0.0, 1.4)))) @@ -47,7 +47,7 @@ def setUp(self): Frame(Vector(0.0, 0.0, 0.0)))) self.chain.addSegment(Segment(Joint(Joint.TransY), Frame(Vector(0.0, 0.0, 0.4)))) - self.chain.addSegment(Segment(Joint(Joint.None), + self.chain.addSegment(Segment(Joint(Joint.Fixed), Frame(Vector(0.0, 0.0, 0.0)))) self.jacsolver = ChainJntToJacSolver(self.chain) @@ -227,7 +227,7 @@ def setUp(self): self.tree = Tree() self.tree.addSegment(Segment(Joint(Joint.RotZ), Frame(Vector(0.0, 0.0, 0.0))), "foo") - self.tree.addSegment(Segment(Joint(Joint.None), + self.tree.addSegment(Segment(Joint(Joint.Fixed), Frame(Vector(0.0, 0.0, 0.0))), "bar") def testTreeGetChainMemLeak(self): From e8ecba1fe9e87f1a062ad63f084b75ca5b3805f2 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 10:04:29 +0200 Subject: [PATCH 116/136] (PyKDL) fix author info --- python_orocos_kdl/tests/dynamicstest.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python_orocos_kdl/tests/dynamicstest.py b/python_orocos_kdl/tests/dynamicstest.py index 3f5d73e22..324585e1e 100644 --- a/python_orocos_kdl/tests/dynamicstest.py +++ b/python_orocos_kdl/tests/dynamicstest.py @@ -1,8 +1,8 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Matthijs van der Burgh +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or From afe780d98a526fee87b4df8bec4199babedbd440 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 10:05:51 +0200 Subject: [PATCH 117/136] (PyKDL) add missing import in test --- python_orocos_kdl/tests/dynamicstest.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python_orocos_kdl/tests/dynamicstest.py b/python_orocos_kdl/tests/dynamicstest.py index 324585e1e..d9b889bb7 100644 --- a/python_orocos_kdl/tests/dynamicstest.py +++ b/python_orocos_kdl/tests/dynamicstest.py @@ -65,6 +65,7 @@ def suite(): if __name__ == '__main__': + import sys suite = suite() result = unittest.TextTestRunner(verbosity=3).run(suite) From 0c8b922cca67acf807aafe6741a8493c7a5dc6e8 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 10:08:52 +0200 Subject: [PATCH 118/136] (PyKDL) fix shebang in test --- python_orocos_kdl/tests/PyKDLtest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 4c3b4cbac..7816780b5 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#! /usr/bin/env python # Copyright (C) 2007 Ruben Smits # Version: 1.0 From 96479735ae2fc244a6a9f84ce47f5baf8cc7be87 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 10:11:43 +0200 Subject: [PATCH 119/136] (PyKDL) fix range in test --- .travis.yml | 2 +- python_orocos_kdl/tests/kinfamtest.py | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0e9660b5a..dad4a432c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,7 +14,7 @@ env: before_script: - sudo apt-get -qq update - - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python3-sip-dev python-psutil python3-psutil + - sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python3-sip-dev python-psutil python3-psutil python-future python3-future # build orocos_kdl - cd orocos_kdl - mkdir build diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index faa2ce316..f1d51ce5d 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -20,6 +20,8 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +from builtins import range + import gc import psutil from PyKDL import * @@ -238,7 +240,7 @@ def testTreeGetChainMemLeak(self): mem_before = process.memory_info().vms # needs at least 2000 iterations on my system to cause a detectable # difference in memory usage - for _ in xrange(10000): + for _ in range(10000): self.tree.getChain("foo", "bar") gc.collect() mem_after = process.memory_info().vms From 8c87e96b4024dde39d8ecddc646566d93365a8d9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 10:15:25 +0200 Subject: [PATCH 120/136] (PyKDL) read/write pickle binary --- python_orocos_kdl/tests/framestest.py | 4 ++-- python_orocos_kdl/tests/frameveltest.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index add19d0a1..f9b84025b 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -405,10 +405,10 @@ def testPickle(self): data['tw'] = Twist(data['v'], Vector(4, 5, 6)) data['wr'] = Wrench(Vector(0.1, 0.2, 0.3), data['v']) - with open('/tmp/pickle_test', 'w') as f: + with open('/tmp/pickle_test', 'wb') as f: pickle.dump(data, f, 2) - with open('/tmp/pickle_test', 'r') as f: + with open('/tmp/pickle_test', 'rb') as f: data1 = pickle.load(f) self.assertEqual(data, data1) diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index ab2e5bab8..f7293e844 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -255,10 +255,10 @@ def testPickle(self): data['fv'] = FrameVel(data['rv'], data['vv']) data['tv'] = TwistVel(data['vv'], data['vv']) - with open('/tmp/pickle_test_kdl_framevel', 'w') as f: + with open('/tmp/pickle_test_kdl_framevel', 'wb') as f: pickle.dump(data, f, 2) - with open('/tmp/pickle_test_kdl_framevel', 'r') as f: + with open('/tmp/pickle_test_kdl_framevel', 'rb') as f: data1 = pickle.load(f) self.assertEqual(data, data1) From 6984454403b6cbaff1c15b212c40755cd1906379 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 10:30:33 +0200 Subject: [PATCH 121/136] (PyKDL) tests, update author info --- python_orocos_kdl/tests/PyKDLtest.py | 6 +++--- python_orocos_kdl/tests/framestest.py | 6 +++--- python_orocos_kdl/tests/frameveltest.py | 6 +++--- python_orocos_kdl/tests/kinfamtest.py | 6 +++--- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/python_orocos_kdl/tests/PyKDLtest.py b/python_orocos_kdl/tests/PyKDLtest.py index 7816780b5..94addc3e7 100644 --- a/python_orocos_kdl/tests/PyKDLtest.py +++ b/python_orocos_kdl/tests/PyKDLtest.py @@ -1,9 +1,9 @@ #! /usr/bin/env python -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index f9b84025b..41b73fdb7 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -1,8 +1,8 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index f7293e844..2471340f1 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -1,8 +1,8 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index f1d51ce5d..093cde87e 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -1,8 +1,8 @@ -# Copyright (C) 2007 Ruben Smits +# Copyright (C) 2020 Ruben Smits # Version: 1.0 -# Author: Ruben Smits -# Maintainer: Ruben Smits +# Author: Ruben Smits +# Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl # This library is free software; you can redistribute it and/or From 61f457a8a759c8e1f8a491dbbc9ca97eebb886bf Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 10:32:20 +0200 Subject: [PATCH 122/136] (PyKDL) update author info --- python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp | 7 ++++--- python_orocos_kdl/PyKDL/pybind11/PyKDL.h | 7 ++++--- python_orocos_kdl/PyKDL/pybind11/dynamics.cpp | 7 ++++--- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 7 ++++--- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 7 ++++--- python_orocos_kdl/PyKDL/pybind11/kinfam.cpp | 7 ++++--- 6 files changed, 24 insertions(+), 18 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp index b9c8f508b..113981910 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.cpp @@ -1,9 +1,10 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits +//Author: Ruben Smits Ruben Smits //Author: Zihan Chen -//Maintainer: Ruben Smits +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h index 78bc0d889..17256935d 100644 --- a/python_orocos_kdl/PyKDL/pybind11/PyKDL.h +++ b/python_orocos_kdl/PyKDL/pybind11/PyKDL.h @@ -1,9 +1,10 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits +//Author: Ruben Smits Ruben Smits //Author: Zihan Chen -//Maintainer: Ruben Smits +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp index 89b88b197..b63b08763 100644 --- a/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/dynamics.cpp @@ -1,9 +1,10 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits +//Author: Ruben Smits Ruben Smits //Author: Zihan Chen -//Maintainer: Ruben Smits +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index 848da49f0..ee4b11ee9 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -1,9 +1,10 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits +//Author: Ruben Smits Ruben Smits //Author: Zihan Chen -//Maintainer: Ruben Smits +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index 9ea711aef..c166d47d8 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -1,9 +1,10 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits +//Author: Ruben Smits Ruben Smits //Author: Zihan Chen -//Maintainer: Ruben Smits +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp index 560ece1d5..8eaee9c5a 100644 --- a/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/kinfam.cpp @@ -1,9 +1,10 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits +//Author: Ruben Smits Ruben Smits //Author: Zihan Chen -//Maintainer: Ruben Smits +//Author: Matthijs van der Burgh +//Maintainer: Ruben Smits Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or From 8c2d6bb055e20360d4f8074d1407855ef306da5c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 11:36:07 +0200 Subject: [PATCH 123/136] Update Author info --- orocos_kdl/src/chain.cpp | 6 ++--- orocos_kdl/src/chaindynparam.cpp | 27 ++++++++++--------- orocos_kdl/src/chainfksolverpos_recursive.cpp | 10 +++---- orocos_kdl/src/chainfksolvervel_recursive.cpp | 10 +++---- .../chainidsolver_recursive_newton_euler.cpp | 14 +++++----- orocos_kdl/src/chainidsolver_vereshchagin.cpp | 8 +++--- orocos_kdl/src/chainiksolverpos_lma.cpp | 4 +-- orocos_kdl/src/chainjnttojacdotsolver.cpp | 22 +++++++-------- orocos_kdl/src/chainjnttojacsolver.cpp | 6 ++--- orocos_kdl/src/jacobian.cpp | 24 ++++++++--------- orocos_kdl/src/joint.cpp | 10 +++---- orocos_kdl/src/joint.hpp | 26 +++++++++--------- orocos_kdl/src/segment.hpp | 8 +++--- orocos_kdl/src/tree.cpp | 6 ++--- .../treeidsolver_recursive_newton_euler.cpp | 4 +-- python_orocos_kdl/PyKDL/sip/PyKDL.sip | 8 +++--- python_orocos_kdl/PyKDL/sip/dynamics.sip | 6 ++--- python_orocos_kdl/PyKDL/sip/frames.sip | 6 ++--- python_orocos_kdl/PyKDL/sip/framevel.sip | 6 ++--- python_orocos_kdl/PyKDL/sip/kinfam.sip | 6 ++--- 20 files changed, 110 insertions(+), 107 deletions(-) diff --git a/orocos_kdl/src/chain.cpp b/orocos_kdl/src/chain.cpp index 2fba4582a..016dc4b90 100644 --- a/orocos_kdl/src/chain.cpp +++ b/orocos_kdl/src/chain.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or diff --git a/orocos_kdl/src/chaindynparam.cpp b/orocos_kdl/src/chaindynparam.cpp index 980481412..814a94ef9 100644 --- a/orocos_kdl/src/chaindynparam.cpp +++ b/orocos_kdl/src/chaindynparam.cpp @@ -1,8 +1,9 @@ -// Copyright (C) 2009 Dominick Vanthienen +// Copyright (C) 2020 Ruben Smits // Version: 1.0 // Author: Dominick Vanthienen -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -65,7 +66,7 @@ namespace KDL { return (error = E_SIZE_MISMATCH); unsigned int k=0; double q_; - + //Sweep from root to leaf for(unsigned int i=0;i=0;i--) { - + if(i!=0) { //assumption that previous segment is parent Ic[i-1]=Ic[i-1]+X[i]*Ic[i]; - } + } F=Ic[i]*S[i]; if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed) @@ -107,14 +108,14 @@ namespace KDL { //assumption that previous segment is parent F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] l--; //go down a segment - + if(chain.getSegment(l).getJoint().getType()!=Joint::Fixed) //if the joint connected to segment is not a fixed joint - { + { j--; - H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment + H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment H(j,k)=H(k,j); } - } + } k--; //this if-loop should be repeated nj times (k=nj-1 to k=0) } @@ -128,10 +129,10 @@ namespace KDL { //make a null matrix with the size of q_dotdot and a null wrench SetToZero(jntarraynull); - + //the calculation of coriolis matrix C return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis); - + } //calculate gravity matrix G @@ -139,7 +140,7 @@ namespace KDL { { //make a null matrix with the size of q_dotdot and a null wrench - + SetToZero(jntarraynull); //the calculation of coriolis matrix C return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity); diff --git a/orocos_kdl/src/chainfksolverpos_recursive.cpp b/orocos_kdl/src/chainfksolverpos_recursive.cpp index 051a3afc7..c7bc8bbd3 100644 --- a/orocos_kdl/src/chainfksolverpos_recursive.cpp +++ b/orocos_kdl/src/chainfksolverpos_recursive.cpp @@ -1,9 +1,9 @@ -// Copyright (C) 2007 Francois Cauwe -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Francois Cauwe +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -79,7 +79,7 @@ namespace KDL { j++; }else p_out[0] = chain.getSegment(0).pose(0.0); - + for(unsigned int i=1;i -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Francois Cauwe +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -92,7 +92,7 @@ namespace KDL }else out[0] = FrameVel(chain.getSegment(0).pose(0.0), chain.getSegment(0).twist(0.0,0.0)); - + for (unsigned int i=1;i +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -23,7 +23,7 @@ #include "frames_io.hpp" namespace KDL{ - + ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav): chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()), X(ns),S(ns),v(ns),a(ns),f(ns) @@ -61,10 +61,10 @@ namespace KDL{ j++; }else q_=qdot_=qdotdot_=0.0; - + //Calculate segment properties: X,S,vj,cj - X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the - //frame for transformations from + X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the + //frame for transformations from //the parent to the current coord frame //Transform velocity and unit velocity to segment frame Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_)); diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.cpp b/orocos_kdl/src/chainidsolver_vereshchagin.cpp index ddc86c535..5b5e95a49 100644 --- a/orocos_kdl/src/chainidsolver_vereshchagin.cpp +++ b/orocos_kdl/src/chainidsolver_vereshchagin.cpp @@ -1,8 +1,10 @@ -// Copyright (C) 2009, 2011 +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov -// Maintainer: Ruben Smits, Azamat Shakhimardanov +// Author: Ruben Smits +// Author: Herman Bruyninckx +// Author: Azamat Shakhimardanov +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or diff --git a/orocos_kdl/src/chainiksolverpos_lma.cpp b/orocos_kdl/src/chainiksolverpos_lma.cpp index dd17c345e..a9eb134bf 100644 --- a/orocos_kdl/src/chainiksolverpos_lma.cpp +++ b/orocos_kdl/src/chainiksolverpos_lma.cpp @@ -5,7 +5,7 @@ /************************************************************************** begin : May 2012 - copyright : (C) 2012 Erwin Aertbelien + copyright : (C) 2020 Erwin Aertbelien email : firstname.lastname@mech.kuleuven.ac.be History (only major changes)( AUTHOR-Description ) : @@ -187,7 +187,7 @@ void ChainIkSolverPos_LMA::display_jac(const KDL::JntArray& jval) { int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) { if (nj != chain.getNrOfJoints()) return (error = E_NOT_UP_TO_DATE); - + if (nj != q_init.rows() || nj != q_out.rows()) return (error = E_SIZE_MISMATCH); diff --git a/orocos_kdl/src/chainjnttojacdotsolver.cpp b/orocos_kdl/src/chainjnttojacdotsolver.cpp index 389359233..c7ab26b27 100644 --- a/orocos_kdl/src/chainjnttojacdotsolver.cpp +++ b/orocos_kdl/src/chainjnttojacdotsolver.cpp @@ -1,6 +1,6 @@ /* Computes the Jacobian time derivative - Copyright (C) 2015 Antoine Hoarau + Copyright (C) 2020 Antoine Hoarau This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public @@ -114,9 +114,9 @@ int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, return (error = E_NOERROR); } -const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J, - const unsigned int& joint_idx, - const unsigned int& column_idx, +const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J, + const unsigned int& joint_idx, + const unsigned int& column_idx, const int& representation) { switch(representation) @@ -133,8 +133,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivative(const KDL::Jacobian& J } } -const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee, - const unsigned int& joint_idx, +const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacobian& bs_J_ee, + const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; @@ -164,8 +164,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivativeHybrid(const KDL::Jacob return t_djdq_; } -const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, - const unsigned int& joint_idx, +const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee, + const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; @@ -186,8 +186,8 @@ const Twist& ChainJntToJacDotSolver::getPartialDerivativeBodyFixed(const Jacobia return t_djdq_; } -const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs, - const unsigned int& joint_idx, +const Twist& ChainJntToJacDotSolver::getPartialDerivativeInertial(const KDL::Jacobian& bs_J_bs, + const unsigned int& joint_idx, const unsigned int& column_idx) { int j=joint_idx; @@ -211,7 +211,7 @@ void ChainJntToJacDotSolver::setRepresentation(const int& representation) { if(representation == HYBRID || representation == BODYFIXED || - representation == INERTIAL) + representation == INERTIAL) this->representation_ = representation; } diff --git a/orocos_kdl/src/chainjnttojacsolver.cpp b/orocos_kdl/src/chainjnttojacsolver.cpp index 1fc394b83..1116ac02d 100644 --- a/orocos_kdl/src/chainjnttojacsolver.cpp +++ b/orocos_kdl/src/chainjnttojacsolver.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or diff --git a/orocos_kdl/src/jacobian.cpp b/orocos_kdl/src/jacobian.cpp index c6a5f0d9a..7b99f23a7 100644 --- a/orocos_kdl/src/jacobian.cpp +++ b/orocos_kdl/src/jacobian.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -35,14 +35,14 @@ namespace KDL { data.setZero(); } - + Jacobian::Jacobian(const Jacobian& arg): data(arg.data) { } Jacobian& Jacobian::operator = (const Jacobian& arg) - { + { this->data=arg.data; return *this; } @@ -50,7 +50,7 @@ namespace KDL Jacobian::~Jacobian() { - + } void Jacobian::resize(unsigned int new_nr_of_columns) @@ -96,7 +96,7 @@ namespace KDL dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB)); return true; } - + void Jacobian::changeBase(const Rotation& rot){ for(unsigned int i=0;isetColumn(i,rot*this->getColumn(i));; @@ -115,7 +115,7 @@ namespace KDL for(unsigned int i=0;isetColumn(i,frame*this->getColumn(i)); } - + bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest) { if(src1.columns()!=dest.columns()) @@ -129,12 +129,12 @@ namespace KDL { return Equal((*this),arg); } - + bool Jacobian::operator!=(const Jacobian& arg)const { return !Equal((*this),arg); } - + bool Equal(const Jacobian& a,const Jacobian& b,double eps) { if(a.rows()==b.rows()&&a.columns()==b.columns()){ @@ -142,11 +142,11 @@ namespace KDL }else return false; } - + Twist Jacobian::getColumn(unsigned int i) const{ return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i))); } - + void Jacobian::setColumn(unsigned int i,const Twist& t){ data.col(i).head<3>()=Eigen::Map(t.vel.data); data.col(i).tail<3>()=Eigen::Map(t.rot.data); diff --git a/orocos_kdl/src/joint.cpp b/orocos_kdl/src/joint.cpp index 215bdee15..b2e7c4a3b 100644 --- a/orocos_kdl/src/joint.cpp +++ b/orocos_kdl/src/joint.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -42,7 +42,7 @@ namespace KDL { } // constructor for joint along arbitrary axis, at arbitrary origin - Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, + Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) , axis(_axis / _axis.Norm()), origin(_origin) @@ -56,7 +56,7 @@ namespace KDL { } // constructor for joint along arbitrary axis, at arbitrary origin - Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, + Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()),origin(_origin) diff --git a/orocos_kdl/src/joint.hpp b/orocos_kdl/src/joint.hpp index dc55db1c6..e472c0315 100644 --- a/orocos_kdl/src/joint.hpp +++ b/orocos_kdl/src/joint.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -128,16 +128,16 @@ namespace KDL { */ Twist twist(const double& qdot)const; - /** - * Request the Vector corresponding to the axis of a revolute joint. - * - * @return Vector. e.g (1,0,0) for RotX etc. + /** + * Request the Vector corresponding to the axis of a revolute joint. + * + * @return Vector. e.g (1,0,0) for RotX etc. */ Vector JointAxis() const; - /** - * Request the Vector corresponding to the origin of a revolute joint. - * + /** + * Request the Vector corresponding to the origin of a revolute joint. + * * @return Vector */ Vector JointOrigin() const; @@ -160,8 +160,8 @@ namespace KDL { { return type; }; - - /** + + /** * Request the stringified type of the joint. * * @return const string @@ -239,7 +239,7 @@ namespace KDL { mutable double q_previous; - + class joint_type_exception: public std::exception{ virtual const char* what() const throw(){ return "Joint Type excption";} diff --git a/orocos_kdl/src/segment.hpp b/orocos_kdl/src/segment.hpp index 5ca826980..fff85297e 100644 --- a/orocos_kdl/src/segment.hpp +++ b/orocos_kdl/src/segment.hpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or @@ -148,7 +148,7 @@ namespace KDL { */ Frame getFrameToTip()const { - + return joint.pose(0)*f_tip; } diff --git a/orocos_kdl/src/tree.cpp b/orocos_kdl/src/tree.cpp index 49de45ddd..a4ef97335 100644 --- a/orocos_kdl/src/tree.cpp +++ b/orocos_kdl/src/tree.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2007 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 -// Author: Ruben Smits -// Maintainer: Ruben Smits +// Author: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or diff --git a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp index dfc446e3a..91a5246b3 100644 --- a/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp +++ b/orocos_kdl/src/treeidsolver_recursive_newton_euler.cpp @@ -1,8 +1,8 @@ -// Copyright (C) 2009 Ruben Smits +// Copyright (C) 2020 Ruben Smits // Version: 1.0 // Author: Franco Fusco -// Maintainer: Ruben Smits +// Maintainer: Ruben Smits // URL: http://www.orocos.org/kdl // This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/sip/PyKDL.sip b/python_orocos_kdl/PyKDL/sip/PyKDL.sip index 9910de61f..de11404e7 100644 --- a/python_orocos_kdl/PyKDL/sip/PyKDL.sip +++ b/python_orocos_kdl/PyKDL/sip/PyKDL.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or @@ -22,7 +22,7 @@ %Module(name = PyKDL, keyword_arguments="All") -%License(type="LGPL", licensee="Ruben Smits", signature="ruben@intermodalics.eu", timestamp="2014") +%License(type="LGPL", licensee="Ruben Smits", signature="ruben@intermodalics.eu", timestamp="2020") %Include std_string.sip %Include std_vector.sip diff --git a/python_orocos_kdl/PyKDL/sip/dynamics.sip b/python_orocos_kdl/PyKDL/sip/dynamics.sip index 0b3302f94..c5efb5b7b 100644 --- a/python_orocos_kdl/PyKDL/sip/dynamics.sip +++ b/python_orocos_kdl/PyKDL/sip/dynamics.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/sip/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip index a97a7364f..461fb5b03 100644 --- a/python_orocos_kdl/PyKDL/sip/frames.sip +++ b/python_orocos_kdl/PyKDL/sip/frames.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 5ee1438f9..271700b33 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or diff --git a/python_orocos_kdl/PyKDL/sip/kinfam.sip b/python_orocos_kdl/PyKDL/sip/kinfam.sip index e2b02f23c..b3e9ed366 100644 --- a/python_orocos_kdl/PyKDL/sip/kinfam.sip +++ b/python_orocos_kdl/PyKDL/sip/kinfam.sip @@ -1,8 +1,8 @@ -//Copyright (C) 2007 Ruben Smits +//Copyright (C) 2020 Ruben Smits // //Version: 1.0 -//Author: Ruben Smits -//Maintainer: Ruben Smits +//Author: Ruben Smits +//Maintainer: Ruben Smits //URL: http://www.orocos.org/kdl // //This library is free software; you can redistribute it and/or From 2efa05859456b3d7e625f19e46ed847fe28314dd Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 11:42:28 +0200 Subject: [PATCH 124/136] (PyKDL) add author info for significant modified files --- python_orocos_kdl/tests/framestest.py | 1 + python_orocos_kdl/tests/frameveltest.py | 1 + python_orocos_kdl/tests/kinfamtest.py | 1 + 3 files changed, 3 insertions(+) diff --git a/python_orocos_kdl/tests/framestest.py b/python_orocos_kdl/tests/framestest.py index 41b73fdb7..e8953cfcc 100644 --- a/python_orocos_kdl/tests/framestest.py +++ b/python_orocos_kdl/tests/framestest.py @@ -2,6 +2,7 @@ # Version: 1.0 # Author: Ruben Smits +# Author: Matthijs van der Burgh # Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl diff --git a/python_orocos_kdl/tests/frameveltest.py b/python_orocos_kdl/tests/frameveltest.py index 2471340f1..418920b28 100644 --- a/python_orocos_kdl/tests/frameveltest.py +++ b/python_orocos_kdl/tests/frameveltest.py @@ -2,6 +2,7 @@ # Version: 1.0 # Author: Ruben Smits +# Author: Matthijs van der Burgh # Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl diff --git a/python_orocos_kdl/tests/kinfamtest.py b/python_orocos_kdl/tests/kinfamtest.py index 093cde87e..a1f00e8d6 100644 --- a/python_orocos_kdl/tests/kinfamtest.py +++ b/python_orocos_kdl/tests/kinfamtest.py @@ -2,6 +2,7 @@ # Version: 1.0 # Author: Ruben Smits +# Author: Matthijs van der Burgh # Maintainer: Ruben Smits # URL: http://www.orocos.org/kdl From 8ccc304433894acdf597abf5d0b81e1b7a74bcd4 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 11:59:10 +0200 Subject: [PATCH 125/136] (kdl) move to pkg format 3 --- orocos_kdl/package.xml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/orocos_kdl/package.xml b/orocos_kdl/package.xml index cc8cfbf0b..dd8b7438e 100644 --- a/orocos_kdl/package.xml +++ b/orocos_kdl/package.xml @@ -1,4 +1,5 @@ - + + orocos_kdl 1.4.0 @@ -12,11 +13,12 @@ cmake eigen - catkin - eigen - pkg-config + catkin + eigen + pkg-config cppunit + cmake From 71b3226eeee916ae3bf51c890fcb4959db8093fe Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 11:59:27 +0200 Subject: [PATCH 126/136] (PyKDL) move to pkg format 3 --- python_orocos_kdl/package.xml | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml index aad4711b6..3d93ee144 100644 --- a/python_orocos_kdl/package.xml +++ b/python_orocos_kdl/package.xml @@ -1,4 +1,5 @@ - + + python_orocos_kdl 1.4.0 @@ -14,11 +15,13 @@ orocos_kdl python-sip - catkin - orocos_kdl - python-sip + catkin + orocos_kdl + python-sip + python3-sip - python-psutil + python-psutil + python3-psutil cmake From a66cddfb8f8a2c23b61f84e8dab48336f22a9f17 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 12:00:03 +0200 Subject: [PATCH 127/136] (PyKDL) add python-future test_depend --- python_orocos_kdl/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml index 3d93ee144..92d1f338f 100644 --- a/python_orocos_kdl/package.xml +++ b/python_orocos_kdl/package.xml @@ -20,6 +20,8 @@ python-sip python3-sip + python-future + python3-future python-psutil python3-psutil From fa7ca151ad115102ea9aed36bf4b40dcc510778f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 12:04:33 +0200 Subject: [PATCH 128/136] (PyKDL) update doc information --- python_orocos_kdl/package.xml | 5 ++++- python_orocos_kdl/rosdoc.yaml | 3 ++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/package.xml b/python_orocos_kdl/package.xml index 92d1f338f..03c11530e 100644 --- a/python_orocos_kdl/package.xml +++ b/python_orocos_kdl/package.xml @@ -4,7 +4,7 @@ 1.4.0 This package contains the python bindings PyKDL for the Kinematics and Dynamics - Library (KDL), distributed by the Orocos Project. + Library (KDL), distributed by the Orocos Project. Ruben Smits http://wiki.ros.org/python_orocos_kdl @@ -25,8 +25,11 @@ python-psutil python3-psutil + python-sphinx + cmake + diff --git a/python_orocos_kdl/rosdoc.yaml b/python_orocos_kdl/rosdoc.yaml index daa8282ec..b2f4d1f9e 100644 --- a/python_orocos_kdl/rosdoc.yaml +++ b/python_orocos_kdl/rosdoc.yaml @@ -1,3 +1,4 @@ - builder: sphinx output_dir: doc - sphinx_root_dir: doc \ No newline at end of file + sphinx_root_dir: doc + name: Python API From 60370c89753b9074400f15c656ce9a295beb35b0 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 12:06:35 +0200 Subject: [PATCH 129/136] (kdl) add doc_depend --- orocos_kdl/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/orocos_kdl/package.xml b/orocos_kdl/package.xml index dd8b7438e..f858bfd21 100644 --- a/orocos_kdl/package.xml +++ b/orocos_kdl/package.xml @@ -4,7 +4,7 @@ 1.4.0 This package contains a recent version of the Kinematics and Dynamics - Library (KDL), distributed by the Orocos Project. + Library (KDL), distributed by the Orocos Project. Ruben Smits http://wiki.ros.org/orocos_kdl @@ -19,6 +19,8 @@ cppunit + doxygen + cmake From f23632f7c660f0579642aad25a75ee19b23bb345 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sat, 18 Apr 2020 13:41:30 +0200 Subject: [PATCH 130/136] (PyKDL)(sip) add __version__ --- python_orocos_kdl/PyKDL/sip/PyKDL.sip | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/python_orocos_kdl/PyKDL/sip/PyKDL.sip b/python_orocos_kdl/PyKDL/sip/PyKDL.sip index de11404e7..d5d60756c 100644 --- a/python_orocos_kdl/PyKDL/sip/PyKDL.sip +++ b/python_orocos_kdl/PyKDL/sip/PyKDL.sip @@ -31,3 +31,10 @@ %Include framevel.sip %Include kinfam.sip %Include dynamics.sip + +const std::string __version__; + +%ModuleCode +#include +const std::string __version__ = KDL_VERSION_STRING; +%End From a6fbd38c509ed7d00db0110fb0bf69afa879ad24 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 4 May 2020 23:29:40 +0200 Subject: [PATCH 131/136] (kdl) add eps argument to Norm functions eps is used to determine when to set norm to zero --- orocos_kdl/src/frameacc.hpp | 2 +- orocos_kdl/src/frameacc.inl | 4 ++-- orocos_kdl/src/frames.cpp | 10 +++++----- orocos_kdl/src/frames.hpp | 4 ++-- orocos_kdl/src/framevel.hpp | 2 +- orocos_kdl/src/framevel.inl | 5 +++-- 6 files changed, 14 insertions(+), 13 deletions(-) diff --git a/orocos_kdl/src/frameacc.hpp b/orocos_kdl/src/frameacc.hpp index bdc636cb8..20d4631b3 100644 --- a/orocos_kdl/src/frameacc.hpp +++ b/orocos_kdl/src/frameacc.hpp @@ -76,7 +76,7 @@ class VectorAcc IMETHOD VectorAcc& operator -= (const VectorAcc& arg); IMETHOD static VectorAcc Zero(); IMETHOD void ReverseSign(); - IMETHOD doubleAcc Norm(); + IMETHOD doubleAcc Norm(double eps=epsilon); IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2); IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2); diff --git a/orocos_kdl/src/frameacc.inl b/orocos_kdl/src/frameacc.inl index dfbdc620e..fa9c23ce4 100644 --- a/orocos_kdl/src/frameacc.inl +++ b/orocos_kdl/src/frameacc.inl @@ -126,9 +126,9 @@ void VectorAcc::ReverseSign() { dv.ReverseSign(); } -doubleAcc VectorAcc::Norm() { +doubleAcc VectorAcc::Norm(double eps) { doubleAcc res; - res.t = p.Norm(); + res.t = p.Norm(eps); res.d = dot(p,v)/res.t; res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t; return res; diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index e45e8fc4e..8d1913e6c 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -85,13 +85,13 @@ namespace KDL { ); } - double Vector2::Norm() const + double Vector2::Norm(double eps) const { double tmp1 = fabs(data[0]); double tmp2 = fabs(data[1]); - if (tmp1 == 0.0 && tmp2 == 0.0) - return 0.0; + if (tmp1 < eps && tmp2 < eps) + return 0; if (tmp1 > tmp2) { return tmp1*sqrt(1+sqr(data[1]/data[0])); @@ -115,7 +115,7 @@ namespace KDL { // do some effort not to lose precision - double Vector::Norm() const + double Vector::Norm(double eps) const { double tmp1; double tmp2; @@ -124,7 +124,7 @@ namespace KDL { if (tmp1 >= tmp2) { tmp2=fabs(data[2]); if (tmp1 >= tmp2) { - if (tmp1 == 0) { + if (tmp1 < eps) { // only to everything exactly zero case, all other are handled correctly return 0; } diff --git a/orocos_kdl/src/frames.hpp b/orocos_kdl/src/frames.hpp index 73f3db19c..055beb3b4 100644 --- a/orocos_kdl/src/frames.hpp +++ b/orocos_kdl/src/frames.hpp @@ -237,7 +237,7 @@ class Vector double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm() const; + double Norm(double eps=epsilon) const; @@ -1015,7 +1015,7 @@ class Vector2 double Normalize(double eps=epsilon); //! @return the norm of the vector - double Norm() const; + double Norm(double eps=epsilon) const; //! projects v in its XY plane, and sets *this to these values inline void Set3DXY(const Vector& v); diff --git a/orocos_kdl/src/framevel.hpp b/orocos_kdl/src/framevel.hpp index ba119aa0b..650bd3c83 100644 --- a/orocos_kdl/src/framevel.hpp +++ b/orocos_kdl/src/framevel.hpp @@ -104,7 +104,7 @@ class VectorVel IMETHOD VectorVel& operator -= (const VectorVel& arg); IMETHOD static VectorVel Zero(); IMETHOD void ReverseSign(); - IMETHOD doubleVel Norm() const; + IMETHOD doubleVel Norm(double eps=epsilon) const; IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2); IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2); diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl index c955905cb..a4f86e236 100644 --- a/orocos_kdl/src/framevel.inl +++ b/orocos_kdl/src/framevel.inl @@ -350,9 +350,10 @@ void VectorVel::ReverseSign() { p.ReverseSign(); v.ReverseSign(); } -doubleVel VectorVel::Norm() const { +doubleVel VectorVel::Norm(double eps) const { double n = p.Norm(); - return doubleVel(n,n ? dot(p,v)/n : 0); // Setting norm of p to 0 in case norm of v is 0 + return doubleVel((n < eps) ? 0 : n, + (n < eps) ? 0 : dot(p,v)/n); // Setting norm of p to 0 in case norm of v is 0 } bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) { From 14700030f25658223d5f39f36117ea2a41a0e736 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 4 May 2020 23:30:19 +0200 Subject: [PATCH 132/136] (PyKDL) add eps argument to norm functions --- python_orocos_kdl/PyKDL/pybind11/frames.cpp | 2 +- python_orocos_kdl/PyKDL/pybind11/framevel.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/pybind11/frames.cpp b/python_orocos_kdl/PyKDL/pybind11/frames.cpp index ee4b11ee9..65afa0f10 100644 --- a/python_orocos_kdl/PyKDL/pybind11/frames.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/frames.cpp @@ -81,7 +81,7 @@ void init_frames(py::module &m) return operator-(a); }, py::is_operator()); vector.def_static("Zero", &Vector::Zero); - vector.def("Norm", &Vector::Norm); + vector.def("Norm", &Vector::Norm, py::arg("eps")=epsilon); vector.def("Normalize", &Vector::Normalize, py::arg("eps")=epsilon); vector.def(py::pickle( [](const Vector &v) diff --git a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp index c166d47d8..43842136d 100644 --- a/python_orocos_kdl/PyKDL/pybind11/framevel.cpp +++ b/python_orocos_kdl/PyKDL/pybind11/framevel.cpp @@ -87,7 +87,7 @@ void init_framevel(pybind11::module &m) }); vector_vel.def_static("Zero", &VectorVel::Zero); vector_vel.def("ReverseSign", &VectorVel::ReverseSign); - vector_vel.def("Norm", &VectorVel::Norm); + vector_vel.def("Norm", &VectorVel::Norm, py::arg("eps")=epsilon); vector_vel.def(py::self += py::self); vector_vel.def(py::self -= py::self); vector_vel.def(py::self + py::self); From 3be9580c468145f600e2c5b5a74c41a3f843b1b3 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 4 May 2020 23:31:05 +0200 Subject: [PATCH 133/136] (PyKDL)(sip) add eps argument to norm functions --- python_orocos_kdl/PyKDL/sip/frames.sip | 2 +- python_orocos_kdl/PyKDL/sip/framevel.sip | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python_orocos_kdl/PyKDL/sip/frames.sip b/python_orocos_kdl/PyKDL/sip/frames.sip index 461fb5b03..d0be8ba58 100644 --- a/python_orocos_kdl/PyKDL/sip/frames.sip +++ b/python_orocos_kdl/PyKDL/sip/frames.sip @@ -79,7 +79,7 @@ public: static Vector Zero()/Factory/; - double Norm(); + double Norm(double eps=epsilon) const; double Normalize(double eps=epsilon); %PickleCode diff --git a/python_orocos_kdl/PyKDL/sip/framevel.sip b/python_orocos_kdl/PyKDL/sip/framevel.sip index 271700b33..38c561203 100644 --- a/python_orocos_kdl/PyKDL/sip/framevel.sip +++ b/python_orocos_kdl/PyKDL/sip/framevel.sip @@ -86,7 +86,7 @@ public: VectorVel& operator -= (const VectorVel& arg); static VectorVel Zero(); void ReverseSign(); - doubleVel Norm() const; + doubleVel Norm(double eps=epsilon) const; %PickleCode const sipTypeDef *vector_type = sipFindType("Vector"); From 818f669446e7717ee654925f763cdb7e10bc9241 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Mon, 4 May 2020 23:46:53 +0200 Subject: [PATCH 134/136] (kdl) normalize returns 0 if norm < eps fixes #65 --- orocos_kdl/src/frames.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/frames.cpp b/orocos_kdl/src/frames.cpp index 8d1913e6c..22c436d44 100644 --- a/orocos_kdl/src/frames.cpp +++ b/orocos_kdl/src/frames.cpp @@ -100,13 +100,13 @@ namespace KDL { } } // makes v a unitvector and returns the norm of v. - // if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + // if v is smaller than eps, Vector(1,0) is returned with norm 0. // if this is not good, check the return value of this method. double Vector2::Normalize(double eps) { double v = this->Norm(); if (v < eps) { *this = Vector2(1,0); - return v; + return 0; } else { *this = (*this)/v; return v; @@ -149,7 +149,7 @@ namespace KDL { double v = this->Norm(); if (v < eps) { *this = Vector(1,0,0); - return v; + return 0; } else { *this = (*this)/v; return v; From 080443739030dca3410cf7ecaf80a04cc81d518e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 May 2020 00:29:43 +0200 Subject: [PATCH 135/136] (PyKDL) cmake: add depracation warning --- python_orocos_kdl/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python_orocos_kdl/CMakeLists.txt b/python_orocos_kdl/CMakeLists.txt index c67c98533..e8c074e23 100644 --- a/python_orocos_kdl/CMakeLists.txt +++ b/python_orocos_kdl/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(PythonLibs ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR} REQUIRED set(PYTHON_SITE_PACKAGES_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/dist-packages") set(LIBRARY_NAME "PyKDL") +message(DEPRECATION "PyKDL has been moved to PyBin11. PyKDL based on SIP might become depracted in the (near) future. To keep using the SIP based version set 'BUILD_PYKDL_PYBIND11' to OFF.") + option(BUILD_PYKDL_PYBIND11 "Use PyBind11 instead of SIP" ON) if (BUILD_PYKDL_PYBIND11) SET(PYTHON_MODULE_EXTENSION ".so") From 9ec544ef5aab988a327f338f945aa233e9952e4c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 May 2020 09:34:25 +0200 Subject: [PATCH 136/136] (kdl) improve norm of VectorVel --- orocos_kdl/src/framevel.inl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/orocos_kdl/src/framevel.inl b/orocos_kdl/src/framevel.inl index a4f86e236..05aceb0af 100644 --- a/orocos_kdl/src/framevel.inl +++ b/orocos_kdl/src/framevel.inl @@ -351,9 +351,10 @@ void VectorVel::ReverseSign() { v.ReverseSign(); } doubleVel VectorVel::Norm(double eps) const { - double n = p.Norm(); - return doubleVel((n < eps) ? 0 : n, - (n < eps) ? 0 : dot(p,v)/n); // Setting norm of p to 0 in case norm of v is 0 + double n = p.Norm(eps); + if (n < eps) // Setting norm of p and v to 0 in case norm of p is smaller than eps + return doubleVel(0, 0); + return doubleVel(n, dot(p,v)/n); } bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {