diff --git a/CMakeLists.txt b/CMakeLists.txt index f96e5b6..d1184a9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,24 +16,21 @@ add_custom_target(cfg ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTR ## 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 + roscpp + rospy + std_msgs + tf2_ros + tf2 + sas_common + sas_clock + sas_robot_driver + sas_patient_side_manager pybind11_catkin - ) catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 - pybind11_catkin - + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin ) find_package(Franka REQUIRED) @@ -46,9 +43,9 @@ 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") +if (CMAKE_VERSION VERSION_LESS "3.7.0") set(CMAKE_INCLUDE_CURRENT_DIR ON) -endif() +endif () find_package(Qt5 COMPONENTS Widgets REQUIRED) find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets) @@ -63,27 +60,27 @@ 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) + qpOASES + dqrobotics + ConstraintsManager) add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp) target_link_libraries(CustomMotionGeneration - qpOASES - dqrobotics - ConstraintsManager) + 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) + 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) + dqrobotics) ############ @@ -95,15 +92,15 @@ target_link_libraries(robot_interface_hand Franka::Franka include_directories( - include - include/generator - src/ + include + include/generator + src/ src/robot_dynamics - src/hand - src/joint - ${catkin_INCLUDE_DIRS} - constraints_manager/include - ) + src/hand + src/joint + ${catkin_INCLUDE_DIRS} + constraints_manager/include +) add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp) @@ -119,9 +116,10 @@ target_link_libraries(qros_robot_dynamics_interface 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 - robot_interface_franka) + qros_robot_dynamics_provider + dqrobotics + dqrobotics-interface-json11 + robot_interface_franka) add_library(sas_robot_driver_franka_hand src/hand/sas_robot_driver_franka_hand.cpp) target_link_libraries(sas_robot_driver_franka_hand @@ -131,21 +129,21 @@ target_link_libraries(sas_robot_driver_franka_hand add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp) target_link_libraries(sas_robot_driver_coppelia - dqrobotics - dqrobotics-interface-vrep) + dqrobotics + dqrobotics-interface-vrep) -add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +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_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}) + sas_robot_driver_coppelia + ${catkin_LIBRARIES}) -add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp) +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}) + sas_robot_driver_franka + ${catkin_LIBRARIES}) add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp) @@ -154,19 +152,17 @@ target_link_libraries(sas_robot_driver_franka_hand_node ${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 - ) + 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 +) ##################################################################################### @@ -182,34 +178,32 @@ target_compile_definitions(_qros_robot_dynamic PRIVATE BUILD_PYBIND) target_link_libraries(_qros_robot_dynamic PRIVATE ${catkin_LIBRARIES} -ldqrobotics) - - -if(QT_VERSION_MAJOR EQUAL 6) +if (QT_VERSION_MAJOR EQUAL 6) qt_finalize_executable(JuankaEmika) -endif() +endif () install(TARGETS ${PROJECT_NAME} -DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install(TARGETS sas_robot_driver_franka_node -DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(TARGETS sas_robot_driver_coppelia_node -DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(TARGETS sas_robot_driver_franka_hand_node -DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + 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 -) + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + # PATTERN ".svn" EXCLUDE +) install(TARGETS _qros_robot_dynamic diff --git a/include/robot_dynamic/qros_robot_dynamics_provider.h b/include/robot_dynamic/qros_robot_dynamics_provider.h index 6bc21a7..915b5b4 100644 --- a/include/robot_dynamic/qros_robot_dynamics_provider.h +++ b/include/robot_dynamic/qros_robot_dynamics_provider.h @@ -36,12 +36,14 @@ #include #include #include +#include #include #include #include #include #define REDUCE_TF_PUBLISH_RATE 10 +#define WORLD_FRAME_ID "world" namespace qros { @@ -57,9 +59,13 @@ private: ros::Publisher publisher_cartesian_stiffness_; tf2_ros::TransformBroadcaster tf_broadcaster_; + tf2_ros::StaticTransformBroadcaster static_base_tf_broadcaster_; + DQ world_to_base_tf_; - static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose) ; + static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose); + + void _publish_base_static_tf(); public: RobotDynamicProvider() = delete; @@ -72,6 +78,7 @@ public: void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque); + void set_world_to_base_tf(const DQ& world_to_base_tf); bool is_enabled() const; std::string get_topic_prefix() const {return node_prefix_;} diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index 61a4090..70ee6cf 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -60,6 +60,7 @@ struct RobotDriverFrankaConfiguration int port; double speed; RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration; + DQ robot_reference_frame = DQ(0); }; diff --git a/scripts/publish_dumy_robot_dynmaics.py b/scripts/publish_dumy_robot_dynmaics.py new file mode 100644 index 0000000..2b0123d --- /dev/null +++ b/scripts/publish_dumy_robot_dynmaics.py @@ -0,0 +1,29 @@ +import rospy +from sas_robot_driver_franka import RobotDynamicsProvider +import dqrobotics as dql +import numpy as np + + +if __name__ == "__main__": + rospy.init_node("dummy_robot_dynamics_provider") + + dynam_provider = RobotDynamicsProvider("/franka1") + t = dql.DQ([0, 0, 1]) + r = dql.DQ([1, 0, 0, 0]) + base_pose = r + 0.5 * dql.E_ * t * r + dynam_provider.set_world_to_base_tf(base_pose) + t_ = 0 + rospy.loginfo("Publishing dummy robot dynamics") + r = dql.DQ([0, 0, 0, 1]) + rate = rospy.Rate(100) + dummy_force = np.random.rand(3) * 100 + dummy_torque = np.random.rand(3) * 10 + + while not rospy.is_shutdown(): + t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi) + ee_pose = r + 0.5 * dql.E_ * t * r + dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10 + dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1 + dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque) + rate.sleep() + t_ += 0.01 diff --git a/scripts/subscribe_dummy_dynamic.py b/scripts/subscribe_dummy_dynamic.py new file mode 100644 index 0000000..136bba4 --- /dev/null +++ b/scripts/subscribe_dummy_dynamic.py @@ -0,0 +1,30 @@ +import rospy +from sas_robot_driver_franka import RobotDynamicsInterface +import dqrobotics as dql +import numpy as np + +from dqrobotics.interfaces.vrep import DQ_VrepInterface + +vrep = DQ_VrepInterface() +vrep.connect("192.168.10.103", 19997, 100, 10) + +if __name__ == "__main__": + rospy.init_node("dummy_robot_dynamics_subscriber") + + dynam_provider = RobotDynamicsInterface("/franka1") + while not dynam_provider.is_enabled(): + rospy.loginfo("Waiting for robot dynamics provider to be enabled") + rospy.sleep(1) + + rospy.loginfo("Subscribing to dummy robot dynamics") + rate = rospy.Rate(50) + + while not rospy.is_shutdown(): + force = dynam_provider.get_stiffness_force() + torque = dynam_provider.get_stiffness_torque() + ee_pose = dynam_provider.get_stiffness_frame_pose() + vrep.set_object_pose("xd1", ee_pose) + rospy.loginfo("EE Pose: %s", ee_pose) + rospy.loginfo("Force: %s", force) + rospy.loginfo("Torque: %s", torque) + rate.sleep() \ No newline at end of file diff --git a/src/robot_dynamics/qros_robot_dynamic_py.cpp b/src/robot_dynamics/qros_robot_dynamic_py.cpp index 3c50652..320affc 100644 --- a/src/robot_dynamics/qros_robot_dynamic_py.cpp +++ b/src/robot_dynamics/qros_robot_dynamic_py.cpp @@ -53,6 +53,7 @@ PYBIND11_MODULE(_qros_robot_dynamic, m) py::class_(m, "RobotDynamicsProvider") .def(py::init()) .def("publish_stiffness",&RDP::publish_stiffness) + .def("set_world_to_base_tf", &RDP::set_world_to_base_tf) .def("is_enabled",&RDP::is_enabled,"Returns true if the RobotDynamicProvider is enabled.") .def("get_topic_prefix",&RDP::get_topic_prefix); diff --git a/src/robot_dynamics/qros_robot_dynamics_interface.cpp b/src/robot_dynamics/qros_robot_dynamics_interface.cpp index 0e4d21a..647e35e 100644 --- a/src/robot_dynamics/qros_robot_dynamics_interface.cpp +++ b/src/robot_dynamics/qros_robot_dynamics_interface.cpp @@ -47,8 +47,10 @@ RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &publisher_nodehand last_stiffness_torque_(Vector3d::Zero()), last_stiffness_frame_pose_(0) { + // Strip potential leading slash + if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);} + if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);} ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicInterface with prefix " + node_prefix); - subscriber_cartesian_stiffness_ = subscriber_nodehandle.subscribe(node_prefix_ + "/get/cartesian_stiffness", 1, &RobotDynamicInterface::_callback_cartesian_stiffness, this); } @@ -64,7 +66,7 @@ void RobotDynamicInterface::_callback_cartesian_stiffness(const geometry_msgs::W last_stiffness_torque_(2) = msg->wrench.torque.z; try { - const auto transform_stamped = tf_buffer_.lookupTransform(parent_frame_id_, child_frame_id_, ros::Time(0)); + const auto transform_stamped = tf_buffer_.lookupTransform( parent_frame_id_, child_frame_id_, ros::Time(0)); last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform); } catch (tf2::TransformException &ex) { ROS_WARN_STREAM(ros::this_node::getName() + "::" + ex.what()); diff --git a/src/robot_dynamics/qros_robot_dynamics_provider.cpp b/src/robot_dynamics/qros_robot_dynamics_provider.cpp index 8dc555e..fcde8f5 100644 --- a/src/robot_dynamics/qros_robot_dynamics_provider.cpp +++ b/src/robot_dynamics/qros_robot_dynamics_provider.cpp @@ -40,8 +40,13 @@ RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const st RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix): node_prefix_(node_prefix), child_frame_id_(node_prefix + "_stiffness_frame"), - parent_frame_id_(node_prefix + "_base") + parent_frame_id_(node_prefix + "_base"), + world_to_base_tf_(0) { + // Strip potential leading slash + if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);} + if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);} + ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix); publisher_cartesian_stiffness_ = publisher_nodehandle.advertise(node_prefix + "/get/cartesian_stiffness", 1); @@ -63,6 +68,28 @@ geometry_msgs::Transform RobotDynamicProvider::_dq_to_geometry_msgs_transform(co return tf_msg; } +void RobotDynamicProvider::set_world_to_base_tf(const DQ& world_to_base_tf) +{ + if(world_to_base_tf_==0) + { + world_to_base_tf_ = world_to_base_tf; + _publish_base_static_tf(); + }else + { + throw std::runtime_error("The world to base transform has already been set"); + } +} + +void RobotDynamicProvider::_publish_base_static_tf() +{ + geometry_msgs::TransformStamped base_tf; + base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_); + base_tf.header.stamp = ros::Time::now(); + base_tf.header.frame_id = WORLD_FRAME_ID; + base_tf.child_frame_id = parent_frame_id_; + static_base_tf_broadcaster_.sendTransform(base_tf); + +} void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index a14ff77..4c707a9 100644 --- a/src/sas_robot_driver_franka_node.cpp +++ b/src/sas_robot_driver_franka_node.cpp @@ -35,7 +35,7 @@ #include #include -//#include +#include #include #include #include @@ -121,7 +121,14 @@ int main(int argc, char **argv) ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling."); franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor); } - + if(nh.hasParam(ros::this_node::getName()+"/robot_parameter_file_path")) + { + std::string robot_parameter_file_path; + sas::get_ros_param(nh,"/robot_parameter_file_path",robot_parameter_file_path); + ROS_INFO_STREAM(ros::this_node::getName()+"::Loading robot parameters from file: " + robot_parameter_file_path); + const auto robot = DQ_JsonReader::get_from_json(robot_parameter_file_path); + robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame(); + }else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");} robot_driver_franka_configuration.interface_configuration = franka_interface_configuration; @@ -134,7 +141,10 @@ int main(int argc, char **argv) // initialize the robot dynamic provider robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName(); qros::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix); - + if(robot_driver_franka_configuration.robot_reference_frame!=0) + { + robot_dynamic_provider.set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame); + } try {