diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e162345..9dac810b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,8 @@ find_package(PCL 1.7 QUIET COMPONENTS common #search #io) -find_package(eigen_conversions REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_eigen_kdl REQUIRED) find_package(xbot2_interface REQUIRED) find_package(PkgConfig REQUIRED) find_package(matlogger2 REQUIRED) @@ -84,7 +85,6 @@ set(OPENSOT_CONSTRAINTS_SOURCES src/constraints/Aggregated.cpp src/constraints/acceleration/TorqueLimits.cpp src/constraints/velocity/CartesianPositionConstraint.cpp src/constraints/velocity/CartesianVelocity.cpp - src/constraints/velocity/CoMVelocity.cpp src/constraints/velocity/JointLimits.cpp src/constraints/velocity/JointLimitsInvariance.cpp src/constraints/velocity/VelocityLimits.cpp @@ -194,10 +194,10 @@ ADD_LIBRARY(OpenSoT SHARED set(PRIVATE_TLL ${PRIVATE_TLL} ${eigen_conversions_LIBRARIES}) if(${OPENSOT_COMPILE_COLLISION}) - target_link_libraries(OpenSoT PUBLIC xbot2_interface::collision) + target_link_libraries(OpenSoT PUBLIC xbot2_interface::collision moveit_msgs::moveit_msgs__rosidl_generator_cpp tf2_eigen_kdl::tf2_eigen_kdl) endif() if(${PCL_FOUND}) - set(PRIVATE_TLL ${PRIVATE_TLL} ${QHULL_LIBRARIES} ${PCL_LIBRARIES}) + set(PRIVATE_TLL ${PRIVATE_TLL} ${QHULL_LIBRARIES} ${PCL_LIBRARIES} tf2_eigen::tf2_eigen ) endif() @@ -324,8 +324,6 @@ library_install(OpenSoT ${PROJECT_VERSION_PATCH}) -add_subdirectory(doc) - include(GenerateDeb) diff --git a/OpenSoTConfig.cmake.in b/OpenSoTConfig.cmake.in index 03d2944d..9a1b4d55 100644 --- a/OpenSoTConfig.cmake.in +++ b/OpenSoTConfig.cmake.in @@ -3,6 +3,9 @@ include(CMakeFindDependencyMacro) find_dependency(xbot2_interface) find_dependency(matlogger2) +find_dependency(moveit_msgs) +find_dependency(tf2_eigen_kdl) +find_dependency(tf2_eigen) set_and_check(OpenSoT_TARGETS "${CMAKE_CURRENT_LIST_DIR}/OpenSoTTargets.cmake") diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index cf700166..6b2d3972 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -1,12 +1,6 @@ -cmake_minimum_required(VERSION 3.5) - -if(${CMAKE_VERSION} VERSION_LESS "3.12.0") - message("Please consider to switch to latest version to enable - more reliable Python3 binaries detection") -else() - #find_package(Python ${PYBIND11_PYTHON_VERSION} COMPONENTS Development Interpreter REQUIRED) -endif() +cmake_minimum_required(VERSION 3.20) +find_package(Python ${PYBIND11_PYTHON_VERSION} COMPONENTS Development Interpreter REQUIRED) find_package(pybind11 QUIET) message(STATUS "check for pybind11") diff --git a/include/OpenSoT/solvers/OSQPBackEnd.h b/include/OpenSoT/solvers/OSQPBackEnd.h index 1d5fa09b..07e2ab1a 100644 --- a/include/OpenSoT/solvers/OSQPBackEnd.h +++ b/include/OpenSoT/solvers/OSQPBackEnd.h @@ -141,8 +141,8 @@ class OSQPBackEnd: public BackEnd{ private: - typedef Eigen::SparseMatrix SparseMatrix; - typedef Eigen::SparseMatrix SparseMatrixRowMajor; + typedef Eigen::SparseMatrix SparseMatrix; + typedef Eigen::SparseMatrix SparseMatrixRowMajor; /** * @brief __generate_data_struct creates DENSE Hessian and Constraints matrices using a SPARSE representation. diff --git a/src/solvers/OSQPBackEnd.cpp b/src/solvers/OSQPBackEnd.cpp index c6ead735..9d48d271 100644 --- a/src/solvers/OSQPBackEnd.cpp +++ b/src/solvers/OSQPBackEnd.cpp @@ -107,7 +107,7 @@ void OpenSoT::solvers::OSQPBackEnd::update_data_struct() } -void OSQPBackEnd::setCSCMatrix(csc* a, Eigen::SparseMatrix& A) +void OSQPBackEnd::setCSCMatrix(csc* a, SparseMatrix& A) { a->m = A.rows(); a->n = A.cols(); diff --git a/src/tasks/acceleration/Cartesian.cpp b/src/tasks/acceleration/Cartesian.cpp index 8c0c2e22..94d79cea 100644 --- a/src/tasks/acceleration/Cartesian.cpp +++ b/src/tasks/acceleration/Cartesian.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include using XBot::Logger; using namespace OpenSoT::tasks::acceleration; @@ -215,8 +215,8 @@ void Cartesian::setReference(const Eigen::Affine3d& pose_ref, void Cartesian::setReference(const KDL::Frame& pose_ref, const KDL::Twist& vel_ref) { - tf::transformKDLToEigen(pose_ref, _pose_ref); - tf::twistKDLToEigen(vel_ref, _vel_ref); + tf2::transformKDLToEigen(pose_ref, _pose_ref); + tf2::twistKDLToEigen(vel_ref, _vel_ref); _acc_ref.setZero(); _vel_ref_cached = _vel_ref; @@ -239,9 +239,9 @@ void Cartesian::setReference(const KDL::Frame& pose_ref, const KDL::Twist& vel_ref, const KDL::Twist& acc_ref) { - tf::transformKDLToEigen(pose_ref, _pose_ref); - tf::twistKDLToEigen(vel_ref, _vel_ref); - tf::twistKDLToEigen(acc_ref, _acc_ref); + tf2::transformKDLToEigen(pose_ref, _pose_ref); + tf2::twistKDLToEigen(vel_ref, _vel_ref); + tf2::twistKDLToEigen(acc_ref, _acc_ref); _vel_ref_cached = _vel_ref; _acc_ref_cached = _acc_ref; @@ -319,7 +319,7 @@ void Cartesian::getReference(Eigen::Affine3d& ref) const void Cartesian::getReference(KDL::Frame& ref) const { - tf::transformEigenToKDL(_pose_ref, ref); + tf2::transformEigenToKDL(_pose_ref, ref); } void Cartesian::getReference(Eigen::Affine3d& desiredPose, @@ -332,8 +332,8 @@ void Cartesian::getReference(Eigen::Affine3d& desiredPose, void Cartesian::getReference(KDL::Frame& desiredPose, KDL::Twist& desiredTwist) const { - tf::transformEigenToKDL(_pose_ref, desiredPose); - tf::twistEigenToKDL(_vel_ref, desiredTwist); + tf2::transformEigenToKDL(_pose_ref, desiredPose); + tf2::twistEigenToKDL(_vel_ref, desiredTwist); } void Cartesian::getReference(Eigen::Affine3d& desiredPose, @@ -349,9 +349,9 @@ void Cartesian::getReference(KDL::Frame& desiredPose, KDL::Twist& desiredTwist, KDL::Twist& desiredAcceleration) const { - tf::transformEigenToKDL(_pose_ref, desiredPose); - tf::twistEigenToKDL(_vel_ref, desiredTwist); - tf::twistEigenToKDL(_acc_ref, desiredAcceleration); + tf2::transformEigenToKDL(_pose_ref, desiredPose); + tf2::twistEigenToKDL(_vel_ref, desiredTwist); + tf2::twistEigenToKDL(_acc_ref, desiredAcceleration); } @@ -362,7 +362,7 @@ void Cartesian::getActualPose(Eigen::Affine3d& actual) const void Cartesian::getActualPose(KDL::Frame& actual) { - tf::transformEigenToKDL(_pose_current, actual); + tf2::transformEigenToKDL(_pose_current, actual); } const Eigen::Affine3d& Cartesian::getActualPose() const @@ -377,7 +377,7 @@ void Cartesian::getActualTwist(Eigen::Vector6d& actual) const void Cartesian::getActualTwist(KDL::Twist& actual) { - tf::twistEigenToKDL(_vel_current, actual); + tf2::twistEigenToKDL(_vel_current, actual); } const Eigen::Vector6d& Cartesian::getActualTwist() const diff --git a/src/utils/cartesian_utils.cpp b/src/utils/cartesian_utils.cpp index 5c2aeb85..73c6d814 100644 --- a/src/utils/cartesian_utils.cpp +++ b/src/utils/cartesian_utils.cpp @@ -19,8 +19,7 @@ #include #include -#include - +#include #define toDeg(X) (X*180.0/M_PI) @@ -45,14 +44,14 @@ void cartesian_utils::computeCartesianError(const Eigen::Matrix4d &T, KDL::Frame x; // ee pose x.Identity(); Eigen::Matrix4d tmp = T; - tf::transformEigenToKDL(Eigen::Affine3d(tmp),x); + tf2::transformEigenToKDL(Eigen::Affine3d(tmp),x); quaternion q; x.M.GetQuaternion(q.x, q.y, q.z, q.w); KDL::Frame xd; // ee desired pose xd.Identity(); tmp = Td; - tf::transformEigenToKDL(Eigen::Affine3d(tmp),xd); + tf2::transformEigenToKDL(Eigen::Affine3d(tmp),xd); quaternion qd; xd.M.GetQuaternion(qd.x, qd.y, qd.z, qd.w);