diff --git a/CMakeLists.txt b/CMakeLists.txt index dd121147..8a8291ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,11 +15,6 @@ add_subdirectory(src) add_subdirectory(test) add_subdirectory(tutorials) add_subdirectory(bin) -if(USE_PYTHON) - message("USING PYTHON.") - add_subdirectory(bindings/python) -endif() - find_package(Doxygen) if(DOXYGEN_FOUND) set(doxyfile_in ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile) diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt deleted file mode 100644 index b49f4433..00000000 --- a/bindings/python/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -find_package(PythonInterp 3 REQUIRED) -find_package(PythonLibs 3 REQUIRED) -find_package(Boost COMPONENTS python numpy REQUIRED) - -set(PYTHON_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages) - -message(STATUS "PYTHON_VERSION_MAJOR.PYTHON_VERSION_MINOR = ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}") -message(STATUS "PYTHON_LIBRARIES = ${PYTHON_LIBRARIES}") -message(STATUS "PYTHON_EXECUTABLE = ${PYTHON_EXECUTABLE}") -message(STATUS "PYTHON_INCLUDE_DIRS = ${PYTHON_INCLUDE_DIRS}") -message(STATUS "Boost_LIBRARIES = ${Boost_LIBRARIES}") -message(STATUS "PYTHON_INSTALL_PATH = ${PYTHON_INSTALL_PATH}") - -add_subdirectory(core) -add_subdirectory(controllers) -add_subdirectory(robot_models) -add_subdirectory(scenes) -add_subdirectory(solvers) - -install(FILES __init__.py - DESTINATION ${PYTHON_INSTALL_PATH}/wbc) diff --git a/bindings/python/__init__.py b/bindings/python/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/bindings/python/base_types_conversion.h b/bindings/python/base_types_conversion.h deleted file mode 100644 index 408f5dc4..00000000 --- a/bindings/python/base_types_conversion.h +++ /dev/null @@ -1,79 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace py = boost::python; -namespace np = boost::python::numpy; - -namespace wbc_py{ - -base::samples::Joints tobaseSamplesJoints(const base::NamedVector &joints_in){ - base::samples::Joints joints_out; - joints_out.elements = joints_in.elements; - joints_out.names = joints_in.names; - joints_out.time = base::Time::now(); - return joints_out; -} - -base::NamedVector toNamedVector(const base::samples::Joints &joints_in){ - base::NamedVector joints_out; - joints_out.elements = joints_in.elements; - joints_out.names = joints_in.names; - return joints_out; -} - -base::NamedVector fromJointLimits(const base::JointLimits &limits_in){ - base::NamedVector limits_out; - limits_out.elements = limits_in.elements; - limits_out.names = limits_in.names; - return limits_out; -} - - -} - -namespace pygen { - -template -struct python_list_to_joint_vector { - python_list_to_joint_vector() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - if (!PySequence_Check(obj_ptr)) - return 0; - - py::list arr = py::extract(obj_ptr); - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - py::list arr = py::extract(obj_ptr); - auto len = py::len(arr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) JointVectorType; - JointVectorType& vec = *static_cast(storage); - - vec.resize(len); - - for (long i = 0; i < len; ++i) - vec[i] = py::extract(arr[i]); - - data->convertible = storage; - } -}; - -} // namespace pygen diff --git a/bindings/python/controllers/CMakeLists.txt b/bindings/python/controllers/CMakeLists.txt deleted file mode 100644 index 8a5ae4e6..00000000 --- a/bindings/python/controllers/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -PYTHON_ADD_MODULE(controllers controllers.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(controllers PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-controllers) - -target_include_directories(controllers PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS controllers - DESTINATION ${PYTHON_INSTALL_PATH}/wbc) diff --git a/bindings/python/controllers/controllers.cpp b/bindings/python/controllers/controllers.cpp deleted file mode 100644 index da9e53ed..00000000 --- a/bindings/python/controllers/controllers.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "controllers.hpp" - -namespace wbc_py { -JointPosPDController::JointPosPDController(const std::vector &joint_names) : - ctrl_lib::JointPosPDController(joint_names){ -} -base::NamedVector JointPosPDController::update(const base::NamedVector &setpoint, - const base::NamedVector &feedback){ - return toNamedVector(ctrl_lib::JointPosPDController::update(tobaseSamplesJoints(setpoint), tobaseSamplesJoints(feedback))); -} -} - -BOOST_PYTHON_MODULE(controllers){ - - np::initialize(); - - py::class_("CartesianPosPDController") - .def("update", &ctrl_lib::CartesianPosPDController::update, py::return_value_policy()) - .def("setPGain", &ctrl_lib::CartesianPosPDController::setPGain) - .def("setDGain", &ctrl_lib::CartesianPosPDController::setDGain) - .def("setFFGain", &ctrl_lib::CartesianPosPDController::setFFGain) - .def("setMaxCtrlOutput", &ctrl_lib::CartesianPosPDController::setMaxCtrlOutput) - .def("setDeadZone", &ctrl_lib::CartesianPosPDController::setDeadZone) - .def("pGain", &ctrl_lib::CartesianPosPDController::pGain, py::return_value_policy()) - .def("dGain", &ctrl_lib::CartesianPosPDController::dGain, py::return_value_policy()) - .def("ffGain", &ctrl_lib::CartesianPosPDController::ffGain, py::return_value_policy()) - .def("maxCtrlOutput", &ctrl_lib::CartesianPosPDController::maxCtrlOutput, py::return_value_policy()) - .def("deadZone", &ctrl_lib::CartesianPosPDController::deadZone, py::return_value_policy()) - .def("getControlError", &ctrl_lib::CartesianPosPDController::getControlError); - - py::class_("JointPosPDController", py::init>()) - .def("update", &wbc_py::JointPosPDController::update) - .def("setPGain", &wbc_py::JointPosPDController::setPGain) - .def("setDGain", &wbc_py::JointPosPDController::setDGain) - .def("setFFGain", &wbc_py::JointPosPDController::setFFGain) - .def("setMaxCtrlOutput", &wbc_py::JointPosPDController::setMaxCtrlOutput) - .def("setDeadZone", &wbc_py::JointPosPDController::setDeadZone) - .def("pGain", &wbc_py::JointPosPDController::pGain, py::return_value_policy()) - .def("dGain", &wbc_py::JointPosPDController::dGain, py::return_value_policy()) - .def("ffGain", &wbc_py::JointPosPDController::ffGain, py::return_value_policy()) - .def("maxCtrlOutput", &wbc_py::JointPosPDController::maxCtrlOutput, py::return_value_policy()) - .def("deadZone", &wbc_py::JointPosPDController::deadZone, py::return_value_policy()) - .def("getControlError", &wbc_py::JointPosPDController::getControlError); -} diff --git a/bindings/python/controllers/controllers.hpp b/bindings/python/controllers/controllers.hpp deleted file mode 100644 index a46cfb40..00000000 --- a/bindings/python/controllers/controllers.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef WBC_PY_CONTROLLERS_HPP -#define WBC_PY_CONTROLLERS_HPP - -#include "../eigen_conversion.h" -#include "../base_types_conversion.h" -#include "../std_vector_conversion.h" -#include "controllers/CartesianPosPDController.hpp" -#include "controllers/JointPosPDController.hpp" - -namespace wbc_py { - -class JointPosPDController : public ctrl_lib::JointPosPDController{ -public: - JointPosPDController(const std::vector &joint_names); - base::NamedVector update(const base::NamedVector &setpoint, - const base::NamedVector &feedback); -}; -} - -#endif diff --git a/bindings/python/core/CMakeLists.txt b/bindings/python/core/CMakeLists.txt deleted file mode 100644 index 8d3c57c2..00000000 --- a/bindings/python/core/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -PYTHON_ADD_MODULE(core core.cpp) -# Set up the libraries and header search paths for this target -target_link_libraries(core PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-core) - -target_include_directories(core PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS core - DESTINATION ${PYTHON_INSTALL_PATH}/wbc) diff --git a/bindings/python/core/core.cpp b/bindings/python/core/core.cpp deleted file mode 100644 index c0925413..00000000 --- a/bindings/python/core/core.cpp +++ /dev/null @@ -1,240 +0,0 @@ -#include "../eigen_conversion.h" -#include "../std_vector_conversion.h" -#include "../wbc_types_conversions.h" -#include "core/RobotModelConfig.hpp" -#include "core/TaskConfig.hpp" -#include "core/TaskStatus.hpp" -#include "core/QuadraticProgram.hpp" -#include -#include - -BOOST_PYTHON_MODULE(core){ - - np::initialize(); - - pygen::convertMatrix>(); - pygen::convertVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertStdVector>(); - pygen::convertVector>(); - pygen::convertVector>(); - pygen::convertTransform>(); - pygen::convertQuaternion>(); - pygen::convertStdVector>(); - - py::class_("Pose") - .add_property("position", - py::make_getter(&base::Pose::position, py::return_value_policy()), - py::make_setter(&base::Pose::position)) - .add_property("orientation", - py::make_getter(&base::Pose::orientation, py::return_value_policy()), - py::make_setter(&base::Pose::orientation)) - .def("toVector6d", &base::Pose::toVector6d) - .def("toTransform", &base::Pose::toTransform) - .def("fromTransform", &base::Pose::fromTransform); - - py::class_("Twist") - .add_property("linear", - py::make_getter(&base::Twist::linear,py::return_value_policy()), - py::make_setter(&base::Twist::linear)) - .add_property("angular", - py::make_getter(&base::Twist::angular, py::return_value_policy()), - py::make_setter(&base::Twist::angular)); - - py::class_("Acceleration") - .add_property("linear", - py::make_getter(&base::Acceleration::linear,py::return_value_policy()), - py::make_setter(&base::Acceleration::linear)) - .add_property("angular", - py::make_getter(&base::Acceleration::angular, py::return_value_policy()), - py::make_setter(&base::Acceleration::angular)); - - py::class_("Wrench") - .add_property("force", - py::make_getter(&base::Wrench::force,py::return_value_policy()), - py::make_setter(&base::Wrench::force)) - .add_property("torque", - py::make_getter(&base::Wrench::torque, py::return_value_policy()), - py::make_setter(&base::Wrench::torque)); - - py::class_>("Wrenches") - .add_property("names", - py::make_getter(&base::NamedVector::names,py::return_value_policy()), - py::make_setter(&base::NamedVector::names)) - .add_property("elements", - py::make_getter(&base::NamedVector::elements, py::return_value_policy()), - py::make_setter(&base::NamedVector::elements)); - - py::class_("baseRigidBodyStateSE3") - .def_readwrite("pose", &base::RigidBodyStateSE3::pose) - .def_readwrite("twist", &base::RigidBodyStateSE3::twist) - .def_readwrite("acceleration", &base::RigidBodyStateSE3::acceleration) - .def_readwrite("wrench", &base::RigidBodyStateSE3::wrench); - - py::class_("Time") - .def_readwrite("microseconds", &base::Time::microseconds); - - py::class_("RigidBodyStateSE3") - .def_readwrite("time", &base::samples::RigidBodyStateSE3::time) - .def_readwrite("pose", &base::RigidBodyStateSE3::pose) - .def_readwrite("twist", &base::RigidBodyStateSE3::twist) - .def_readwrite("acceleration", &base::RigidBodyStateSE3::acceleration) - .def_readwrite("wrench", &base::RigidBodyStateSE3::wrench); - - py::class_("JointState") - .def_readwrite("position", &base::JointState::position) - .def_readwrite("speed", &base::JointState::speed) - .def_readwrite("acceleration", &base::JointState::acceleration) - .def_readwrite("effort", &base::JointState::effort) - .def_readwrite("raw", &base::JointState::raw); - - py::class_>("Joints") - .add_property("names", - py::make_getter(&base::NamedVector::names, py::return_value_policy()), - py::make_setter(&base::NamedVector::names)) - .add_property("elements", - py::make_getter(&base::NamedVector::elements, py::return_value_policy()), - py::make_setter(&base::NamedVector::elements)); - - py::class_("JointLimitRange") - .add_property("min", - py::make_getter(&base::JointLimitRange::min, py::return_value_policy()), - py::make_setter(&base::JointLimitRange::min)) - .add_property("max", - py::make_getter(&base::JointLimitRange::max, py::return_value_policy()), - py::make_setter(&base::JointLimitRange::max)); - - py::class_>("JointLimits") - .add_property("names", - py::make_getter(&base::NamedVector::names, py::return_value_policy()), - py::make_setter(&base::NamedVector::names)) - .add_property("elements", - py::make_getter(&base::NamedVector::elements, py::return_value_policy()), - py::make_setter(&base::NamedVector::elements)); - - py::class_("RobotModelConfig") - .def_readwrite("file_or_string", &wbc::RobotModelConfig::file_or_string) - .def_readwrite("submechanism_file", &wbc::RobotModelConfig::submechanism_file) - .def_readwrite("floating_base", &wbc::RobotModelConfig::floating_base) - .def_readwrite("floating_base", &wbc::RobotModelConfig::floating_base) - .add_property("contact_points", &wbc_py::RobotModelConfig::getActiveContacts, &wbc_py::RobotModelConfig::setActiveContacts); - - py::enum_("TaskType") - .value("unset", wbc::TaskType::unset) - .value("cart", wbc::TaskType::cart) - .value("jnt", wbc::TaskType::jnt); - - py::class_("TaskConfig") - .def_readwrite("name", &wbc::TaskConfig::name) - .def_readwrite("type", &wbc::TaskConfig::type) - .def_readwrite("priority", &wbc::TaskConfig::priority) - .add_property("weights", - py::make_getter(&wbc::TaskConfig::weights, py::return_value_policy()), - py::make_setter(&wbc::TaskConfig::weights)) - .def_readwrite("activation", &wbc::TaskConfig::activation) - .def_readwrite("timeout", &wbc::TaskConfig::timeout) - .add_property("joint_names", - py::make_getter(&wbc::TaskConfig::joint_names, py::return_value_policy()), - py::make_setter(&wbc::TaskConfig::joint_names)) - .def_readwrite("root", &wbc::TaskConfig::root) - .def_readwrite("tip", &wbc::TaskConfig::tip) - .def_readwrite("ref_frame", &wbc::TaskConfig::ref_frame); - - py::class_("QuadraticProgram") - .add_property("A", - py::make_getter(&wbc::QuadraticProgram::A, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::A)) - .add_property("g", - py::make_getter(&wbc::QuadraticProgram::g, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::g)) - .add_property("b", - py::make_getter(&wbc::QuadraticProgram::b, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::b)) - .add_property("C", - py::make_getter(&wbc::QuadraticProgram::C, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::C)) - .add_property("lower_x", - py::make_getter(&wbc::QuadraticProgram::lower_x, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::lower_x)) - .add_property("upper_x", - py::make_getter(&wbc::QuadraticProgram::upper_x, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::upper_x)) - .add_property("lower_y", - py::make_getter(&wbc::QuadraticProgram::lower_y, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::lower_y)) - .add_property("upper_y", - py::make_getter(&wbc::QuadraticProgram::upper_y, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::upper_y)) - .add_property("H", - py::make_getter(&wbc::QuadraticProgram::H, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::H)) - .add_property("Wy", - py::make_getter(&wbc::QuadraticProgram::Wy, py::return_value_policy()), - py::make_setter(&wbc::QuadraticProgram::Wy)) - .def_readwrite("neq", &wbc::QuadraticProgram::neq) - .def_readwrite("nin", &wbc::QuadraticProgram::nin) - .def_readwrite("nq", &wbc::QuadraticProgram::nq) - .def_readwrite("bounded", &wbc::QuadraticProgram::bounded) - .def("resize", &wbc::QuadraticProgram::resize); - - py::class_("HierarchicalQP") - .add_property("prios", - py::make_getter(&wbc::HierarchicalQP::prios, py::return_value_policy()), - py::make_setter(&wbc::HierarchicalQP::prios)) - .add_property("Wq", - py::make_getter(&wbc::HierarchicalQP::Wq, py::return_value_policy()), - py::make_setter(&wbc::HierarchicalQP::Wq)) - .def("resize", &wbc::HierarchicalQP::resize); - - py::class_>("JointWeights") - .add_property("names", - py::make_getter(&wbc::JointWeights::names, py::return_value_policy()), - py::make_setter(&wbc::JointWeights::names)) - .add_property("elements", - py::make_getter(&wbc::JointWeights::elements, py::return_value_policy()), - py::make_setter(&wbc::JointWeights::elements)); - - py::class_("ActiveContact") - .def_readwrite("active", &wbc::ActiveContact::active) - .def_readwrite("mu", &wbc::ActiveContact::mu); - - py::class_>("ActiveContacts") - .add_property("names", - py::make_getter(&wbc::ActiveContacts::names, py::return_value_policy()), - py::make_setter(&wbc::ActiveContacts::names)) - .add_property("elements", - py::make_getter(&wbc::ActiveContacts::elements, py::return_value_policy()), - py::make_setter(&wbc::ActiveContacts::elements)); - - py::class_("TaskStatus") - .def_readwrite("config", &wbc::TaskStatus::config) - .def_readwrite("activation", &wbc::TaskStatus::activation) - .def_readwrite("timeout", &wbc::TaskStatus::timeout) - .add_property("weights", - py::make_getter(&wbc::TaskStatus::weights, py::return_value_policy()), - py::make_setter(&wbc::TaskStatus::weights)) - .add_property("y_ref", - py::make_getter(&wbc::TaskStatus::y_ref, py::return_value_policy()), - py::make_setter(&wbc::TaskStatus::y_ref)) - .add_property("y_solution", - py::make_getter(&wbc::TaskStatus::y_solution, py::return_value_policy()), - py::make_setter(&wbc::TaskStatus::y_solution)) - .add_property("y", - py::make_getter(&wbc::TaskStatus::y, py::return_value_policy()), - py::make_setter(&wbc::TaskStatus::y)); - - py::class_>("TasksStatus") - .add_property("names", - py::make_getter(&wbc::TasksStatus::names, py::return_value_policy()), - py::make_setter(&wbc::TasksStatus::names)) - .add_property("elements", - py::make_getter(&wbc::TasksStatus::elements, py::return_value_policy()), - py::make_setter(&wbc::TasksStatus::elements)); -} diff --git a/bindings/python/eigen_conversion.h b/bindings/python/eigen_conversion.h deleted file mode 100644 index 55406589..00000000 --- a/bindings/python/eigen_conversion.h +++ /dev/null @@ -1,647 +0,0 @@ -// MIT License - -// Copyright (c) 2019 Vincent SAMY - -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: - -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. - -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#pragma once - -#include -#include -#include -#include -#include - -namespace py = boost::python; -namespace np = boost::python::numpy; - -namespace pygen { - -/* - * ***************** List -> Eigen converters ***************** - */ - - -/** Conversion from a python list to an Eigen Vector - * This convert list of type T (int, float32, float64, ...) to an Eigen::VectorType - * The template VectorType should be a type as Eigen::Matrix or Eigen::Matrix - * Note it should also work for Eigen::Array - */ -template -struct python_list_to_eigen_vector { - python_list_to_eigen_vector() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - static_assert(VectorType::ColsAtCompileTime == 1 || VectorType::RowsAtCompileTime == 1, "Passed a Matrix into a Vector generator"); // Only for vectors conversion - - if (!PySequence_Check(obj_ptr) - || (VectorType::ColsAtCompileTime == 1 && VectorType::RowsAtCompileTime != Eigen::Dynamic - && (PySequence_Size(obj_ptr) != VectorType::RowsAtCompileTime)) // Check Fixed-size vector - || (VectorType::RowsAtCompileTime == 1 && VectorType::ColsAtCompileTime != Eigen::Dynamic - && (PySequence_Size(obj_ptr) != VectorType::ColsAtCompileTime))) // Check Fixed-size row vector - return 0; - - if (VectorType::ColsAtCompileTime == 1 && VectorType::RowsAtCompileTime != Eigen::Dynamic && (PySequence_Size(obj_ptr) != VectorType::RowsAtCompileTime)) - return 0; - - py::list arr = py::extract(obj_ptr); - for (long i = 0; i < py::len(arr); i++) - if (!py::extract(arr[i]).check()) - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - py::list arr = py::extract(obj_ptr); - auto len = py::len(arr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) VectorType; - VectorType& vec = *static_cast(storage); - - if (VectorType::RowsAtCompileTime == Eigen::Dynamic || VectorType::ColsAtCompileTime == Eigen::Dynamic) - vec.resize(len); - - for (long i = 0; i < len; ++i) - vec(i) = py::extract(arr[i]); - - data->convertible = storage; - } -}; - -/** Conversion from a python list to an Eigen Matrix - * This convert list of list of type T (int, float32, float64, ...) to an Eigen::MatrixType - * The template MatrixType should be a type as Eigen::Matrix - * Note it should also work for Eigen::Array - */ -template -struct python_list_to_eigen_matrix { - python_list_to_eigen_matrix() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - static_assert(MatrixType::ColsAtCompileTime != 1 && MatrixType::RowsAtCompileTime != 1, "Passed a Vector into a Matrix generator"); // Only for matrix conversion - - auto checkNestedList = [](const py::list& list) { - py::extract nested_list(list[0]); - if (!nested_list.check()) - return false; - - auto cols = py::len(nested_list()); - for (long i = 1; i < py::len(list); ++i) { - py::extract nested_list(list[i]); - if (!nested_list.check() || py::len(nested_list) != cols) // Check nested list size - return false; - for (long j = 0; j < cols; ++j) - if (!py::extract(nested_list()[j]).check()) // Check list type - return false; - } - - return true; - }; - - py::extract extract_list(obj_ptr); - py::extract extract_nested_list(extract_list()); - if (!extract_list.check() // Check list - || !checkNestedList(extract_list()) // Check nested lists - || (MatrixType::RowsAtCompileTime != Eigen::Dynamic && MatrixType::RowsAtCompileTime != py::len(extract_list())) // Check rows are the same - || (MatrixType::ColsAtCompileTime != Eigen::Dynamic && MatrixType::ColsAtCompileTime != py::len(extract_nested_list()))) // Check cols are the same - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - py::list arr = py::extract(obj_ptr); - auto rows = py::len(arr); - auto cols = py::len(py::extract(arr[0])()); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) MatrixType; - MatrixType& mat = *static_cast(storage); - - // Resize matrix if (half-)dynamic-sized matrix - if (MatrixType::RowsAtCompileTime == Eigen::Dynamic || MatrixType::ColsAtCompileTime == Eigen::Dynamic) - mat.resize(rows, cols); - - for (long i = 0; i < rows; ++i) - for (long j = 0; j < cols; ++j) - mat(i, j) = py::extract(arr[i][j]); - - data->convertible = storage; - } -}; - -/** Conversion from a python list to an Eigen Quaternion - * This convert a list of type T (float32, float64) to an Eigen::Quaternion type - * The template Quaternion type should be a type as e.g. Eigen::Quaternion - */ -template -struct python_list_to_eigen_quaternion { - python_list_to_eigen_quaternion() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - //static_assert(QuaternionType::ColsAtCompileTime == 1 || QuaternionType::RowsAtCompileTime == 1, "Passed a Matrix into a Vector generator"); // Only for vectors conversion - - if (!PySequence_Check(obj_ptr) - || (PySequence_Size(obj_ptr) != 4)) // Check Fixed-size vector - return 0; - - if (PySequence_Size(obj_ptr) != 4) - return 0; - - py::list arr = py::extract(obj_ptr); - for (long i = 0; i < py::len(arr); i++) - if (!py::extract(arr[i]).check()) - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - py::list arr = py::extract(obj_ptr); - auto len = py::len(arr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) QuaternionType; - QuaternionType& vec = *static_cast(storage); - - vec.x() = py::extract(arr[0]); - vec.y() = py::extract(arr[1]); - vec.z() = py::extract(arr[2]); - vec.w() = py::extract(arr[3]); - - data->convertible = storage; - } -}; - -/** Conversion from a python list to an Eigen Transform - * This convert a list of lists of type T (float32, float64) to an Eigen::Transform type - * The template Transform type should be a type as e.g. Eigen::Transform - */ -template -struct python_list_to_eigen_transform { - python_list_to_eigen_transform() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - static_assert(TransformType::Dim != 1, "Passed a Vector into a Matrix generator"); // Only for matrix conversion - - auto checkNestedList = [](const py::list& list) { - py::extract nested_list(list[0]); - if (!nested_list.check()) - return false; - - auto cols = py::len(nested_list()); - for (long i = 1; i < py::len(list); ++i) { - py::extract nested_list(list[i]); - if (!nested_list.check() || py::len(nested_list) != cols) // Check nested list size - return false; - for (long j = 0; j < cols; ++j) - if (!py::extract(nested_list()[j]).check()) // Check list type - return false; - } - - return true; - }; - - py::extract extract_list(obj_ptr); - py::extract extract_nested_list(extract_list()); - if (!extract_list.check() // Check list - || !checkNestedList(extract_list()) // Check nested lists - || (TransformType::Dim != py::len(extract_list())) // Check rows are the same - || (TransformType::Dim != py::len(extract_nested_list()))) // Check cols are the same - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - py::list arr = py::extract(obj_ptr); - auto rows = py::len(arr); - auto cols = py::len(py::extract(arr[0])()); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) TransformType; - TransformType& mat = *static_cast(storage); - - // Resize matrix if (half-)dynamic-sized matrix - //if (TransformType::RowsAtCompileTime == Eigen::Dynamic || TransformType::ColsAtCompileTime == Eigen::Dynamic) - // mat.resize(rows, cols); - - for (long i = 0; i < rows; ++i) - for (long j = 0; j < cols; ++j) - mat(i, j) = py::extract(arr[i][j]); - - data->convertible = storage; - } -}; - - -/* - * ***************** Numpy -> Eigen converters ***************** - */ - -/** Conversion from a numpy ndarray to an Eigen Vector - * This convert numpy.array of dtype T (int, float32, float64, ...) to an Eigen::VectorType - * The template VectorType should be a type as Eigen::Matrix or Eigen::Matrix - * Note it should also work for Eigen::Array - */ -template -struct numpy_array_to_eigen_vector { - numpy_array_to_eigen_vector() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - static_assert(VectorType::RowsAtCompileTime == 1 || VectorType::ColsAtCompileTime == 1, "Passed a Matrix into a Vector generator"); // Check that in c++ side, it is an Eigen vector - py::extract arr(obj_ptr); - if (!arr.check() // Check it is a numpy array - || arr().get_nd() != 1 // Check array dimension (does not allow numpy array of type (1, 3), needs to ravel it first) - || arr().get_dtype() != np::dtype::get_builtin() // Check type - || (VectorType::RowsAtCompileTime == 1 - && VectorType::ColsAtCompileTime != Eigen::Dynamic - && VectorType::ColsAtCompileTime != arr().shape(0)) // Check vector size in case of fixed-size array (for a row-vector) - || (VectorType::ColsAtCompileTime == 1 - && VectorType::RowsAtCompileTime != Eigen::Dynamic - && VectorType::RowsAtCompileTime != arr().shape(0))) // Check vector size in case of fixed-size array (for a column-vector) - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - np::ndarray arr = py::extract(obj_ptr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) VectorType; - VectorType& vec = *static_cast(storage); - // Resize for dynamic-sized matrices - if (VectorType::RowsAtCompileTime == Eigen::Dynamic || VectorType::ColsAtCompileTime == Eigen::Dynamic) - vec.resize(arr.shape(0)); - - // Extract values. The type has been check in the convertible function - for (int i = 0; i < arr.shape(0); ++i) - vec(i) = py::extract(arr[i]); - - data->convertible = storage; - } -}; - -/** Conversion from a numpy ndarray to an Eigen Matrix - * This convert numpy.array of dtype T (int, float32, float64, ...) to an Eigen::MatrixType - * The template MatrixType should be a type as Eigen::Matrix - * Note it should also work for Eigen::Array - */ -template -struct numpy_array_to_eigen_matrix { - numpy_array_to_eigen_matrix() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - static_assert(MatrixType::ColsAtCompileTime != 1 && MatrixType::RowsAtCompileTime != 1, "Passed a Vector into a Matrix generator"); // Only for matrix conversion - - py::extract arr(obj_ptr); - if (!arr.check() // Check it is a numpy array - || arr().get_nd() != 2 // Check array dimension - || arr().get_dtype() != np::dtype::get_builtin() // Check type - || (MatrixType::RowsAtCompileTime != Eigen::Dynamic && MatrixType::RowsAtCompileTime != arr().shape(0)) // Check rows are the same - || (MatrixType::ColsAtCompileTime != Eigen::Dynamic && MatrixType::ColsAtCompileTime != arr().shape(1))) // Check cols are the same - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - np::ndarray arr = py::extract(obj_ptr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) MatrixType; - MatrixType& mat = *static_cast(storage); - // Resize for dynamic-sized matrices - // For half dynamic-sized matrices, the fixed-size part has been check in the convertible function - if (MatrixType::RowsAtCompileTime == Eigen::Dynamic || MatrixType::ColsAtCompileTime == Eigen::Dynamic) - mat.resize(arr.shape(0), arr.shape(1)); - - // Extract values. The type has been check in the convertible function - for (int i = 0; i < arr.shape(0); ++i) - for (int j = 0; j < arr.shape(1); ++j) - mat(i, j) = py::extract(arr[i][j]); - - data->convertible = storage; - } -}; - -template -struct numpy_array_to_eigen_quaternion{ - numpy_array_to_eigen_quaternion() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - //static_assert(QuaternionType::RowsAtCompileTime == 1 || QuaternionType::ColsAtCompileTime == 1, "Passed a Matrix into a Vector generator"); // Check that in c++ side, it is an Eigen vector - py::extract arr(obj_ptr); - if (!arr.check() // Check it is a numpy array - || arr().get_nd() != 1 // Check array dimension (does not allow numpy array of type (1, 3), needs to ravel it first) - || arr().get_dtype() != np::dtype::get_builtin() // Check type - || 4 != arr().shape(0) // Check vector size in case of fixed-size array (for a row-vector) - || 4 != arr().shape(0)) // Check vector size in case of fixed-size array (for a column-vector) - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - np::ndarray arr = py::extract(obj_ptr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) QuaternionType; - QuaternionType& vec = *static_cast(storage); - // Resize for dynamic-sized matrices - //if (QuaternionType::RowsAtCompileTime == Eigen::Dynamic || QuaternionType::ColsAtCompileTime == Eigen::Dynamic) - // vec.resize(arr.shape(0)); - - // Extract values. The type has been check in the convertible function - vec.x() = py::extract(arr[0]); - vec.y() = py::extract(arr[1]); - vec.z() = py::extract(arr[2]); - vec.w() = py::extract(arr[3]); - - data->convertible = storage; - } -}; - -template -struct numpy_array_to_eigen_transform { - numpy_array_to_eigen_transform() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - static_assert(TransformType::Dim != 1, "Passed a Vector into a Matrix generator"); // Only for matrix conversion - - py::extract arr(obj_ptr); - if (!arr.check() // Check it is a numpy array - || arr().get_nd() != 2 // Check array dimension - || arr().get_dtype() != np::dtype::get_builtin() // Check type - || TransformType::Dim != arr().shape(0) // Check rows are the same - || TransformType::Dim != arr().shape(1)) // Check cols are the same - return 0; - - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - np::ndarray arr = py::extract(obj_ptr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) TransformType; - TransformType& mat = *static_cast(storage); - // Resize for dynamic-sized matrices - // For half dynamic-sized matrices, the fixed-size part has been check in the convertible function - //if (TransformType::Dim == Eigen::Dynamic || TransformType::Dim == Eigen::Dynamic) - // mat.resize(arr.shape(0), arr.shape(1)); - - // Extract values. The type has been check in the convertible function - for (int i = 0; i < arr.shape(0); ++i) - for (int j = 0; j < arr.shape(1); ++j) - mat(i, j) = py::extract(arr[i][j]); - - data->convertible = storage; - } -}; - -/* - * ****************** Eigen -> Numpy converters ****************** - */ - -/** Conversion from an Eigen Vector to an numpy ndarray - * This convert Eigen::VectorType of type T (int, float, double, ...) to an numpy.array - * The template VectorType should be a type as Eigen::Matrix or Eigen::Matrix - * Note it should also work for Eigen::Array - */ -template -struct eigen_vector_to_numpy_array { - static PyObject* convert(const VectorType& mat) - { - static_assert(VectorType::ColsAtCompileTime == 1 || VectorType::RowsAtCompileTime == 1, "Passed a Matrix into a Vector generator"); // Ensure that it is a vector - - np::dtype dt = np::dtype::get_builtin(); - auto shape = py::make_tuple(mat.size()); - np::ndarray mOut = np::empty(shape, dt); - - for (Eigen::Index i = 0; i < mat.size(); ++i) - mOut[i] = mat(i); - - return py::incref(mOut.ptr()); - } -}; - -/** Conversion from an Eigen Matrix to an numpy ndarray - * This convert Eigen::MatrixType of type T (int, float, double, ...) to an numpy.array - * The template MatrixType should be a type as Eigen::Matrix - * Note it should also work for Eigen::Array - */ -template -struct eigen_matrix_to_numpy_array { - static PyObject* convert(const MatrixType& mat) - { - static_assert(MatrixType::ColsAtCompileTime != 1 && MatrixType::RowsAtCompileTime != 1, "Passed a Vector into a Matrix generator"); // Ensure that it is not a vector - - np::dtype dt = np::dtype::get_builtin(); - auto shape = py::make_tuple(mat.rows(), mat.cols()); - np::ndarray mOut = np::empty(shape, dt); - - for (Eigen::Index i = 0; i < mat.rows(); ++i) - for (Eigen::Index j = 0; j < mat.cols(); ++j) - mOut[i][j] = mat(i, j); - - return py::incref(mOut.ptr()); - } -}; - -/** Conversion from an Eigen Quaternion to an numpy ndarray - * This converts Eigen::Quaternion of type T (float, double) to an numpy.array. - * Order is [im_x,im_y,im_z,re]! - * The template QauternionType should be a type as Eigen::Quaternion - */ -template -struct eigen_quaternion_to_numpy_array { - static PyObject* convert(const QuaternionType& mat) - { - //static_assert(VectorType::ColsAtCompileTime == 1 || VectorType::RowsAtCompileTime == 1, "Passed a Matrix into a Vector generator"); // Ensure that it is a vector - - np::dtype dt = np::dtype::get_builtin(); - auto shape = py::make_tuple(4); - np::ndarray mOut = np::empty(shape, dt); - - for (Eigen::Index i = 0; i < 4; ++i) - mOut[i] = mat.coeffs()[i]; - - return py::incref(mOut.ptr()); - } -}; - -/** Conversion from an Eigen Transform to an numpy matrix - * This converts Eigen Transform of type T (float, double) to an numpy.matrix. - * The template TransformType should be a type as Eigen::Transform - */ -template -struct eigen_transform_to_numpy_array { - static PyObject* convert(const TransformType& mat) - { - static_assert(TransformType::Dim != 1, "Passed a Vector into a Matrix generator"); // Ensure that it is not a vector - - np::dtype dt = np::dtype::get_builtin(); - auto shape = py::make_tuple(mat.rows(), mat.cols()); - np::ndarray mOut = np::empty(shape, dt); - - for (Eigen::Index i = 0; i < mat.rows(); ++i) - for (Eigen::Index j = 0; j < mat.cols(); ++j) - mOut[i][j] = mat(i, j); - - return py::incref(mOut.ptr()); - } -}; - -/* - * **************************************** Main converters ******************************************* - */ - -/** Eigen Matrix and Array conversion - * Generate the conversion for Eigen MatrixType (MatrixXd, Array2Xf, ...) - * \param isListConvertible if true, generate conversion from python list to Eigen MatrixType - */ -template -void convertMatrix(bool isListConvertible = true) -{ - // python -> eigen - numpy_array_to_eigen_matrix(); - - // list -> eigen - if (isListConvertible) - python_list_to_eigen_matrix(); - - // eigen -> python - py::to_python_converter >(); // Vector2 -} - -/** Eigen quaternion conversion - * Generate the conversion for Eigen quaternion type (Quaterniond, Quaternionf) - * \param isListConvertible if true, generate conversion from python list to Eigen quaternion type - */ -template -void convertQuaternion(bool isListConvertible = true) -{ - // python -> eigen - numpy_array_to_eigen_quaternion(); - - // list -> eigen - if (isListConvertible) - python_list_to_eigen_quaternion(); - - // eigen -> python - py::to_python_converter >(); -} - -/** Eigen Transform conversion - * Generate the conversion for Eigen Transform (Affine2d, Affine3d, ...) - * \param isListConvertible if true, generate conversion from python list to Eigen Transform - */ -template -void convertTransform(bool isListConvertible = true) -{ - // python -> eigen - numpy_array_to_eigen_transform(); - - // list -> eigen - if (isListConvertible) - python_list_to_eigen_transform(); - - // eigen -> python - py::to_python_converter >(); // Vector2 -} - -/** Eigen Vector and Array conversion - * Generate the conversion for Eigen VectorType (VectorXd, RowVector4f, ArrayXf, ...) - * \param isListConvertible if true, generate conversion from python list to Eigen VectorType - */ -template -void convertVector(bool isListConvertible = true) -{ - // python -> eigen - numpy_array_to_eigen_vector(); - - // list -> eigen - if (isListConvertible) - python_list_to_eigen_vector(); - - // eigen -> python - py::to_python_converter >(); -} - -} // namespace pygen diff --git a/bindings/python/examples/kuka_iiwa/cart_pos_ctrl_hls_solver.py b/bindings/python/examples/kuka_iiwa/cart_pos_ctrl_hls_solver.py deleted file mode 100644 index a0ad334b..00000000 --- a/bindings/python/examples/kuka_iiwa/cart_pos_ctrl_hls_solver.py +++ /dev/null @@ -1,88 +0,0 @@ -from wbc.core import * -from wbc.robot_models.robot_model_rbdl import RobotModelRBDL -from wbc.scenes.velocity_scene import VelocityScene -from wbc.solvers.hls_solver import HierarchicalLSSolver -from wbc.controllers import CartesianPosPDController -import time -import numpy as np - -# Configure robot model -robot_model=RobotModelRBDL() -r=RobotModelConfig() -r.file="../../../../models/kuka/urdf/kuka_iiwa.urdf" -r.actuated_joint_names = ["kuka_lbr_l_joint_1", "kuka_lbr_l_joint_2", "kuka_lbr_l_joint_3", "kuka_lbr_l_joint_4", "kuka_lbr_l_joint_5", "kuka_lbr_l_joint_6", "kuka_lbr_l_joint_7"] -r.joint_names = r.actuated_joint_names -if robot_model.configure(r) == False: - print("Failed to configure robot model ") - exit(0) - -# Create solver -solver = HierarchicalLSSolver() -solver.setMaxSolverOutputNorm(10) - -# Set up Tasks: Only a single, Cartesian positioning task -cfg = TaskConfig() -cfg.name = "tcp_pose" -cfg.root = "kuka_lbr_l_link_0" -cfg.tip = "kuka_lbr_l_tcp" -cfg.ref_frame = "kuka_lbr_l_link_0" -cfg.priority = 0 -cfg.activation = 1 -cfg.type = TaskType.cart -cfg.weights = [1]*6 - -# Configure WBC Scene -scene=VelocityScene(robot_model, solver, 0.001) -if scene.configure([cfg]) == False: - print("Failed to configure scene") - exit(0) - -# Configure Cartesian position controller -ctrl = CartesianPosPDController() -ctrl.setPGain([3]*6) - -# Target Pose -setpoint = RigidBodyStateSE3() -setpoint.pose.position = [0.0,0.0,0.8] -setpoint.pose.orientation = [0,0,0,1] - -# Actual pose -feedback = RigidBodyStateSE3() -feedback.pose.position = [0.0,0.0,0.0] -feedback.pose.orientation = [0,0,0,1] -control_output = RigidBodyStateSE3() - -# Initial joint state -joint_state = Joints() -js = JointState() -js.position = 0.1 -joint_state.elements = [js]*robot_model.noOfJoints() -joint_state.names = robot_model.jointNames() - -start = time.time() - -# Control Loop -sample_time = 0.01 -while np.linalg.norm(setpoint.pose.position-feedback.pose.position) > 1e-4: - - robot_model.update(joint_state) - - feedback = robot_model.rigidBodyState(cfg.root, cfg.tip) - control_output = ctrl.update(setpoint,feedback) - - scene.setReference(cfg.name,control_output) - qp = scene.update() - solver_output = scene.solve(qp) - - # We have to update the joint state in this unintuitive way for now, since the [] operator for the Joints-type is not exposed to python yet.... - js = joint_state.elements - for i in range(robot_model.noOfJoints()): - js[i].position = js[i].position + solver_output.elements[i].speed * sample_time - joint_state.elements = js - - print("Time: [" + str(time.time()-start) + " sec]") - print("Ref. Pos: %.4f %.4f %.4f" % (setpoint.pose.position[0],setpoint.pose.position[1],setpoint.pose.position[2])) - print("Act. Pos: %.4f %.4f %.4f" % (feedback.pose.position[0],feedback.pose.position[1],feedback.pose.position[2])) - print("Ctrl. Out: %.4f %.4f %.4f" % (control_output.twist.linear[0],control_output.twist.linear[1],control_output.twist.linear[2])) - print("-------------------------------------------") - time.sleep(sample_time) diff --git a/bindings/python/examples/rh5/rh5_legs_floating_base.py b/bindings/python/examples/rh5/rh5_legs_floating_base.py deleted file mode 100644 index 57a4f616..00000000 --- a/bindings/python/examples/rh5/rh5_legs_floating_base.py +++ /dev/null @@ -1,101 +0,0 @@ -from wbc.core import * -from wbc.robot_models.robot_model_rbdl import RobotModelRBDL -from wbc.scenes.acceleration_scene_tsid import AccelerationSceneTSID -from wbc.solvers.qpoases_solver import QPOASESSolver -from wbc.controllers import CartesianPosPDController -import time -import numpy as np - -# Configure robot model -floating_base_state = RigidBodyStateSE3() -floating_base_state.pose.position = [-0.03, 0.0, 0.9] -floating_base_state.pose.orientation = [0,0,0,1] -floating_base_state.twist.linear = floating_base_state.twist.angular = [0,0,0] -floating_base_state.acceleration.linear = floating_base_state.acceleration.angular = [0,0,0] - -robot_model=RobotModelRBDL() -r=RobotModelConfig() -r.file="../../../../models/rh5/urdf/rh5_legs.urdf" -r.floating_base = True -contacts = ActiveContacts() -contacts.names = ["FL_SupportCenter", "FR_SupportCenter"] -a = ActiveContact() -a.mu = 0.6 -a.active = 1 -contacts.elements = [a,a] -r.contact_points = contacts -if robot_model.configure(r) == False: - print("Failed to configure robot model") - exit(0) - -# Create solver -solver = QPOASESSolver() -solver.setMaxNoWSR(100) - -# Set up Tasks: Only a single, Cartesian positioning task -cfg = TaskConfig() -cfg.name = "zero_com_acceleration" -cfg.root = "world" -cfg.tip = "RH5_Root_Link" -cfg.ref_frame = "world" -cfg.priority = 0 -cfg.activation = 1 -cfg.type = TaskType.cart -cfg.weights = [1]*6 - -# Configure WBC Scene -scene=AccelerationSceneTSID(robot_model, solver, 0.001) -if scene.configure([cfg]) == False: - print("Failed to configure scene") - exit(0) - -# Configure Cartesian position controller -ctrl = CartesianPosPDController() -ctrl.setPGain([3]*6) -ctrl.setDGain([3]*6) -ctrl.setFFGain([1]*6) - -# Target Pose -setpoint = RigidBodyStateSE3() -setpoint.pose.position = [-0.03, 0.0, 0.9] -setpoint.pose.orientation = [0,0,0,1] - -# Actual pose -feedback = RigidBodyStateSE3() -feedback.pose.position = [-0.0, 0.0, 0.0] -feedback.pose.orientation = [0,0,0,1] -control_output = RigidBodyStateSE3() - -# Initial joint state -joint_state = Joints() -init_pos = [0,0,-0.2,0.4,0,-0.2, - 0,0,-0.2,0.4,0,-0.2] -# We have to update the joint state in this unintuitive way for now, since the [] operator for the Joints-type is not exposed to python yet.... -elements = [] -for i in init_pos: - js = JointState() - js.position = i - js.speed = 0 - js.acceleration = 0 - elements.append(js) -joint_state.elements = elements -joint_state.names = robot_model.actuatedJointNames() - -floating_base_state.time.microseconds=int(round(time.time()*1e6)) -robot_model.update(joint_state,floating_base_state) - -feedback = robot_model.rigidBodyState(cfg.root, cfg.tip) -control_output = ctrl.update(setpoint,feedback) - -scene.setReference(cfg.name,control_output) -qp = scene.update() -solver_output = scene.solve(qp) -#wrenches_output = scene.getContactWrenches() - -print("----- Solver output -----") -print("Names: " + str(robot_model.actuatedJointNames())) -print("Acc: " + str([e.acceleration for e in solver_output.elements])) -print("Tau: " + str([e.effort for e in solver_output.elements])) -#print("Contact Wrenches") -#for name,elem in zip(wrenches_output.names, wrenches_output.elements): -# print(name + ": Force " + str(elem.force.transpose()) + ", Torque " + str(elem.torque.transpose())) diff --git a/bindings/python/robot_models/CMakeLists.txt b/bindings/python/robot_models/CMakeLists.txt deleted file mode 100644 index 742fc0f6..00000000 --- a/bindings/python/robot_models/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ - -add_subdirectory(rbdl) -if(USE_HYRODYN) - add_subdirectory(hyrodyn) -endif() -if(USE_KDL) - add_subdirectory(kdl) -endif() -if(USE_PINOCCHIO) - add_subdirectory(pinocchio) -endif() - -install(FILES __init__.py - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/robot_models) diff --git a/bindings/python/robot_models/__init__.py b/bindings/python/robot_models/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/bindings/python/robot_models/hyrodyn/CMakeLists.txt b/bindings/python/robot_models/hyrodyn/CMakeLists.txt deleted file mode 100644 index 6a5b8e99..00000000 --- a/bindings/python/robot_models/hyrodyn/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -PYTHON_ADD_MODULE(robot_model_hyrodyn robot_model_hyrodyn.cpp) -target_link_libraries(robot_model_hyrodyn PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-robot_models-hyrodyn) - -target_include_directories(robot_model_hyrodyn PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS robot_model_hyrodyn - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/robot_models) diff --git a/bindings/python/robot_models/hyrodyn/robot_model_hyrodyn.cpp b/bindings/python/robot_models/hyrodyn/robot_model_hyrodyn.cpp deleted file mode 100644 index 51283c64..00000000 --- a/bindings/python/robot_models/hyrodyn/robot_model_hyrodyn.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "robot_model_hyrodyn.hpp" - -namespace wbc_py { - -bool RobotModelHyrodyn::configure(const wbc_py::RobotModelConfig &cfg){ - return wbc::RobotModelHyrodyn::configure(toRobotModelConfig(cfg)); -} -void RobotModelHyrodyn::update(const base::NamedVector &joint_state){ - wbc::RobotModelHyrodyn::update(tobaseSamplesJoints(joint_state)); -} -void RobotModelHyrodyn::update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state){ - wbc::RobotModelHyrodyn::update(tobaseSamplesJoints(joint_state), floating_base_state); -} -base::NamedVector RobotModelHyrodyn::jointState2(const std::vector &names){ - return toNamedVector(wbc::RobotModelHyrodyn::jointState(names)); -} -void RobotModelHyrodyn::setActiveContacts(const base::NamedVector & active_contacts){ - wbc::RobotModelHyrodyn::setActiveContacts(toActiveContacts(active_contacts)); -} -base::NamedVector RobotModelHyrodyn::getActiveContacts2(){ - return fromActiveContacts(wbc::RobotModelHyrodyn::getActiveContacts()); -} -base::NamedVector RobotModelHyrodyn::jointLimits2(){ - return fromJointLimits(wbc::RobotModelHyrodyn::jointLimits()); -} -wbc_py::RobotModelConfig RobotModelHyrodyn::getRobotModelConfig(){ - return fromRobotModelConfig(wbc::RobotModelHyrodyn::getRobotModelConfig()); -} - - -} - -BOOST_PYTHON_MODULE(robot_model_hyrodyn){ - - np::initialize(); - - py::class_("RobotModelHyrodyn") - .def("configure", &wbc_py::RobotModelHyrodyn::configure) - .def("update", &wbc_py::RobotModelHyrodyn::update) - .def("update", &wbc_py::RobotModelHyrodyn::update2) - .def("jointState", &wbc_py::RobotModelHyrodyn::jointState2) - .def("rigidBodyState", &wbc_py::RobotModelHyrodyn::rigidBodyState, py::return_value_policy()) - .def("spaceJacobian", &wbc_py::RobotModelHyrodyn::spaceJacobian, py::return_value_policy()) - .def("bodyJacobian", &wbc_py::RobotModelHyrodyn::spaceJacobian, py::return_value_policy()) - .def("spatialAccelerationBias", &wbc_py::RobotModelHyrodyn::spatialAccelerationBias, py::return_value_policy()) - .def("jacobianDot", &wbc_py::RobotModelHyrodyn::jacobianDot, py::return_value_policy()) - .def("jointSpaceInertiaMatrix", &wbc_py::RobotModelHyrodyn::jointSpaceInertiaMatrix, py::return_value_policy()) - .def("biasForces", &wbc_py::RobotModelHyrodyn::biasForces, py::return_value_policy()) - .def("jointNames", &wbc_py::RobotModelHyrodyn::jointNames, py::return_value_policy()) - .def("actuatedJointNames", &wbc_py::RobotModelHyrodyn::actuatedJointNames, py::return_value_policy()) - .def("independentJointNames", &wbc_py::RobotModelHyrodyn::independentJointNames, py::return_value_policy()) - .def("jointIndex", &wbc_py::RobotModelHyrodyn::jointIndex) - .def("baseFrame", &wbc_py::RobotModelHyrodyn::baseFrame, py::return_value_policy()) - .def("jointLimits", &wbc_py::RobotModelHyrodyn::jointLimits2) - .def("selectionMatrix", &wbc_py::RobotModelHyrodyn::selectionMatrix, py::return_value_policy()) - .def("hasLink", &wbc_py::RobotModelHyrodyn::hasLink) - .def("hasJoint", &wbc_py::RobotModelHyrodyn::hasJoint) - .def("hasActuatedJoint", &wbc_py::RobotModelHyrodyn::hasActuatedJoint) - .def("centerOfMass", &wbc_py::RobotModelHyrodyn::centerOfMass, py::return_value_policy()) - .def("setActiveContacts", &wbc_py::RobotModelHyrodyn::setActiveContacts) - .def("getActiveContacts", &wbc_py::RobotModelHyrodyn::getActiveContacts2) - .def("noOfJoints", &wbc_py::RobotModelHyrodyn::noOfJoints) - .def("noOfActuatedJoints", &wbc_py::RobotModelHyrodyn::noOfActuatedJoints) - .def("setGravityVector", &wbc_py::RobotModelHyrodyn::setGravityVector) - .def("floatingBaseState", &wbc_py::RobotModelHyrodyn::floatingBaseState, py::return_value_policy()) - .def("getRobotModelConfig", &wbc_py::RobotModelHyrodyn::getRobotModelConfig); - -} - - diff --git a/bindings/python/robot_models/hyrodyn/robot_model_hyrodyn.hpp b/bindings/python/robot_models/hyrodyn/robot_model_hyrodyn.hpp deleted file mode 100644 index fcd84c2f..00000000 --- a/bindings/python/robot_models/hyrodyn/robot_model_hyrodyn.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef WBC_PY_ROBOT_MODEL_HYRODYN_HPP -#define WBC_PY_ROBOT_MODEL_HYRODYN_HPP - -#include "robot_models/hyrodyn/RobotModelHyrodyn.hpp" -#include "../../wbc_types_conversions.h" - -namespace wbc_py { - -/** Wrapper for wbc::RobotModelHyrodyn. Unfortunately we have to do this, since base::samples::Joints and other Rock types cannot be easily exposed to python*/ -class RobotModelHyrodyn : public wbc::RobotModelHyrodyn{ -public: - bool configure(const wbc_py::RobotModelConfig &cfg); - void update(const base::NamedVector &joint_state); - void update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state); - base::NamedVector jointState2(const std::vector &names); - void setActiveContacts(const base::NamedVector & active_contacts); - base::NamedVector getActiveContacts2(); - base::NamedVector jointLimits2(); - wbc_py::RobotModelConfig getRobotModelConfig(); -}; - -} - -#endif - diff --git a/bindings/python/robot_models/kdl/CMakeLists.txt b/bindings/python/robot_models/kdl/CMakeLists.txt deleted file mode 100644 index d26ae2ac..00000000 --- a/bindings/python/robot_models/kdl/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -PYTHON_ADD_MODULE(robot_model_kdl robot_model_kdl.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(robot_model_kdl PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-robot_models-kdl) - -target_include_directories(robot_model_kdl PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS robot_model_kdl - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/robot_models) diff --git a/bindings/python/robot_models/kdl/robot_model_kdl.cpp b/bindings/python/robot_models/kdl/robot_model_kdl.cpp deleted file mode 100644 index fea5789b..00000000 --- a/bindings/python/robot_models/kdl/robot_model_kdl.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "../../wbc_types_conversions.h" -#include "robot_model_kdl.hpp" - -namespace wbc_py { - -bool RobotModelKDL::configure(const wbc_py::RobotModelConfig &cfg){ - return wbc::RobotModelKDL::configure(toRobotModelConfig(cfg)); -} -void RobotModelKDL::update(const base::NamedVector &joint_state){ - wbc::RobotModelKDL::update(tobaseSamplesJoints(joint_state)); -} -void RobotModelKDL::update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state){ - wbc::RobotModelKDL::update(tobaseSamplesJoints(joint_state), floating_base_state); -} -base::NamedVector RobotModelKDL::jointState2(const std::vector &names){ - return toNamedVector(wbc::RobotModelKDL::jointState(names)); -} -void RobotModelKDL::setActiveContacts(const base::NamedVector & active_contacts){ - wbc::RobotModelKDL::setActiveContacts(toActiveContacts(active_contacts)); -} -base::NamedVector RobotModelKDL::getActiveContacts2(){ - return fromActiveContacts(wbc::RobotModelKDL::getActiveContacts()); -} -base::NamedVector RobotModelKDL::jointLimits2(){ - return fromJointLimits(wbc::RobotModelKDL::jointLimits()); -} -wbc_py::RobotModelConfig RobotModelKDL::getRobotModelConfig(){ - return fromRobotModelConfig(wbc::RobotModelKDL::getRobotModelConfig()); -} - -} - -BOOST_PYTHON_MODULE(robot_model_kdl){ - - np::initialize(); - - py::class_("RobotModelKDL") - .def("configure", &wbc_py::RobotModelKDL::configure) - .def("update", &wbc_py::RobotModelKDL::update) - .def("update", &wbc_py::RobotModelKDL::update2) - .def("jointState", &wbc_py::RobotModelKDL::jointState2) - .def("rigidBodyState", &wbc_py::RobotModelKDL::rigidBodyState, py::return_value_policy()) - .def("spaceJacobian", &wbc_py::RobotModelKDL::spaceJacobian, py::return_value_policy()) - .def("bodyJacobian", &wbc_py::RobotModelKDL::spaceJacobian, py::return_value_policy()) - .def("spatialAccelerationBias", &wbc_py::RobotModelKDL::spatialAccelerationBias, py::return_value_policy()) - .def("jacobianDot", &wbc_py::RobotModelKDL::jacobianDot, py::return_value_policy()) - .def("jointSpaceInertiaMatrix", &wbc_py::RobotModelKDL::jointSpaceInertiaMatrix, py::return_value_policy()) - .def("biasForces", &wbc_py::RobotModelKDL::biasForces, py::return_value_policy()) - .def("jointNames", &wbc_py::RobotModelKDL::jointNames, py::return_value_policy()) - .def("actuatedJointNames", &wbc_py::RobotModelKDL::actuatedJointNames, py::return_value_policy()) - .def("independentJointNames", &wbc_py::RobotModelKDL::independentJointNames, py::return_value_policy()) - .def("jointIndex", &wbc_py::RobotModelKDL::jointIndex) - .def("baseFrame", &wbc_py::RobotModelKDL::baseFrame, py::return_value_policy()) - .def("jointLimits", &wbc_py::RobotModelKDL::jointLimits2) - .def("selectionMatrix", &wbc_py::RobotModelKDL::selectionMatrix, py::return_value_policy()) - .def("hasLink", &wbc_py::RobotModelKDL::hasLink) - .def("hasJoint", &wbc_py::RobotModelKDL::hasJoint) - .def("hasActuatedJoint", &wbc_py::RobotModelKDL::hasActuatedJoint) - .def("centerOfMass", &wbc_py::RobotModelKDL::centerOfMass, py::return_value_policy()) - .def("setActiveContacts", &wbc_py::RobotModelKDL::setActiveContacts) - .def("getActiveContacts", &wbc_py::RobotModelKDL::getActiveContacts2) - .def("noOfJoints", &wbc_py::RobotModelKDL::noOfJoints) - .def("noOfActuatedJoints", &wbc_py::RobotModelKDL::noOfActuatedJoints) - .def("setGravityVector", &wbc_py::RobotModelKDL::setGravityVector) - .def("floatingBaseState", &wbc_py::RobotModelKDL::floatingBaseState, py::return_value_policy()) - .def("getRobotModelConfig", &wbc_py::RobotModelKDL::getRobotModelConfig); -} - - diff --git a/bindings/python/robot_models/kdl/robot_model_kdl.hpp b/bindings/python/robot_models/kdl/robot_model_kdl.hpp deleted file mode 100644 index 990f35d6..00000000 --- a/bindings/python/robot_models/kdl/robot_model_kdl.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef WBC_PY_ROBOT_MODEL_KDL_HPP -#define WBC_PY_ROBOT_MODEL_KDL_HPP - -#include "robot_models/kdl/RobotModelKDL.hpp" -#include "../../wbc_types_conversions.h" - -namespace wbc_py { - -/** Wrapper for wbc::RobotModelKDL. Unfortunately we have to do this, since base::samples::Joints and other Rock types cannot be easily exposed to python*/ -class RobotModelKDL : public wbc::RobotModelKDL{ -public: - bool configure(const wbc_py::RobotModelConfig &cfg); - void update(const base::NamedVector &joint_state); - void update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state); - base::NamedVector jointState2(const std::vector &names); - void setActiveContacts(const base::NamedVector & active_contacts); - base::NamedVector getActiveContacts2(); - base::NamedVector jointLimits2(); - wbc_py::RobotModelConfig getRobotModelConfig(); -}; - -} - -#endif - diff --git a/bindings/python/robot_models/pinocchio/CMakeLists.txt b/bindings/python/robot_models/pinocchio/CMakeLists.txt deleted file mode 100644 index f33e09fa..00000000 --- a/bindings/python/robot_models/pinocchio/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -PYTHON_ADD_MODULE(robot_model_pinocchio robot_model_pinocchio.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(robot_model_pinocchio PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-robot_models-pinocchio) - -target_include_directories(robot_model_pinocchio PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS robot_model_pinocchio - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/robot_models) diff --git a/bindings/python/robot_models/pinocchio/robot_model_pinocchio.cpp b/bindings/python/robot_models/pinocchio/robot_model_pinocchio.cpp deleted file mode 100644 index 1b2f2fee..00000000 --- a/bindings/python/robot_models/pinocchio/robot_model_pinocchio.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "robot_model_pinocchio.hpp" -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "../../wbc_types_conversions.h" - -namespace wbc_py { - -bool RobotModelPinocchio::configure(const wbc_py::RobotModelConfig &cfg){ - return wbc::RobotModelPinocchio::configure(toRobotModelConfig(cfg)); -} -void RobotModelPinocchio::update(const base::NamedVector &joint_state){ - wbc::RobotModelPinocchio::update(tobaseSamplesJoints(joint_state)); -} -void RobotModelPinocchio::update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state){ - wbc::RobotModelPinocchio::update(tobaseSamplesJoints(joint_state), floating_base_state); -} -base::NamedVector RobotModelPinocchio::jointState2(const std::vector &names){ - return toNamedVector(wbc::RobotModelPinocchio::jointState(names)); -} -void RobotModelPinocchio::setActiveContacts(const base::NamedVector & active_contacts){ - wbc::RobotModelPinocchio::setActiveContacts(toActiveContacts(active_contacts)); -} -base::NamedVector RobotModelPinocchio::getActiveContacts2(){ - return fromActiveContacts(wbc::RobotModelPinocchio::getActiveContacts()); -} -base::NamedVector RobotModelPinocchio::jointLimits2(){ - return fromJointLimits(wbc::RobotModelPinocchio::jointLimits()); -} -wbc_py::RobotModelConfig RobotModelPinocchio::getRobotModelConfig(){ - return fromRobotModelConfig(wbc::RobotModelPinocchio::getRobotModelConfig()); -} - -} - -BOOST_PYTHON_MODULE(robot_model_pinocchio){ - - np::initialize(); - - py::class_("RobotModelPinocchio") - .def("configure", &wbc_py::RobotModelPinocchio::configure) - .def("update", &wbc_py::RobotModelPinocchio::update) - .def("update", &wbc_py::RobotModelPinocchio::update2) - .def("jointState", &wbc_py::RobotModelPinocchio::jointState2) - .def("rigidBodyState", &wbc_py::RobotModelPinocchio::rigidBodyState, py::return_value_policy()) - .def("spaceJacobian", &wbc_py::RobotModelPinocchio::spaceJacobian, py::return_value_policy()) - .def("bodyJacobian", &wbc_py::RobotModelPinocchio::spaceJacobian, py::return_value_policy()) - .def("spatialAccelerationBias", &wbc_py::RobotModelPinocchio::spatialAccelerationBias, py::return_value_policy()) - .def("jacobianDot", &wbc_py::RobotModelPinocchio::jacobianDot, py::return_value_policy()) - .def("jointSpaceInertiaMatrix", &wbc_py::RobotModelPinocchio::jointSpaceInertiaMatrix, py::return_value_policy()) - .def("biasForces", &wbc_py::RobotModelPinocchio::biasForces, py::return_value_policy()) - .def("jointNames", &wbc_py::RobotModelPinocchio::jointNames, py::return_value_policy()) - .def("actuatedJointNames", &wbc_py::RobotModelPinocchio::actuatedJointNames, py::return_value_policy()) - .def("independentJointNames", &wbc_py::RobotModelPinocchio::independentJointNames, py::return_value_policy()) - .def("jointIndex", &wbc_py::RobotModelPinocchio::jointIndex) - .def("baseFrame", &wbc_py::RobotModelPinocchio::baseFrame, py::return_value_policy()) - .def("jointLimits", &wbc_py::RobotModelPinocchio::jointLimits2) - .def("selectionMatrix", &wbc_py::RobotModelPinocchio::selectionMatrix, py::return_value_policy()) - .def("hasLink", &wbc_py::RobotModelPinocchio::hasLink) - .def("hasJoint", &wbc_py::RobotModelPinocchio::hasJoint) - .def("hasActuatedJoint", &wbc_py::RobotModelPinocchio::hasActuatedJoint) - .def("centerOfMass", &wbc_py::RobotModelPinocchio::centerOfMass, py::return_value_policy()) - .def("setActiveContacts", &wbc_py::RobotModelPinocchio::setActiveContacts) - .def("getActiveContacts", &wbc_py::RobotModelPinocchio::getActiveContacts2) - .def("noOfJoints", &wbc_py::RobotModelPinocchio::noOfJoints) - .def("noOfActuatedJoints", &wbc_py::RobotModelPinocchio::noOfActuatedJoints) - .def("setGravityVector", &wbc_py::RobotModelPinocchio::setGravityVector) - .def("floatingBaseState", &wbc_py::RobotModelPinocchio::floatingBaseState, py::return_value_policy()) - .def("getRobotModelConfig", &wbc_py::RobotModelPinocchio::getRobotModelConfig); -} - - diff --git a/bindings/python/robot_models/pinocchio/robot_model_pinocchio.hpp b/bindings/python/robot_models/pinocchio/robot_model_pinocchio.hpp deleted file mode 100644 index 6aabd29e..00000000 --- a/bindings/python/robot_models/pinocchio/robot_model_pinocchio.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef WBC_PY_ROBOT_MODEL_PINOCCHIO_HPP -#define WBC_PY_ROBOT_MODEL_PINOCCHIO_HPP - -#include "robot_models/pinocchio/RobotModelPinocchio.hpp" -#include "../../wbc_types_conversions.h" - -namespace wbc_py { - -/** Wrapper for wbc::RobotModelPinocchio. Unfortunately we have to do this, since base::samples::Joints and other Rock types cannot be easily exposed to python*/ -class RobotModelPinocchio : public wbc::RobotModelPinocchio{ -public: - bool configure(const wbc_py::RobotModelConfig &cfg); - void update(const base::NamedVector &joint_state); - void update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state); - base::NamedVector jointState2(const std::vector &names); - void setActiveContacts(const base::NamedVector & active_contacts); - base::NamedVector getActiveContacts2(); - base::NamedVector jointLimits2(); - wbc_py::RobotModelConfig getRobotModelConfig(); -}; - -} - -#endif - diff --git a/bindings/python/robot_models/rbdl/CMakeLists.txt b/bindings/python/robot_models/rbdl/CMakeLists.txt deleted file mode 100644 index 14cbe51e..00000000 --- a/bindings/python/robot_models/rbdl/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -PYTHON_ADD_MODULE(robot_model_rbdl robot_model_rbdl.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(robot_model_rbdl PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-robot_models-rbdl) - -target_include_directories(robot_model_rbdl PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS robot_model_rbdl - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/robot_models) diff --git a/bindings/python/robot_models/rbdl/robot_model_rbdl.cpp b/bindings/python/robot_models/rbdl/robot_model_rbdl.cpp deleted file mode 100644 index 9b98d602..00000000 --- a/bindings/python/robot_models/rbdl/robot_model_rbdl.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "../../wbc_types_conversions.h" -#include "robot_model_rbdl.hpp" - -namespace wbc_py { - -bool RobotModelRBDL::configure(const wbc_py::RobotModelConfig &cfg){ - return wbc::RobotModelRBDL::configure(toRobotModelConfig(cfg)); -} -void RobotModelRBDL::update(const base::NamedVector &joint_state){ - wbc::RobotModelRBDL::update(tobaseSamplesJoints(joint_state)); -} -void RobotModelRBDL::update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state){ - wbc::RobotModelRBDL::update(tobaseSamplesJoints(joint_state), floating_base_state); -} -base::NamedVector RobotModelRBDL::jointState2(const std::vector &names){ - return toNamedVector(wbc::RobotModelRBDL::jointState(names)); -} -void RobotModelRBDL::setActiveContacts(const base::NamedVector & active_contacts){ - wbc::RobotModelRBDL::setActiveContacts(toActiveContacts(active_contacts)); -} -base::NamedVector RobotModelRBDL::getActiveContacts2(){ - return fromActiveContacts(wbc::RobotModelRBDL::getActiveContacts()); -} -base::NamedVector RobotModelRBDL::jointLimits2(){ - return fromJointLimits(wbc::RobotModelRBDL::jointLimits()); -} -wbc_py::RobotModelConfig RobotModelRBDL::getRobotModelConfig(){ - return fromRobotModelConfig(wbc::RobotModelRBDL::getRobotModelConfig()); -} - -} - -BOOST_PYTHON_MODULE(robot_model_rbdl){ - - np::initialize(); - - py::class_("RobotModelRBDL") - .def("configure", &wbc_py::RobotModelRBDL::configure) - .def("update", &wbc_py::RobotModelRBDL::update) - .def("update", &wbc_py::RobotModelRBDL::update2) - .def("jointState", &wbc_py::RobotModelRBDL::jointState2) - .def("rigidBodyState", &wbc_py::RobotModelRBDL::rigidBodyState, py::return_value_policy()) - .def("spaceJacobian", &wbc_py::RobotModelRBDL::spaceJacobian, py::return_value_policy()) - .def("bodyJacobian", &wbc_py::RobotModelRBDL::spaceJacobian, py::return_value_policy()) - .def("spatialAccelerationBias", &wbc_py::RobotModelRBDL::spatialAccelerationBias, py::return_value_policy()) - .def("jacobianDot", &wbc_py::RobotModelRBDL::jacobianDot, py::return_value_policy()) - .def("jointSpaceInertiaMatrix", &wbc_py::RobotModelRBDL::jointSpaceInertiaMatrix, py::return_value_policy()) - .def("biasForces", &wbc_py::RobotModelRBDL::biasForces, py::return_value_policy()) - .def("jointNames", &wbc_py::RobotModelRBDL::jointNames, py::return_value_policy()) - .def("actuatedJointNames", &wbc_py::RobotModelRBDL::actuatedJointNames, py::return_value_policy()) - .def("independentJointNames", &wbc_py::RobotModelRBDL::independentJointNames, py::return_value_policy()) - .def("jointIndex", &wbc_py::RobotModelRBDL::jointIndex) - .def("baseFrame", &wbc_py::RobotModelRBDL::baseFrame, py::return_value_policy()) - .def("jointLimits", &wbc_py::RobotModelRBDL::jointLimits2) - .def("selectionMatrix", &wbc_py::RobotModelRBDL::selectionMatrix, py::return_value_policy()) - .def("hasLink", &wbc_py::RobotModelRBDL::hasLink) - .def("hasJoint", &wbc_py::RobotModelRBDL::hasJoint) - .def("hasActuatedJoint", &wbc_py::RobotModelRBDL::hasActuatedJoint) - .def("centerOfMass", &wbc_py::RobotModelRBDL::centerOfMass, py::return_value_policy()) - .def("setActiveContacts", &wbc_py::RobotModelRBDL::setActiveContacts) - .def("getActiveContacts", &wbc_py::RobotModelRBDL::getActiveContacts2) - .def("noOfJoints", &wbc_py::RobotModelRBDL::noOfJoints) - .def("noOfActuatedJoints", &wbc_py::RobotModelRBDL::noOfActuatedJoints) - .def("setGravityVector", &wbc_py::RobotModelRBDL::setGravityVector) - .def("floatingBaseState", &wbc_py::RobotModelRBDL::floatingBaseState, py::return_value_policy()) - .def("getRobotModelConfig", &wbc_py::RobotModelRBDL::getRobotModelConfig); -} - - diff --git a/bindings/python/robot_models/rbdl/robot_model_rbdl.hpp b/bindings/python/robot_models/rbdl/robot_model_rbdl.hpp deleted file mode 100644 index 35f8822b..00000000 --- a/bindings/python/robot_models/rbdl/robot_model_rbdl.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef WBC_PY_ROBOT_MODEL_RBDL_HPP -#define WBC_PY_ROBOT_MODEL_RBDL_HPP - -#include "robot_models/rbdl/RobotModelRBDL.hpp" -#include "../../wbc_types_conversions.h" - -namespace wbc_py { - -/** Wrapper for wbc::RobotModelRBDL. Unfortunately we have to do this, since base::samples::Joints and other Rock types cannot be easily exposed to python*/ -class RobotModelRBDL : public wbc::RobotModelRBDL{ -public: - bool configure(const wbc_py::RobotModelConfig &cfg); - void update(const base::NamedVector &joint_state); - void update2(const base::NamedVector &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state); - base::NamedVector jointState2(const std::vector &names); - void setActiveContacts(const base::NamedVector & active_contacts); - base::NamedVector getActiveContacts2(); - base::NamedVector jointLimits2(); - wbc_py::RobotModelConfig getRobotModelConfig(); -}; - -} - -#endif - diff --git a/bindings/python/scenes/CMakeLists.txt b/bindings/python/scenes/CMakeLists.txt deleted file mode 100644 index f389dce4..00000000 --- a/bindings/python/scenes/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -add_subdirectory(velocity) -add_subdirectory(velocity_qp) -add_subdirectory(acceleration) -add_subdirectory(acceleration_tsid) -add_subdirectory(acceleration_reduced_tsid) - -install(FILES __init__.py - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes) diff --git a/bindings/python/scenes/__init__.py b/bindings/python/scenes/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/bindings/python/scenes/acceleration/CMakeLists.txt b/bindings/python/scenes/acceleration/CMakeLists.txt deleted file mode 100644 index 05265f03..00000000 --- a/bindings/python/scenes/acceleration/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -PYTHON_ADD_MODULE(acceleration_scene acceleration_scene.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(acceleration_scene PUBLIC - wbc-scenes-acceleration - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::python - Boost::numpy) - -target_include_directories(acceleration_scene PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS acceleration_scene - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes) diff --git a/bindings/python/scenes/acceleration/acceleration_scene.cpp b/bindings/python/scenes/acceleration/acceleration_scene.cpp deleted file mode 100644 index e7f97597..00000000 --- a/bindings/python/scenes/acceleration/acceleration_scene.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "acceleration_scene.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights){ - wbc::JointWeights weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::JointWeights &weights){ - base::NamedVector weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::TasksStatus& status_in){ - base::NamedVector status_out; - status_out.elements = status_in.elements; - status_out.names = status_in.names; - return status_out; -} - -base::NamedVector toNamedVector(const base::samples::Wrenches& wrenches_in){ - base::NamedVector wrenches_out; - wrenches_out.elements = wrenches_in.elements; - wrenches_out.names = wrenches_in.names; - return wrenches_out; -} - -AccelerationScene::AccelerationScene(std::shared_ptr robot_model, std::shared_ptr solver, const double dt) : - wbc::AccelerationScene(robot_model, solver, dt){ -} -void AccelerationScene::setJointReference(const std::string& task_name, const base::NamedVector& ref){ - wbc::AccelerationScene::setReference(task_name, tobaseSamplesJoints(ref)); -} -void AccelerationScene::setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref){ - wbc::AccelerationScene::setReference(task_name, ref); -} -void AccelerationScene::setJointWeights(const base::NamedVector &weights){ - wbc::AccelerationScene::setJointWeights(toJointWeights(weights)); -} -base::NamedVector AccelerationScene::getJointWeights2(){ - return toNamedVector(wbc::AccelerationScene::getJointWeights()); -} -base::NamedVector AccelerationScene::getActuatedJointWeights2(){ - return toNamedVector(wbc::AccelerationScene::getActuatedJointWeights()); -} -base::NamedVector AccelerationScene::solve2(const wbc::HierarchicalQP &hqp){ - return toNamedVector(wbc::AccelerationScene::solve(hqp)); -} -base::NamedVector AccelerationScene::updateTasksStatus2(){ - return toNamedVector(wbc::AccelerationScene::updateTasksStatus()); -} - -} - -BOOST_PYTHON_MODULE(acceleration_scene){ - - np::initialize(); - - py::class_("AccelerationScene", py::init,std::shared_ptr,const double>()) - .def("configure", &wbc_py::AccelerationScene::configure) - .def("update", &wbc_py::AccelerationScene::update, py::return_value_policy()) - .def("solve", &wbc_py::AccelerationScene::solve2) - .def("setReference", &wbc_py::AccelerationScene::setJointReference) - .def("setReference", &wbc_py::AccelerationScene::setCartReference) - .def("setTaskWeights", &wbc_py::AccelerationScene::setTaskWeights) - .def("setTaskActivation", &wbc_py::AccelerationScene::setTaskActivation) - .def("getTasksStatus", &wbc_py::AccelerationScene::getTasksStatus, py::return_value_policy()) - .def("getNConstraintVariablesPerPrio", &wbc_py::AccelerationScene::getNTaskVariablesPerPrio) - .def("hasTask", &wbc_py::AccelerationScene::hasTask) - .def("updateTasksStatus", &wbc_py::AccelerationScene::updateTasksStatus2) - .def("getHierarchicalQP", &wbc_py::AccelerationScene::getHierarchicalQP, py::return_value_policy()) - .def("getSolverOutput", &wbc_py::AccelerationScene::getSolverOutput, py::return_value_policy()) - .def("setJointWeights", &wbc_py::AccelerationScene::setJointWeights) - .def("getJointWeights", &wbc_py::AccelerationScene::getJointWeights2) - .def("getActuatedJointWeights", &wbc_py::AccelerationScene::getActuatedJointWeights2); -} diff --git a/bindings/python/scenes/acceleration/acceleration_scene.hpp b/bindings/python/scenes/acceleration/acceleration_scene.hpp deleted file mode 100644 index 288ab553..00000000 --- a/bindings/python/scenes/acceleration/acceleration_scene.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef WBC_PY_ACCELERATION_SCENE_HPP -#define WBC_PY_ACCELERATION_SCENE_HPP - -#include "scenes/acceleration/AccelerationScene.hpp" -#include "../../solvers/qpoases/QPOasesSolver.hpp" -#include "../../robot_models/rbdl/robot_model_rbdl.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights); - -class AccelerationScene : public wbc::AccelerationScene{ -public: - AccelerationScene(std::shared_ptr robot_model, std::shared_ptr solver, const double dt); - void setJointReference(const std::string& task_name, const base::NamedVector& ref); - void setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref); - void setJointWeights(const base::NamedVector &weights); - base::NamedVector getJointWeights2(); - base::NamedVector getActuatedJointWeights2(); - base::NamedVector solve2(const wbc::HierarchicalQP &hqp); - base::NamedVector updateTasksStatus2(); -}; - -} - -#endif diff --git a/bindings/python/scenes/acceleration_reduced_tsid/CMakeLists.txt b/bindings/python/scenes/acceleration_reduced_tsid/CMakeLists.txt deleted file mode 100644 index a1949daa..00000000 --- a/bindings/python/scenes/acceleration_reduced_tsid/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -PYTHON_ADD_MODULE(acceleration_scene_reduced_tsid acceleration_scene_reduced_tsid.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(acceleration_scene_reduced_tsid PUBLIC - wbc-scenes-acceleration_reduced_tsid - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::python - Boost::numpy) - -target_include_directories(acceleration_scene_reduced_tsid PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS acceleration_scene_reduced_tsid - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes) diff --git a/bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.cpp b/bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.cpp deleted file mode 100644 index d3b63270..00000000 --- a/bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "acceleration_scene_reduced_tsid.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights){ - wbc::JointWeights weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::JointWeights &weights){ - base::NamedVector weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::TasksStatus& status_in){ - base::NamedVector status_out; - status_out.elements = status_in.elements; - status_out.names = status_in.names; - return status_out; -} - -base::NamedVector toNamedVector(const base::samples::Wrenches& wrenches_in){ - base::NamedVector wrenches_out; - wrenches_out.elements = wrenches_in.elements; - wrenches_out.names = wrenches_in.names; - return wrenches_out; -} - -AccelerationSceneReducedTSID::AccelerationSceneReducedTSID(std::shared_ptr robot_model, std::shared_ptr solver, const double dt) : - wbc::AccelerationSceneReducedTSID(robot_model, solver, dt){ -} -void AccelerationSceneReducedTSID::setJointReference(const std::string& task_name, const base::NamedVector& ref){ - wbc::AccelerationSceneReducedTSID::setReference(task_name, tobaseSamplesJoints(ref)); -} -void AccelerationSceneReducedTSID::setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref){ - wbc::AccelerationSceneReducedTSID::setReference(task_name, ref); -} -void AccelerationSceneReducedTSID::setJointWeights(const base::NamedVector &weights){ - wbc::AccelerationSceneReducedTSID::setJointWeights(toJointWeights(weights)); -} -base::NamedVector AccelerationSceneReducedTSID::getJointWeights2(){ - return toNamedVector(wbc::AccelerationSceneReducedTSID::getJointWeights()); -} -base::NamedVector AccelerationSceneReducedTSID::getActuatedJointWeights2(){ - return toNamedVector(wbc::AccelerationSceneReducedTSID::getActuatedJointWeights()); -} -base::NamedVector AccelerationSceneReducedTSID::solve2(const wbc::HierarchicalQP &hqp){ - return toNamedVector(wbc::AccelerationSceneReducedTSID::solve(hqp)); -} -base::NamedVector AccelerationSceneReducedTSID::updateTasksStatus2(){ - return toNamedVector(wbc::AccelerationSceneReducedTSID::updateTasksStatus()); -} - -} - -BOOST_PYTHON_MODULE(acceleration_scene_reduced_tsid){ - - np::initialize(); - - py::class_("AccelerationSceneReducedTSID", py::init,std::shared_ptr,const double>()) - .def("configure", &wbc_py::AccelerationSceneReducedTSID::configure) - .def("update", &wbc_py::AccelerationSceneReducedTSID::update, py::return_value_policy()) - .def("solve", &wbc_py::AccelerationSceneReducedTSID::solve2) - .def("setReference", &wbc_py::AccelerationSceneReducedTSID::setJointReference) - .def("setReference", &wbc_py::AccelerationSceneReducedTSID::setCartReference) - .def("setTaskWeights", &wbc_py::AccelerationSceneReducedTSID::setTaskWeights) - .def("setTaskActivation", &wbc_py::AccelerationSceneReducedTSID::setTaskActivation) - .def("getTasksStatus", &wbc_py::AccelerationSceneReducedTSID::getTasksStatus, py::return_value_policy()) - .def("getNConstraintVariablesPerPrio", &wbc_py::AccelerationSceneReducedTSID::getNTaskVariablesPerPrio) - .def("hasTask", &wbc_py::AccelerationSceneReducedTSID::hasTask) - .def("updateTasksStatus", &wbc_py::AccelerationSceneReducedTSID::updateTasksStatus2) - .def("getHierarchicalQP", &wbc_py::AccelerationSceneReducedTSID::getHierarchicalQP, py::return_value_policy()) - .def("getSolverOutput", &wbc_py::AccelerationSceneReducedTSID::getSolverOutput, py::return_value_policy()) - .def("setJointWeights", &wbc_py::AccelerationSceneReducedTSID::setJointWeights) - .def("getJointWeights", &wbc_py::AccelerationSceneReducedTSID::getJointWeights2) - .def("getActuatedJointWeights", &wbc_py::AccelerationSceneReducedTSID::getActuatedJointWeights2); -} diff --git a/bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.hpp b/bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.hpp deleted file mode 100644 index df37b598..00000000 --- a/bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef WBC_PY_ACCELERATION_SCENE_REDUCED_TSID_HPP -#define WBC_PY_ACCELERATION_SCENE_REDUCED_TSID_HPP - -#include "scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp" -#include "../../solvers/qpoases/QPOasesSolver.hpp" -#include "../../robot_models/rbdl/robot_model_rbdl.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights); - -class AccelerationSceneReducedTSID : public wbc::AccelerationSceneReducedTSID{ -public: - AccelerationSceneReducedTSID(std::shared_ptr robot_model, std::shared_ptr solver, const double dt); - void setJointReference(const std::string& task_name, const base::NamedVector& ref); - void setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref); - void setJointWeights(const base::NamedVector &weights); - base::NamedVector getJointWeights2(); - base::NamedVector getActuatedJointWeights2(); - base::NamedVector solve2(const wbc::HierarchicalQP &hqp); - base::NamedVector updateTasksStatus2(); -}; - -} - -#endif diff --git a/bindings/python/scenes/acceleration_tsid/CMakeLists.txt b/bindings/python/scenes/acceleration_tsid/CMakeLists.txt deleted file mode 100644 index a38215c5..00000000 --- a/bindings/python/scenes/acceleration_tsid/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -PYTHON_ADD_MODULE(acceleration_scene_tsid acceleration_scene_tsid.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(acceleration_scene_tsid PUBLIC - wbc-scenes-acceleration_tsid - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::python - Boost::numpy) - -target_include_directories(acceleration_scene_tsid PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS acceleration_scene_tsid - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes) diff --git a/bindings/python/scenes/acceleration_tsid/acceleration_scene_tsid.cpp b/bindings/python/scenes/acceleration_tsid/acceleration_scene_tsid.cpp deleted file mode 100644 index 60751981..00000000 --- a/bindings/python/scenes/acceleration_tsid/acceleration_scene_tsid.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "acceleration_scene_tsid.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights){ - wbc::JointWeights weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::JointWeights &weights){ - base::NamedVector weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::TasksStatus& status_in){ - base::NamedVector status_out; - status_out.elements = status_in.elements; - status_out.names = status_in.names; - return status_out; -} - -base::NamedVector toNamedVector(const base::samples::Wrenches& wrenches_in){ - base::NamedVector wrenches_out; - wrenches_out.elements = wrenches_in.elements; - wrenches_out.names = wrenches_in.names; - return wrenches_out; -} - -AccelerationSceneTSID::AccelerationSceneTSID(std::shared_ptr robot_model, std::shared_ptr solver, const double dt) : - wbc::AccelerationSceneTSID(robot_model, solver, dt){ -} -void AccelerationSceneTSID::setJointReference(const std::string& task_name, const base::NamedVector& ref){ - wbc::AccelerationSceneTSID::setReference(task_name, tobaseSamplesJoints(ref)); -} -void AccelerationSceneTSID::setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref){ - wbc::AccelerationSceneTSID::setReference(task_name, ref); -} -void AccelerationSceneTSID::setJointWeights(const base::NamedVector &weights){ - wbc::AccelerationSceneTSID::setJointWeights(toJointWeights(weights)); -} -base::NamedVector AccelerationSceneTSID::getJointWeights2(){ - return toNamedVector(wbc::AccelerationSceneTSID::getJointWeights()); -} -base::NamedVector AccelerationSceneTSID::getActuatedJointWeights2(){ - return toNamedVector(wbc::AccelerationSceneTSID::getActuatedJointWeights()); -} -base::NamedVector AccelerationSceneTSID::solve2(const wbc::HierarchicalQP &hqp){ - return toNamedVector(wbc::AccelerationSceneTSID::solve(hqp)); -} -base::NamedVector AccelerationSceneTSID::updateTasksStatus2(){ - return toNamedVector(wbc::AccelerationSceneTSID::updateTasksStatus()); -} - -} - -BOOST_PYTHON_MODULE(acceleration_scene_tsid){ - - np::initialize(); - - py::class_("AccelerationSceneTSID", py::init,std::shared_ptr,const double>()) - .def("configure", &wbc_py::AccelerationSceneTSID::configure) - .def("update", &wbc_py::AccelerationSceneTSID::update, py::return_value_policy()) - .def("solve", &wbc_py::AccelerationSceneTSID::solve2) - .def("setReference", &wbc_py::AccelerationSceneTSID::setJointReference) - .def("setReference", &wbc_py::AccelerationSceneTSID::setCartReference) - .def("setTaskWeights", &wbc_py::AccelerationSceneTSID::setTaskWeights) - .def("setTaskActivation", &wbc_py::AccelerationSceneTSID::setTaskActivation) - .def("getTasksStatus", &wbc_py::AccelerationSceneTSID::getTasksStatus, py::return_value_policy()) - .def("getNConstraintVariablesPerPrio", &wbc_py::AccelerationSceneTSID::getNTaskVariablesPerPrio) - .def("hasTask", &wbc_py::AccelerationSceneTSID::hasTask) - .def("updateTasksStatus", &wbc_py::AccelerationSceneTSID::updateTasksStatus2) - .def("getHierarchicalQP", &wbc_py::AccelerationSceneTSID::getHierarchicalQP, py::return_value_policy()) - .def("getSolverOutput", &wbc_py::AccelerationSceneTSID::getSolverOutput, py::return_value_policy()) - .def("setJointWeights", &wbc_py::AccelerationSceneTSID::setJointWeights) - .def("getJointWeights", &wbc_py::AccelerationSceneTSID::getJointWeights2) - .def("getActuatedJointWeights", &wbc_py::AccelerationSceneTSID::getActuatedJointWeights2); -} diff --git a/bindings/python/scenes/acceleration_tsid/acceleration_scene_tsid.hpp b/bindings/python/scenes/acceleration_tsid/acceleration_scene_tsid.hpp deleted file mode 100644 index bb141731..00000000 --- a/bindings/python/scenes/acceleration_tsid/acceleration_scene_tsid.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef WBC_PY_ACCELERATION_SCENE_TSID_HPP -#define WBC_PY_ACCELERATION_SCENE_TSID_HPP - -#include "scenes/acceleration_tsid/AccelerationSceneTSID.hpp" -#include "../../solvers/qpoases/QPOasesSolver.hpp" -#include "../../robot_models/rbdl/robot_model_rbdl.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights); - -class AccelerationSceneTSID : public wbc::AccelerationSceneTSID{ -public: - AccelerationSceneTSID(std::shared_ptr robot_model, std::shared_ptr solver, const double dt); - void setJointReference(const std::string& task_name, const base::NamedVector& ref); - void setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref); - void setJointWeights(const base::NamedVector &weights); - base::NamedVector getJointWeights2(); - base::NamedVector getActuatedJointWeights2(); - base::NamedVector solve2(const wbc::HierarchicalQP &hqp); - base::NamedVector updateTasksStatus2(); -}; - -} - -#endif diff --git a/bindings/python/scenes/velocity/CMakeLists.txt b/bindings/python/scenes/velocity/CMakeLists.txt deleted file mode 100644 index 608b93c8..00000000 --- a/bindings/python/scenes/velocity/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -PYTHON_ADD_MODULE(velocity_scene velocity_scene.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(velocity_scene PUBLIC - wbc-scenes-velocity - wbc-robot_models-rbdl - wbc-solvers-hls - Boost::python - Boost::numpy) - -target_include_directories(velocity_scene PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS velocity_scene - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes) diff --git a/bindings/python/scenes/velocity/velocity_scene.cpp b/bindings/python/scenes/velocity/velocity_scene.cpp deleted file mode 100644 index b142d611..00000000 --- a/bindings/python/scenes/velocity/velocity_scene.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "velocity_scene.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights){ - wbc::JointWeights weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::JointWeights &weights){ - base::NamedVector weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::TasksStatus& status_in){ - base::NamedVector status_out; - status_out.elements = status_in.elements; - status_out.names = status_in.names; - return status_out; -} - -base::NamedVector toNamedVector(const base::samples::Wrenches& wrenches_in){ - base::NamedVector wrenches_out; - wrenches_out.elements = wrenches_in.elements; - wrenches_out.names = wrenches_in.names; - return wrenches_out; -} - -VelocityScene::VelocityScene(std::shared_ptr robot_model, std::shared_ptr solver, const double dt) : - wbc::VelocityScene(robot_model, solver, dt){ -} -void VelocityScene::setJointReference(const std::string& task_name, const base::NamedVector& ref){ - wbc::VelocityScene::setReference(task_name, tobaseSamplesJoints(ref)); -} -void VelocityScene::setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref){ - wbc::VelocityScene::setReference(task_name, ref); -} -void VelocityScene::setJointWeights(const base::NamedVector &weights){ - wbc::VelocityScene::setJointWeights(toJointWeights(weights)); -} -base::NamedVector VelocityScene::getJointWeights2(){ - return toNamedVector(wbc::VelocityScene::getJointWeights()); -} -base::NamedVector VelocityScene::getActuatedJointWeights2(){ - return toNamedVector(wbc::VelocityScene::getActuatedJointWeights()); -} -base::NamedVector VelocityScene::solve2(const wbc::HierarchicalQP &hqp){ - return toNamedVector(wbc::VelocityScene::solve(hqp)); -} -base::NamedVector VelocityScene::updateTasksStatus2(){ - return toNamedVector(wbc::VelocityScene::updateTasksStatus()); -} - -} - -BOOST_PYTHON_MODULE(velocity_scene){ - - np::initialize(); - - py::class_("VelocityScene", py::init,std::shared_ptr,const double>()) - .def("configure", &wbc_py::VelocityScene::configure) - .def("update", &wbc_py::VelocityScene::update, py::return_value_policy()) - .def("solve", &wbc_py::VelocityScene::solve2) - .def("setReference", &wbc_py::VelocityScene::setJointReference) - .def("setReference", &wbc_py::VelocityScene::setCartReference) - .def("setTaskWeights", &wbc_py::VelocityScene::setTaskWeights) - .def("setTaskActivation", &wbc_py::VelocityScene::setTaskActivation) - .def("getTasksStatus", &wbc_py::VelocityScene::getTasksStatus, py::return_value_policy()) - .def("getNConstraintVariablesPerPrio", &wbc_py::VelocityScene::getNTaskVariablesPerPrio) - .def("hasTask", &wbc_py::VelocityScene::hasTask) - .def("updateTasksStatus", &wbc_py::VelocityScene::updateTasksStatus2) - .def("getHierarchicalQP", &wbc_py::VelocityScene::getHierarchicalQP, py::return_value_policy()) - .def("getSolverOutput", &wbc_py::VelocityScene::getSolverOutput, py::return_value_policy()) - .def("setJointWeights", &wbc_py::VelocityScene::setJointWeights) - .def("getJointWeights", &wbc_py::VelocityScene::getJointWeights2) - .def("getActuatedJointWeights", &wbc_py::VelocityScene::getActuatedJointWeights2); -} diff --git a/bindings/python/scenes/velocity/velocity_scene.hpp b/bindings/python/scenes/velocity/velocity_scene.hpp deleted file mode 100644 index 35d409e5..00000000 --- a/bindings/python/scenes/velocity/velocity_scene.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef WBC_PY_VELOCITY_SCENE_HPP -#define WBC_PY_VELOCITY_SCENE_HPP - -#include "scenes/velocity/VelocityScene.hpp" -#include "../../solvers/hls/HierarchicalLSSolver.hpp" -#include "../../robot_models/rbdl/robot_model_rbdl.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights); - -class VelocityScene : public wbc::VelocityScene{ -public: - VelocityScene(std::shared_ptr robot_model, std::shared_ptr solver, const double dt); - void setJointReference(const std::string& task_name, const base::NamedVector& ref); - void setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref); - void setJointWeights(const base::NamedVector &weights); - base::NamedVector getJointWeights2(); - base::NamedVector getActuatedJointWeights2(); - base::NamedVector solve2(const wbc::HierarchicalQP &hqp); - base::NamedVector updateTasksStatus2(); -}; - -} - -#endif diff --git a/bindings/python/scenes/velocity_qp/CMakeLists.txt b/bindings/python/scenes/velocity_qp/CMakeLists.txt deleted file mode 100644 index f1d7a5ea..00000000 --- a/bindings/python/scenes/velocity_qp/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -PYTHON_ADD_MODULE(velocity_scene_qp velocity_scene_qp.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(velocity_scene_qp PUBLIC - wbc-scenes-velocity_qp - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::python - Boost::numpy) - -target_include_directories(velocity_scene_qp PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS velocity_scene_qp - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes) diff --git a/bindings/python/scenes/velocity_qp/velocity_scene_qp.cpp b/bindings/python/scenes/velocity_qp/velocity_scene_qp.cpp deleted file mode 100644 index c593268e..00000000 --- a/bindings/python/scenes/velocity_qp/velocity_scene_qp.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "velocity_scene_qp.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights){ - wbc::JointWeights weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::JointWeights &weights){ - base::NamedVector weights_out; - weights_out.elements = weights.elements; - weights_out.names = weights.names; - return weights_out; -} - -base::NamedVector toNamedVector(const wbc::TasksStatus& status_in){ - base::NamedVector status_out; - status_out.elements = status_in.elements; - status_out.names = status_in.names; - return status_out; -} - -base::NamedVector toNamedVector(const base::samples::Wrenches& wrenches_in){ - base::NamedVector wrenches_out; - wrenches_out.elements = wrenches_in.elements; - wrenches_out.names = wrenches_in.names; - return wrenches_out; -} - -VelocitySceneQP::VelocitySceneQP(std::shared_ptr robot_model, std::shared_ptr solver, const double dt) : - wbc::VelocitySceneQP(robot_model, solver, dt){ -} -void VelocitySceneQP::setJointReference(const std::string& task_name, const base::NamedVector& ref){ - wbc::VelocitySceneQP::setReference(task_name, tobaseSamplesJoints(ref)); -} -void VelocitySceneQP::setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref){ - wbc::VelocitySceneQP::setReference(task_name, ref); -} -void VelocitySceneQP::setJointWeights(const base::NamedVector &weights){ - wbc::VelocitySceneQP::setJointWeights(toJointWeights(weights)); -} -base::NamedVector VelocitySceneQP::getJointWeights2(){ - return toNamedVector(wbc::VelocitySceneQP::getJointWeights()); -} -base::NamedVector VelocitySceneQP::getActuatedJointWeights2(){ - return toNamedVector(wbc::VelocitySceneQP::getActuatedJointWeights()); -} -base::NamedVector VelocitySceneQP::solve2(const wbc::HierarchicalQP &hqp){ - return toNamedVector(wbc::VelocitySceneQP::solve(hqp)); -} -base::NamedVector VelocitySceneQP::updateTasksStatus2(){ - return toNamedVector(wbc::VelocitySceneQP::updateTasksStatus()); -} - -} - -BOOST_PYTHON_MODULE(velocity_scene_qp){ - - np::initialize(); - - py::class_("VelocitySceneQP", py::init,std::shared_ptr,const double>()) - .def("configure", &wbc_py::VelocitySceneQP::configure) - .def("update", &wbc_py::VelocitySceneQP::update, py::return_value_policy()) - .def("solve", &wbc_py::VelocitySceneQP::solve2) - .def("setReference", &wbc_py::VelocitySceneQP::setJointReference) - .def("setReference", &wbc_py::VelocitySceneQP::setCartReference) - .def("setTaskWeights", &wbc_py::VelocitySceneQP::setTaskWeights) - .def("setTaskActivation", &wbc_py::VelocitySceneQP::setTaskActivation) - .def("getTasksStatus", &wbc_py::VelocitySceneQP::getTasksStatus, py::return_value_policy()) - .def("getNConstraintVariablesPerPrio", &wbc_py::VelocitySceneQP::getNTaskVariablesPerPrio) - .def("hasTask", &wbc_py::VelocitySceneQP::hasTask) - .def("updateTasksStatus", &wbc_py::VelocitySceneQP::updateTasksStatus2) - .def("getHierarchicalQP", &wbc_py::VelocitySceneQP::getHierarchicalQP, py::return_value_policy()) - .def("getSolverOutput", &wbc_py::VelocitySceneQP::getSolverOutput, py::return_value_policy()) - .def("setJointWeights", &wbc_py::VelocitySceneQP::setJointWeights) - .def("getJointWeights", &wbc_py::VelocitySceneQP::getJointWeights2) - .def("getActuatedJointWeights", &wbc_py::VelocitySceneQP::getActuatedJointWeights2); -} diff --git a/bindings/python/scenes/velocity_qp/velocity_scene_qp.hpp b/bindings/python/scenes/velocity_qp/velocity_scene_qp.hpp deleted file mode 100644 index 44c263e4..00000000 --- a/bindings/python/scenes/velocity_qp/velocity_scene_qp.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef WBC_PY_VELOCITY_SCENE_QP_HPP -#define WBC_PY_VELOCITY_SCENE_QP_HPP - -#include "scenes/velocity_qp/VelocitySceneQP.hpp" -#include "../../solvers/qpoases/QPOasesSolver.hpp" -#include "../../robot_models/rbdl/robot_model_rbdl.hpp" - -namespace wbc_py { - -wbc::JointWeights toJointWeights(const base::NamedVector &weights); - -class VelocitySceneQP : public wbc::VelocitySceneQP{ -public: - VelocitySceneQP(std::shared_ptr robot_model, std::shared_ptr solver, const double dt); - void setJointReference(const std::string& task_name, const base::NamedVector& ref); - void setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref); - void setJointWeights(const base::NamedVector &weights); - base::NamedVector getJointWeights2(); - base::NamedVector getActuatedJointWeights2(); - base::NamedVector solve2(const wbc::HierarchicalQP &hqp); - base::NamedVector updateTasksStatus2(); -}; - -} - -#endif diff --git a/bindings/python/solvers/CMakeLists.txt b/bindings/python/solvers/CMakeLists.txt deleted file mode 100644 index c2dda88e..00000000 --- a/bindings/python/solvers/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -add_subdirectory(qpoases) -add_subdirectory(hls) - -install(FILES __init__.py - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/solvers) diff --git a/bindings/python/solvers/__init__.py b/bindings/python/solvers/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/bindings/python/solvers/hls/CMakeLists.txt b/bindings/python/solvers/hls/CMakeLists.txt deleted file mode 100644 index ac628997..00000000 --- a/bindings/python/solvers/hls/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -PYTHON_ADD_MODULE(hls_solver HierarchicalLSSolver.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(hls_solver PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-solvers-hls) - -target_include_directories(hls_solver PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS hls_solver - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/solvers) diff --git a/bindings/python/solvers/hls/HierarchicalLSSolver.cpp b/bindings/python/solvers/hls/HierarchicalLSSolver.cpp deleted file mode 100644 index 97fd9845..00000000 --- a/bindings/python/solvers/hls/HierarchicalLSSolver.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "solvers/hls/HierarchicalLSSolver.hpp" -#include "HierarchicalLSSolver.hpp" -#include "core/QuadraticProgram.hpp" - -namespace wbc_py { -base::VectorXd HierarchicalLSSolver::solve(const wbc::HierarchicalQP &hqp){ - base::VectorXd solver_output; - wbc::HierarchicalLSSolver::solve(hqp, solver_output); - return solver_output; -} -} - -BOOST_PYTHON_MODULE(hls_solver){ - - np::initialize(); - - py::class_("HierarchicalLSSolver") - .def("solve", &wbc_py::HierarchicalLSSolver::solve) - .def("setMaxSolverOutputNorm", &wbc_py::HierarchicalLSSolver::setMaxSolverOutputNorm) - .def("getMaxSolverOutputNorm", &wbc_py::HierarchicalLSSolver::getMaxSolverOutputNorm) - .def("setMinEigenvalue", &wbc_py::HierarchicalLSSolver::setMinEigenvalue) - .def("getMinEigenvalue", &wbc_py::HierarchicalLSSolver::getMinEigenvalue); -} - - diff --git a/bindings/python/solvers/hls/HierarchicalLSSolver.hpp b/bindings/python/solvers/hls/HierarchicalLSSolver.hpp deleted file mode 100644 index a35393ea..00000000 --- a/bindings/python/solvers/hls/HierarchicalLSSolver.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef WBC_PY_HLS_SOLVER_HPP -#define WBC_PY_HLS_SOLVER_HPP - -#include "solvers/hls/HierarchicalLSSolver.hpp" - -namespace wbc_py { - -class HierarchicalLSSolver : public wbc::HierarchicalLSSolver{ -public: - base::VectorXd solve(const wbc::HierarchicalQP &hqp); -}; - -} - -#endif diff --git a/bindings/python/solvers/hls/solvers.cpp b/bindings/python/solvers/hls/solvers.cpp deleted file mode 100644 index 77d9cef3..00000000 --- a/bindings/python/solvers/hls/solvers.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include "eigen_conversion.h" -#include "base_types_conversion.h" -#include "std_vector_conversion.h" -#include "solvers/hls/HierarchicalLSSolver.hpp" -#include "solvers/qpoases/QPOasesSolver.hpp" -#include "solvers.hpp" -#include "core/QuadraticProgram.hpp" - -namespace wbc_py { -base::VectorXd HierarchicalLSSolver::solve(const wbc::HierarchicalQP &hqp){ - base::VectorXd solver_output; - wbc::HierarchicalLSSolver::solve(hqp, solver_output); - return solver_output; -} -base::VectorXd QPOASESSolver::solve(const wbc::HierarchicalQP &hqp){ - base::VectorXd solver_output; - wbc::QPOASESSolver::solve(hqp, solver_output); - return solver_output; -} -int QPOASESSolver::getReturnValueAsInt(){ - return (int)wbc::QPOASESSolver::getReturnValue(); -} -} - -BOOST_PYTHON_MODULE(solvers){ - - np::initialize(); - - py::class_("HierarchicalLSSolver") - .def("solve", &wbc_py::HierarchicalLSSolver::solve) - .def("setMaxSolverOutputNorm", &wbc_py::HierarchicalLSSolver::setMaxSolverOutputNorm) - .def("getMaxSolverOutputNorm", &wbc_py::HierarchicalLSSolver::getMaxSolverOutputNorm) - .def("setMinEigenvalue", &wbc_py::HierarchicalLSSolver::setMinEigenvalue) - .def("getMinEigenvalue", &wbc_py::HierarchicalLSSolver::getMinEigenvalue); - py::class_("QPOASESSolver") - .def("solve", &wbc_py::QPOASESSolver::solve) - .def("setMaxNoWSR", &wbc_py::QPOASESSolver::setMaxNoWSR) - .def("getMaxNoWSR", &wbc_py::QPOASESSolver::getMaxNoWSR) - .def("getReturnValue", &wbc_py::QPOASESSolver::getReturnValueAsInt) - .def("getNoWSR", &wbc_py::QPOASESSolver::getNoWSR) - .def("getOptions", &wbc_py::QPOASESSolver::getOptions) - .def("setOptions", &wbc_py::QPOASESSolver::setOptions); -} - - diff --git a/bindings/python/solvers/hls/solvers.hpp b/bindings/python/solvers/hls/solvers.hpp deleted file mode 100644 index b06be5f5..00000000 --- a/bindings/python/solvers/hls/solvers.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef WBC_PY_SOLVER_HPP -#define WBC_PY_SOLVER_HPP - -#include "solvers/hls/HierarchicalLSSolver.hpp" -#include "solvers/qpoases/QPOasesSolver.hpp" - -namespace wbc_py { - -class HierarchicalLSSolver : public wbc::HierarchicalLSSolver{ -public: - base::VectorXd solve(const wbc::HierarchicalQP &hqp); -}; - -class QPOASESSolver : public wbc::QPOASESSolver{ -public: - base::VectorXd solve(const wbc::HierarchicalQP &hqp); - int getReturnValueAsInt(); -}; -} - -#endif diff --git a/bindings/python/solvers/qpoases/CMakeLists.txt b/bindings/python/solvers/qpoases/CMakeLists.txt deleted file mode 100644 index 0bc51e37..00000000 --- a/bindings/python/solvers/qpoases/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -PYTHON_ADD_MODULE(qpoases_solver QPOasesSolver.cpp) - -# Set up the libraries and header search paths for this target -target_link_libraries(qpoases_solver PUBLIC - ${PYTHON_LIBRARIES} - Boost::python - Boost::numpy - wbc-solvers-hls - wbc-solvers-qpoases) - -target_include_directories(qpoases_solver PUBLIC - ${PYTHON_INCLUDE_DIRS}) - -install(TARGETS qpoases_solver - DESTINATION ${PYTHON_INSTALL_PATH}/wbc/solvers) diff --git a/bindings/python/solvers/qpoases/QPOasesSolver.cpp b/bindings/python/solvers/qpoases/QPOasesSolver.cpp deleted file mode 100644 index c6288440..00000000 --- a/bindings/python/solvers/qpoases/QPOasesSolver.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "../../eigen_conversion.h" -#include "../../base_types_conversion.h" -#include "../../std_vector_conversion.h" -#include "solvers/qpoases/QPOasesSolver.hpp" -#include "QPOasesSolver.hpp" -#include "core/QuadraticProgram.hpp" - -namespace wbc_py { -base::VectorXd QPOASESSolver::solve(const wbc::HierarchicalQP &hqp){ - base::VectorXd solver_output; - wbc::QPOASESSolver::solve(hqp, solver_output); - return solver_output; -} -int QPOASESSolver::getReturnValueAsInt(){ - return (int)wbc::QPOASESSolver::getReturnValue(); -} -} - -BOOST_PYTHON_MODULE(qpoases_solver){ - - np::initialize(); - py::class_("QPOASESSolver") - .def("solve", &wbc_py::QPOASESSolver::solve) - .def("setMaxNoWSR", &wbc_py::QPOASESSolver::setMaxNoWSR) - .def("getMaxNoWSR", &wbc_py::QPOASESSolver::getMaxNoWSR) - .def("getReturnValue", &wbc_py::QPOASESSolver::getReturnValueAsInt) - .def("getNoWSR", &wbc_py::QPOASESSolver::getNoWSR) - .def("getOptions", &wbc_py::QPOASESSolver::getOptions) - .def("setOptions", &wbc_py::QPOASESSolver::setOptions); -} - - diff --git a/bindings/python/solvers/qpoases/QPOasesSolver.hpp b/bindings/python/solvers/qpoases/QPOasesSolver.hpp deleted file mode 100644 index f510a144..00000000 --- a/bindings/python/solvers/qpoases/QPOasesSolver.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef WBC_PY_QPOASES_SOLVER_HPP -#define WBC_PY_QPOASES_SOLVER_HPP - -#include "solvers/qpoases/QPOasesSolver.hpp" - -namespace wbc_py { - - -class QPOASESSolver : public wbc::QPOASESSolver{ -public: - base::VectorXd solve(const wbc::HierarchicalQP &hqp); - int getReturnValueAsInt(); -}; -} - -#endif diff --git a/bindings/python/std_vector_conversion.h b/bindings/python/std_vector_conversion.h deleted file mode 100644 index 8cd708ef..00000000 --- a/bindings/python/std_vector_conversion.h +++ /dev/null @@ -1,77 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace py = boost::python; - -namespace pygen { - -/* - * List -> std vector converters - */ - -template -struct python_list_to_std_vector { - python_list_to_std_vector() - { - py::converter::registry::push_back(&convertible, &construct, py::type_id()); - } - - static void* convertible(PyObject* obj_ptr) - { - if (!PySequence_Check(obj_ptr)) - return 0; - - py::list arr = py::extract(obj_ptr); - return obj_ptr; - } - - static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) - { - py::list arr = py::extract(obj_ptr); - auto len = py::len(arr); - - using storage_type = py::converter::rvalue_from_python_storage; - void* storage = reinterpret_cast(data)->storage.bytes; - - new (storage) StdVectorType; - StdVectorType& vec = *static_cast(storage); - - vec.resize(len); - - for (long i = 0; i < len; ++i) - vec[i] = py::extract(arr[i]); - - data->convertible = storage; - } -}; - -/* - * To std vector -> List convertors - */ - -template -struct std_vector_to_python_list { - static PyObject* convert(const StdVectorType& vec) - { - boost::python::list l; - for (int i = 0; i < vec.size(); i++) - l.append(vec[i]); - return py::incref(l.ptr()); - } -}; - - -/* - * Main converters - */ -template -void convertStdVector() -{ - python_list_to_std_vector(); - py::to_python_converter >(); -} - -} // namespace pygen diff --git a/bindings/python/test/__pycache__/test_controllers.cpython-36.pyc b/bindings/python/test/__pycache__/test_controllers.cpython-36.pyc deleted file mode 100644 index c704534d..00000000 Binary files a/bindings/python/test/__pycache__/test_controllers.cpython-36.pyc and /dev/null differ diff --git a/bindings/python/test/__pycache__/test_solvers.cpython-36.pyc b/bindings/python/test/__pycache__/test_solvers.cpython-36.pyc deleted file mode 100644 index ec750aa8..00000000 Binary files a/bindings/python/test/__pycache__/test_solvers.cpython-36.pyc and /dev/null differ diff --git a/bindings/python/test/__pycache__/test_velocity_scene.cpython-36.pyc b/bindings/python/test/__pycache__/test_velocity_scene.cpython-36.pyc deleted file mode 100644 index 1b681f6b..00000000 Binary files a/bindings/python/test/__pycache__/test_velocity_scene.cpython-36.pyc and /dev/null differ diff --git a/bindings/python/test/test_controllers.py b/bindings/python/test/test_controllers.py deleted file mode 100644 index ab93573f..00000000 --- a/bindings/python/test/test_controllers.py +++ /dev/null @@ -1,78 +0,0 @@ -from wbc.controllers import * -from wbc.core import * -import numpy as np -import nose - -def test_cartesian_pos_pd_controller(): - ctrl = CartesianPosPDController() - p_gain = [1]*6 - d_gain = [0.1]*6 - ff_gain = [0.5]*6 - max_ctrl_out = [10]*6 - dead_zone = [0]*6 - - ctrl.setPGain(p_gain) - ctrl.setDGain(d_gain) - ctrl.setFFGain(ff_gain) - ctrl.setMaxCtrlOutput(max_ctrl_out) - ctrl.setDeadZone(dead_zone) - assert np.all(ctrl.pGain() == p_gain) - assert np.all(ctrl.dGain() == d_gain) - assert np.all(ctrl.ffGain() == ff_gain) - assert np.all(ctrl.maxCtrlOutput() == max_ctrl_out) - assert np.all(ctrl.deadZone() == dead_zone) - - ref = RigidBodyStateSE3() - ref.pose.position = [1,2,3] - ref.pose.orientation = [0,0,0,1] - ref.twist.linear = [1,1,1] - ref.twist.angular = [1,1,1] - - act = RigidBodyStateSE3() - act.pose.position = [0,0,0] - act.pose.orientation = [0,0,0,1] - - control_output = ctrl.update(ref,act) - assert np.all(control_output.twist.linear == [1.1, 2.1, 3.1]) - assert np.all(control_output.twist.angular == [0.1, 0.1, 0.1]) - -def test_joint_pos_pd_controller(): - joint_names = ["joint_1","joint_2","joint_3"] - nj = len(joint_names) - ctrl = JointPosPDController(joint_names) - p_gain = [1]*nj - d_gain = [0.1]*nj - ff_gain = [0.1]*nj - max_ctrl_out = [10]*nj - dead_zone = [0]*nj - - ctrl.setPGain(p_gain) - ctrl.setDGain(d_gain) - ctrl.setFFGain(ff_gain) - ctrl.setMaxCtrlOutput(max_ctrl_out) - ctrl.setDeadZone(dead_zone) - assert np.all(ctrl.pGain() == p_gain) - assert np.all(ctrl.dGain() == d_gain) - assert np.all(ctrl.ffGain() == ff_gain) - assert np.all(ctrl.maxCtrlOutput() == max_ctrl_out) - assert np.all(ctrl.deadZone() == dead_zone) - - js = JointState() - js.position = 1 - js.speed = 1 - ref = Joints() - ref.names = joint_names - ref.elements = [js]*nj - - act = Joints() - js.position = 0 - js.speed = 0 - act.names = joint_names - act.elements = [js]*nj - - control_output = ctrl.update(ref,act) - vel = np.array([c.speed for c in control_output.elements]) - assert np.all(np.isclose(vel - [1.1]*nj,np.zeros(nj),rtol=1e-5,atol=1e-7)) - -if __name__ == '__main__': - nose.run() diff --git a/bindings/python/test/test_robot_models.py b/bindings/python/test/test_robot_models.py deleted file mode 100644 index ad1d744f..00000000 --- a/bindings/python/test/test_robot_models.py +++ /dev/null @@ -1,89 +0,0 @@ -from wbc.core import * -from wbc.robot_models.robot_model_rbdl import * -import numpy as np -import nose - -def run(robot_model): - joint_names = ["LLHip1", "LLHip2", "LLHip3", "LLKnee", "LLAnkleRoll", "LLAnklePitch"] - root_link = "RH5_Root_Link" - tip_link = "LLAnkle_FT" - nj = len(joint_names) - gravity_vector = [0,0,-9.81] - contacts = ActiveContacts() - contacts.names = ["RH5_Root_link", "LLAnkle_FT"] - a = ActiveContact() - a.mu = 0.6 - a.active = 1 - contacts.elements = [a,a] - - r=RobotModelConfig() - r.file_or_string="../../../models/rh5/urdf/rh5_single_leg.urdf" - r.floating_base = False - assert robot_model.configure(r) == True - - joint_state = Joints() - joint_state.names = joint_names - js = JointState() - js.position = js.speed = js.acceleration = 0.1 - joint_state.elements = [js]*len(joint_names) - robot_model.update(joint_state) - - robot_model.jointState(joint_names) == joint_state - rbs = robot_model.rigidBodyState(root_link, tip_link) - space_jacobian = robot_model.spaceJacobian(root_link, tip_link) - body_jacobian = robot_model.bodyJacobian(root_link, tip_link) - - twist = np.array(body_jacobian).dot(np.array([js.speed]*nj)) - assert np.all(np.isclose(twist[0:3] - rbs.twist.linear, np.array([0]*3))) - assert np.all(np.isclose(twist[3:6] - rbs.twist.angular, np.array([0]*3))) - - bias_acc = robot_model.spatialAccelerationBias(root_link, tip_link) - accel = np.append(bias_acc.linear,bias_acc.angular) + space_jacobian.dot(np.array([js.acceleration]*nj)) - assert np.all(np.isclose(accel[0:3] - rbs.acceleration.linear, np.array([0]*3))) - assert np.all(np.isclose(accel[3:6] - rbs.acceleration.angular, np.array([0]*3))) - - #jac_dot = robot_model.jacobianDot(root_link,tip_link) - - inertia_mat = robot_model.jointSpaceInertiaMatrix() - assert inertia_mat.shape[0] == nj - assert inertia_mat.shape[1] == nj - - eff_bias = robot_model.biasForces() - assert len(eff_bias) == nj - - assert robot_model.jointNames() == joint_names - assert robot_model.actuatedJointNames() == joint_names - assert robot_model.independentJointNames() == joint_names - assert robot_model.jointIndex("LLHip2") == 1 - assert robot_model.baseFrame() == "RH5_Root_Link" - - joint_limits = robot_model.jointLimits() - joint_limits.names = joint_names - - sel_mat = robot_model.selectionMatrix() - assert np.all(sel_mat == np.eye(nj)) - - assert robot_model.hasLink("LLAnkle_FT") == True - assert robot_model.hasLink("LLAnkle_F") == False - assert robot_model.hasJoint("LLAnkle_F") == False - assert robot_model.hasJoint("LLKnee") == True - assert robot_model.hasActuatedJoint("LLKnee") == True - assert robot_model.hasActuatedJoint("LLKnee_F") == False - - cog = robot_model.centerOfMass() - - robot_model.setActiveContacts(contacts) - robot_model.getActiveContacts() == contacts - - assert robot_model.noOfJoints() == nj - assert robot_model.noOfActuatedJoints() == nj - - robot_model.setGravityVector([0,0,-9.81]) - - robot_model.getRobotModelConfig() == r - -def test_robot_model_rbdl(): - run(RobotModelRBDL()) - -if __name__ == '__main__': - nose.run() diff --git a/bindings/python/test/test_scenes.py b/bindings/python/test/test_scenes.py deleted file mode 100644 index 63772b08..00000000 --- a/bindings/python/test/test_scenes.py +++ /dev/null @@ -1,203 +0,0 @@ -from wbc.robot_models.robot_model_rbdl import * -from wbc.scenes.velocity_scene import * -from wbc.scenes.velocity_scene_qp import * -from wbc.scenes.acceleration_scene import * -from wbc.scenes.acceleration_scene_tsid import * -from wbc.scenes.acceleration_scene_reduced_tsid import * -from wbc.core import * -from wbc.solvers.hls_solver import * -from wbc.solvers.qpoases_solver import * -import numpy as np -import nose -#from IPython import embed - -def configure_wbc(scene): - cfg = TaskConfig() - cfg.name = "tcp_pose" - cfg.root = "kuka_lbr_l_link_0" - cfg.tip = "kuka_lbr_l_tcp" - cfg.ref_frame = "kuka_lbr_l_link_0" - cfg.priority = 0 - cfg.activation = 1 - cfg.type = TaskType.cart - cfg.weights = [1]*6 - constraint_config = [cfg] - - assert scene.configure(constraint_config) == True - -def run_velocity_wbc(scene, robot_model): - cfg = TaskConfig() - cfg.name = "tcp_pose" - cfg.root = "kuka_lbr_l_link_0" - cfg.tip = "kuka_lbr_l_tcp" - cfg.ref_frame = "kuka_lbr_l_link_0" - cfg.priority = 0 - cfg.activation = 1 - cfg.type = TaskType.cart - cfg.weights = [1]*6 - constraint_config = [cfg] - - scene.configure(constraint_config) == True - - ref = RigidBodyStateSE3() - ref.twist.linear = [0.0,0.0,0.1] - ref.twist.angular = [0.0,0.0,0.0] - scene.setReference(cfg.name,ref) - hqp = scene.update() - solver_output = scene.solve(hqp) - status = scene.updateTasksStatus() - - assert np.all(status.elements[0].y_ref[0:3] == ref.twist.linear) - assert np.all(status.elements[0].y_ref[3:6] == ref.twist.angular) - assert np.all(np.isclose(status.elements[0].y_solution[0:3] - ref.twist.linear,np.zeros(3),atol=1e-6)) - assert np.all(np.isclose(status.elements[0].y_solution[3:6] - ref.twist.angular,np.zeros(3),atol=1e-6)) - - qd = np.array([s.speed for s in solver_output.elements]) - J = robot_model.spaceJacobian(cfg.root, cfg.tip) - xd = J.dot(qd) - assert np.all(np.isclose(xd[0:3] - ref.twist.linear,np.zeros(3),atol=1e-6)) - assert np.all(np.isclose(xd[3:6] - ref.twist.angular,np.zeros(3),atol=1e-6)) - - # Test Joint Weights - joint_weights = JointWeights() - joint_weights.elements = [1,1,1,0,1,1,1] - joint_weights.names = robot_model.jointNames() - - scene.setJointWeights(joint_weights) - assert np.all(scene.getJointWeights().elements == joint_weights.elements) - assert np.all(scene.getJointWeights().names == joint_weights.names) - assert np.all(scene.getActuatedJointWeights().elements == joint_weights.elements) - assert np.all(scene.getActuatedJointWeights().names == joint_weights.names) - - hqp = scene.update() - solver_output = scene.solve(hqp) - assert np.all(solver_output.elements[3].speed == 0) - assert np.all(hqp.Wq[3] == 0) - - # Test Task Weights - scene.setTaskWeights(cfg.name,[1,1,0,1,1,1]) - hqp = scene.update() - solver_output = scene.solve(hqp) - qd = np.array([s.speed for s in solver_output.elements]) - xd = J.dot(qd) - assert np.isclose(xd[2],0) - -def run_acceleration_wbc(scene, robot_model): - - cfg = TaskConfig() - cfg.name = "tcp_pose" - cfg.root = "kuka_lbr_l_link_0" - cfg.tip = "kuka_lbr_l_tcp" - cfg.ref_frame = "kuka_lbr_l_link_0" - cfg.priority = 0 - cfg.activation = 1 - cfg.type = TaskType.cart - cfg.weights = [1]*6 - constraint_config = [cfg] - - scene.configure(constraint_config) == True - - ref = RigidBodyStateSE3() - ref.acceleration.linear = [0.0,0.0,0.1] - ref.acceleration.angular = [0.0,0.0,0.0] - scene.setReference(cfg.name,ref) - hqp = scene.update() - solver_output = scene.solve(hqp) - status = scene.updateTasksStatus() - - - assert np.all(status.elements[0].y_ref[0:3] == ref.acceleration.linear) - assert np.all(status.elements[0].y_ref[3:6] == ref.acceleration.angular) - assert np.all(np.isclose(status.elements[0].y_solution[0:3] - ref.acceleration.linear,np.zeros(3),atol=1e-6)) - assert np.all(np.isclose(status.elements[0].y_solution[3:6] - ref.acceleration.angular,np.zeros(3),atol=1e-6)) - - qd = np.array([s.speed for s in solver_output.elements]) - qdd = np.array([s.acceleration for s in solver_output.elements]) - J = robot_model.spaceJacobian(cfg.root, cfg.tip) - acc_bias = np.array([0]*6) - acc_bias[0:3] = robot_model.spatialAccelerationBias(cfg.root, cfg.tip).linear - acc_bias[3:6] = robot_model.spatialAccelerationBias(cfg.root, cfg.tip).angular - xdd = J.dot(qdd) + acc_bias - assert np.all(np.isclose(xdd[0:3] - ref.acceleration.linear,np.zeros(3),atol=1e-6)) - assert np.all(np.isclose(xdd[3:6] - ref.acceleration.angular,np.zeros(3),atol=1e-6)) - - # Test Joint Weights - joint_weights = JointWeights() - joint_weights.elements = [1,1,1,0,1,1,1] - joint_weights.names = robot_model.jointNames() - - scene.setJointWeights(joint_weights) - assert np.all(scene.getJointWeights().elements == joint_weights.elements) - assert np.all(scene.getJointWeights().names == joint_weights.names) - assert np.all(scene.getActuatedJointWeights().elements == joint_weights.elements) - assert np.all(scene.getActuatedJointWeights().names == joint_weights.names) - - hqp = scene.update() - solver_output = scene.solve(hqp) - assert np.all(np.isclose(solver_output.elements[3].acceleration,0)) - assert np.all(hqp.Wq[3] == 0) - - # Test Task Weights - scene.setTaskWeights(cfg.name,[1,1,0,1,1,1]) - hqp = scene.update() - solver_output = scene.solve(hqp) - - qd = np.array([s.speed for s in solver_output.elements]) - qdd = np.array([s.acceleration for s in solver_output.elements]) - xdd = J.dot(qdd) + acc_bias - assert np.isclose(xdd[2],0) - -def test_configure(): - robot_model=RobotModelRBDL() - r=RobotModelConfig() - r.file_or_string="../../../models/kuka/urdf/kuka_iiwa.urdf" - r.actuated_joint_names = ["kuka_lbr_l_joint_1", "kuka_lbr_l_joint_2", "kuka_lbr_l_joint_3", "kuka_lbr_l_joint_4", "kuka_lbr_l_joint_5", "kuka_lbr_l_joint_6", "kuka_lbr_l_joint_7"] - r.joint_names = r.actuated_joint_names - assert robot_model.configure(r) == True - - qp_solver = QPOASESSolver() - hls_solver = HierarchicalLSSolver() - - joint_state = Joints() - js = JointState() - js.position = 1.0 - js.speed = js.acceleration = 0 - joint_state.names = r.joint_names - joint_state.elements = [js]*len(r.actuated_joint_names) - robot_model.update(joint_state) - - configure_wbc(VelocityScene(robot_model, hls_solver, 0.001)) - configure_wbc(VelocitySceneQP(robot_model, qp_solver, 0.001)) - configure_wbc(AccelerationScene(robot_model, qp_solver, 0.001)) - configure_wbc(AccelerationSceneTSID(robot_model, qp_solver, 0.001)) - configure_wbc(AccelerationSceneReducedTSID(robot_model, qp_solver, 0.001)) - -def test_solver_output(): - robot_model=RobotModelRBDL() - r=RobotModelConfig() - r.file_or_string="../../../models/kuka/urdf/kuka_iiwa.urdf" - r.actuated_joint_names = ["kuka_lbr_l_joint_1", "kuka_lbr_l_joint_2", "kuka_lbr_l_joint_3", "kuka_lbr_l_joint_4", "kuka_lbr_l_joint_5", "kuka_lbr_l_joint_6", "kuka_lbr_l_joint_7"] - r.joint_names = r.actuated_joint_names - assert robot_model.configure(r) == True - - qp_solver = QPOASESSolver() - hls_solver = HierarchicalLSSolver() - - joint_state = Joints() - js = JointState() - js.position = 1.0 - js.speed = js.acceleration = 0.0 - joint_state.names = r.joint_names - joint_state.elements = [js]*len(r.actuated_joint_names) - robot_model.update(joint_state) - - run_velocity_wbc(VelocityScene(robot_model, hls_solver, 0.001), robot_model) - run_velocity_wbc(VelocitySceneQP(robot_model, qp_solver, 0.001), robot_model) - run_acceleration_wbc(AccelerationScene(robot_model, qp_solver, 0.001), robot_model) - run_acceleration_wbc(AccelerationSceneTSID(robot_model, qp_solver, 0.001), robot_model) - # Fix me: reduced TSID does not find a solution for this problem! - #run_acceleration_wbc(AccelerationSceneReducedTSID(robot_model, qp_solver, 0.001), robot_model) - - -if __name__ == '__main__': - nose.run() diff --git a/bindings/python/test/test_scenes.pyc b/bindings/python/test/test_scenes.pyc deleted file mode 100644 index b7bc07b4..00000000 Binary files a/bindings/python/test/test_scenes.pyc and /dev/null differ diff --git a/bindings/python/test/test_solvers.py b/bindings/python/test/test_solvers.py deleted file mode 100644 index ee3f9f54..00000000 --- a/bindings/python/test/test_solvers.py +++ /dev/null @@ -1,49 +0,0 @@ -from wbc.solvers.hls_solver import * -from wbc.solvers.qpoases_solver import * -from wbc.core import * -import nose -import numpy as np - -def run(solver): - nc = 6 - nj = 6 - - hqp = HierarchicalQP() - qp = QuadraticProgram() - qp.resize(nj,nc,0,False) - qp.A = [[0.642, 0.706, 0.565, 0.48, 0.59, 0.917], - [0.553, 0.087, 0.43, 0.71, 0.148, 0.87], - [0.249, 0.632, 0.711, 0.13, 0.426, 0.963], - [0.682, 0.123, 0.998, 0.716, 0.961, 0.901], - [0.315, 0.551, 0.462, 0.221, 0.638, 0.244], - [0.891, 0.019, 0.716, 0.534, 0.725, 0.633]] - y_ref = [0.833, 0.096, 0.078, 0.971, 0.883, 0.366] - qp.b = y_ref - qp.g = [0]*nj - qp.H = np.eye(nj).tolist() - hqp.prios = [qp] - solver_output = solver.solve(hqp) - y_solution = np.array(qp.A).dot(solver_output) - assert np.all(np.isclose(y_solution - y_ref,np.zeros(nj))) - -def test_hierarchial_ls_solver(): - solver = HierarchicalLSSolver() - solver.setMaxSolverOutputNorm(100.0) - assert solver.getMaxSolverOutputNorm() == 100.0 - solver.setMinEigenvalue(1e-7) - assert solver.getMinEigenvalue() == 1e-7 - - run(solver) - -def test_qp_oases_solver(): - solver = QPOASESSolver() - solver.setMaxNoWSR(100) - assert solver.getMaxNoWSR() == 100 - - run(solver) - - assert(solver.getNoWSR() < 100) - assert(solver.getReturnValue() == 0) - -if __name__ == '__main__': - test_qp_oases_solver() diff --git a/bindings/python/wbc_types_conversions.h b/bindings/python/wbc_types_conversions.h deleted file mode 100644 index 60d68cf8..00000000 --- a/bindings/python/wbc_types_conversions.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include "core/RobotModelConfig.hpp" - -namespace wbc_py{ -wbc::ActiveContacts toActiveContacts(const base::NamedVector &contacts_in){ - wbc::ActiveContacts contacts_out; - contacts_out.elements = contacts_in.elements; - contacts_out.names = contacts_in.names; - return contacts_out; -} - -base::NamedVector fromActiveContacts(const wbc::ActiveContacts& contacts_in){ - base::NamedVector contacts_out; - contacts_out.names = contacts_in.names; - contacts_out.elements = contacts_in.elements; - return contacts_out; -} - -class RobotModelConfig : public wbc::RobotModelConfig{ -public: - base::NamedVector getActiveContacts(){ - return fromActiveContacts(contact_points); - } - void setActiveContacts(base::NamedVector contacts_in){ - contact_points = toActiveContacts(contacts_in); - } -}; - -wbc::RobotModelConfig toRobotModelConfig(RobotModelConfig cfg_in){ - RobotModelConfig cfg = cfg_in; - return cfg; -} - -RobotModelConfig fromRobotModelConfig(wbc::RobotModelConfig cfg_in){ - RobotModelConfig cfg; - cfg.file_or_string = cfg_in.file_or_string; - cfg.submechanism_file = cfg_in.submechanism_file; - cfg.floating_base = cfg_in.floating_base; - cfg.contact_points = cfg_in.contact_points; - return cfg; -} -} diff --git a/manifest.xml b/manifest.xml index 131ef798..d28446ba 100644 --- a/manifest.xml +++ b/manifest.xml @@ -25,9 +25,7 @@ c++ - rock ubuntu - python diff --git a/scripts/full_install.sh b/scripts/full_install.sh index 48437f1d..ca06b4cc 100644 --- a/scripts/full_install.sh +++ b/scripts/full_install.sh @@ -89,11 +89,8 @@ mkdir build && cd build cmake .. -DBUILD_TESTING=OFF -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_WITH_VECTORIZATION_SUPPORT=OFF make -j8 && sudo make install && cd ../.. -# For Python bindings -sudo apt-get -y install python3-dev python3-numpy python3-nose libboost-python-dev libboost-numpy-dev - # WBC mkdir wbc/build && cd wbc/build -cmake .. -DUSE_PYTHON=1 -DUSE_EIQUADPROG=1 -DUSE_KDL=1 -DUSE_PINOCCHIO=1 -DUSE_QPSWIFT=1 -DUSE_PROXQP=1 +cmake .. -DUSE_EIQUADPROG=1 -DUSE_KDL=1 -DUSE_PINOCCHIO=1 -DUSE_QPSWIFT=1 -DUSE_PROXQP=1 make -j8 && sudo make install && cd ../.. sudo ldconfig diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh index 078476f6..bc4cfeb1 100644 --- a/scripts/run_tests.sh +++ b/scripts/run_tests.sh @@ -75,11 +75,3 @@ if [ -d "qpswift" ]; then ./test_qpswift_solver cd ../../.. fi - -# Python bindings -#if [ -d "bindings" ]; then -# echo "Testing Python bindings ..." -# cd ../bindings/python/test -# nosetests3 -#fi - diff --git a/src/robot_models/rbdl/CMakeLists.txt b/src/robot_models/rbdl/CMakeLists.txt index dd53ab61..16184a65 100644 --- a/src/robot_models/rbdl/CMakeLists.txt +++ b/src/robot_models/rbdl/CMakeLists.txt @@ -17,6 +17,8 @@ target_link_libraries(${TARGET_NAME} PUBLIC PkgConfig::rbdl PkgConfig::rbdl-urdfreader) +target_include_directories(${TARGET_NAME} PUBLIC ../../core) + set_target_properties(${TARGET_NAME} PROPERTIES VERSION ${PROJECT_VERSION} SOVERSION ${API_VERSION}) diff --git a/src/robot_models/rbdl/RobotModelRBDL.hpp b/src/robot_models/rbdl/RobotModelRBDL.hpp index f0831abb..8246106c 100644 --- a/src/robot_models/rbdl/RobotModelRBDL.hpp +++ b/src/robot_models/rbdl/RobotModelRBDL.hpp @@ -1,7 +1,7 @@ #ifndef ROBOT_MODEL_RBDL_HPP #define ROBOT_MODEL_RBDL_HPP -#include "core/RobotModel.hpp" +#include "../../core/RobotModel.hpp" #include namespace wbc {