diff --git a/CMakeLists.txt b/CMakeLists.txt index 40d521b..abd97d6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -244,22 +244,22 @@ install(TARGETS ##### PYBIND11 LIBRARY ROBOT_DYNAMICS ##### ament_python_install_package(${PROJECT_NAME}) -pybind11_add_module(_robot_dynamics SHARED +pybind11_add_module(_qros_franka_robot_dynamic SHARED src/robot_dynamics/qros_robot_dynamics_py.cpp src/robot_dynamics/qros_robot_dynamics_client.cpp src/robot_dynamics/qros_robot_dynamics_server.cpp ) -target_include_directories(_robot_dynamics +target_include_directories(_qros_franka_robot_dynamic PUBLIC $ $) -target_compile_definitions(_robot_dynamics PRIVATE IS_SAS_PYTHON_BUILD) +target_compile_definitions(_qros_franka_robot_dynamic PRIVATE IS_SAS_PYTHON_BUILD) # https://github.com/pybind/pybind11/issues/387 -target_link_libraries(_robot_dynamics PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics) +target_link_libraries(_qros_franka_robot_dynamic PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics) -install(TARGETS _robot_dynamics +install(TARGETS _qros_franka_robot_dynamic DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) ##END## PYBIND11 LIBRARY ROBOT_DYNAMICS ##### diff --git a/CMakeLists_catkin.txt b/CMakeLists_catkin.txt deleted file mode 100644 index 8a21bce..0000000 --- a/CMakeLists_catkin.txt +++ /dev/null @@ -1,239 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(sas_robot_driver_franka) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) -#add_compile_options(-std=c++11) -add_compile_options(-Werror=return-type) - - -#Add custom (non compiling) targets so launch scripts and python files show up in QT Creator's project view. -file(GLOB_RECURSE EXTRA_FILES */*) -#add_custom_target(${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES}) -add_custom_target(cfg ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES}) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - tf2_ros - tf2 - sas_common - sas_clock - sas_robot_driver - sas_patient_side_manager - message_generation - pybind11_catkin -) - - -add_service_files( - DIRECTORY srv - FILES - Move.srv - Grasp.srv -) - -add_message_files( - DIRECTORY msg - FILES - GripperState.msg -) - -catkin_python_setup() - -generate_messages( - DEPENDENCIES - std_msgs - -) - - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin message_runtime std_msgs -) - -find_package(Franka REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(yaml-cpp REQUIRED) -include_directories(${EIGEN3_INCLUDE_DIR}) - -# To correctly find and link with QT -set(CMAKE_PREFIX_PATH $ENV{QT_PATH}) -set(CMAKE_AUTOMOC ON) -set(CMAKE_AUTORCC ON) -set(CMAKE_AUTOUIC ON) -if (CMAKE_VERSION VERSION_LESS "3.7.0") - set(CMAKE_INCLUDE_CURRENT_DIR ON) -endif () -find_package(Qt5 COMPONENTS Widgets REQUIRED) - -find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets) -find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets) - - -add_library(MotionGenerator src/generator/motion_generator.cpp) -target_link_libraries(MotionGenerator Franka::Franka) - - -add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp) - -add_library(QuadraticProgramMotionGenerator src/generator/quadratic_program_motion_generator.cpp) -target_link_libraries(QuadraticProgramMotionGenerator - qpOASES - dqrobotics - ConstraintsManager) - -add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp) -target_link_libraries(CustomMotionGeneration - qpOASES - dqrobotics - ConstraintsManager) - -add_library(robot_interface_franka src/joint/robot_interface_franka.cpp) -target_link_libraries(robot_interface_franka Franka::Franka - dqrobotics - MotionGenerator - ConstraintsManager - QuadraticProgramMotionGenerator - CustomMotionGeneration) - -add_library(robot_interface_hand src/hand/robot_interface_hand.cpp) -target_link_libraries(robot_interface_hand Franka::Franka - dqrobotics) - - -############ -## Build ### -############ - -## Specify additional locations of header files -## Your package locations should be listed before other locations - - -include_directories( - include - include/generator - src/ - src/robot_dynamics - src/hand - src/joint - ${catkin_INCLUDE_DIRS} - constraints_manager/include -) - - -add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp) -target_link_libraries(qros_robot_dynamics_provider - ${catkin_LIBRARIES} - dqrobotics) - -add_library(qros_robot_dynamics_interface src/robot_dynamics/qros_robot_dynamics_interface.cpp) -target_link_libraries(qros_robot_dynamics_interface - ${catkin_LIBRARIES} - dqrobotics) - - -add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp) -target_link_libraries(sas_robot_driver_franka - qros_robot_dynamics_provider - dqrobotics - dqrobotics-interface-json11 - robot_interface_franka) - -add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp) -target_link_libraries(qros_effector_driver_franka_hand - dqrobotics -# robot_interface_hand - Franka::Franka) - - -add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp) -target_link_libraries(sas_robot_driver_coppelia - dqrobotics - dqrobotics-interface-vrep) - -add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(qros_effector_driver_franka_hand ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - - -add_executable(sas_robot_driver_coppelia_node src/sas_robot_driver_coppelia_node.cpp) -target_link_libraries(sas_robot_driver_coppelia_node - sas_robot_driver_coppelia - ${catkin_LIBRARIES}) - -add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp) -target_link_libraries(sas_robot_driver_franka_node - sas_robot_driver_franka - ${catkin_LIBRARIES}) - - -add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp) -target_link_libraries(sas_robot_driver_franka_hand_node - qros_effector_driver_franka_hand - ${catkin_LIBRARIES}) - - -add_executable(JuankaEmika - qt/configuration_window/main.cpp - qt/configuration_window/mainwindow.cpp - qt/configuration_window/mainwindow.ui -) - -target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets - dqrobotics - ${catkin_LIBRARIES} - robot_interface_franka -) - - -##################################################################################### -# python binding -include_directories( - include/robot_dynamic -) -pybind_add_module(_qros_robot_dynamic SHARED - src/robot_dynamics/qros_robot_dynamic_py.cpp src/robot_dynamics/qros_robot_dynamics_interface.cpp src/robot_dynamics/qros_robot_dynamics_provider.cpp -) -target_compile_definitions(_qros_robot_dynamic PRIVATE BUILD_PYBIND) -# https://github.com/pybind/pybind11/issues/387 -target_link_libraries(_qros_robot_dynamic PRIVATE ${catkin_LIBRARIES} -ldqrobotics) - - -if (QT_VERSION_MAJOR EQUAL 6) - qt_finalize_executable(JuankaEmika) -endif () - -install(TARGETS ${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) - -install(TARGETS sas_robot_driver_franka_node - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(TARGETS sas_robot_driver_coppelia_node - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(TARGETS sas_robot_driver_franka_hand_node - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - # PATTERN ".svn" EXCLUDE -) - - -install(TARGETS _qros_robot_dynamic - LIBRARY DESTINATION ${PYTHON_INSTALL_DIR} -) - diff --git a/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp b/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp index 0c914aa..02728e5 100644 --- a/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp +++ b/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp @@ -62,7 +62,7 @@ private: std::shared_ptr tf_broadcaster_; std::shared_ptr static_base_tf_broadcaster_; - DQ world_to_base_tf_; + DQ world_to_base_tf_ = DQ(0); static geometry_msgs::msg::Transform _dq_to_geometry_msgs_transform(const DQ& pose); diff --git a/sas_robot_driver_franka/__init__.py b/sas_robot_driver_franka/__init__.py index 24bea56..b4d84ca 100644 --- a/sas_robot_driver_franka/__init__.py +++ b/sas_robot_driver_franka/__init__.py @@ -1,6 +1,6 @@ """ """ -from sas_robot_driver_franka._robot_dynamics import RobotDynamicsInterface, RobotDynamicsProvider +from sas_robot_driver_franka._qros_franka_robot_dynamic import RobotDynamicsClient, RobotDynamicsServer import rclpy from rclpy.node import Node diff --git a/src/robot_dynamics/qros_robot_dynamics_py.cpp b/src/robot_dynamics/qros_robot_dynamics_py.cpp index 391c114..aa8e8ef 100644 --- a/src/robot_dynamics/qros_robot_dynamics_py.cpp +++ b/src/robot_dynamics/qros_robot_dynamics_py.cpp @@ -40,7 +40,7 @@ using RDC = qros::RobotDynamicsClient; using RDS = qros::RobotDynamicsServer; -PYBIND11_MODULE(_qros_robot_dynamic, m) +PYBIND11_MODULE(_qros_franka_robot_dynamic, m) { py::class_(m, "RobotDynamicsClient") .def(py::init&,const std::string&>()) diff --git a/src/robot_dynamics/qros_robot_dynamics_server.cpp b/src/robot_dynamics/qros_robot_dynamics_server.cpp index 4b807fc..2e29054 100644 --- a/src/robot_dynamics/qros_robot_dynamics_server.cpp +++ b/src/robot_dynamics/qros_robot_dynamics_server.cpp @@ -34,9 +34,9 @@ using namespace qros; RobotDynamicsServer::RobotDynamicsServer(const std::shared_ptr &node, const std::string& topic_prefix): node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix), child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"), - world_to_base_tf_(0), tf_broadcaster_(std::make_shared(node_)), - static_base_tf_broadcaster_(std::make_shared(node_)) + static_base_tf_broadcaster_(std::make_shared(node_)), + world_to_base_tf_(0) { // Strip potential leading slash if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}