more optimization..

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
+        roscpp
-  rospy
+        rospy
-  std_msgs
+        std_msgs
-  tf2_ros
+        tf2_ros
-  tf2
+        tf2
-  sas_common
+        sas_common
-  sas_clock
+        sas_clock
-  sas_robot_driver
+        sas_robot_driver
-  sas_patient_side_manager
+        sas_patient_side_manager
         pybind11_catkin
-
 )
 
 catkin_package(
-  INCLUDE_DIRS include
+        INCLUDE_DIRS include
-  CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2
+        CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin
-        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
+        qpOASES
-                       dqrobotics
+        dqrobotics
-                       ConstraintsManager)
+        ConstraintsManager)
 
 add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp)
 target_link_libraries(CustomMotionGeneration
-                        qpOASES
+        qpOASES
-                        dqrobotics
+        dqrobotics
-                        ConstraintsManager)
+        ConstraintsManager)
 
 add_library(robot_interface_franka src/joint/robot_interface_franka.cpp)
 target_link_libraries(robot_interface_franka Franka::Franka
-                      dqrobotics
+        dqrobotics
-                      MotionGenerator
+        MotionGenerator
-                      ConstraintsManager
+        ConstraintsManager
-                      QuadraticProgramMotionGenerator
+        QuadraticProgramMotionGenerator
-                      CustomMotionGeneration)
+        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
-    include/generator
+        include/generator
-    src/
+        src/
         src/robot_dynamics
-    src/hand
+        src/hand
-    src/joint
+        src/joint
-    ${catkin_INCLUDE_DIRS}
+        ${catkin_INCLUDE_DIRS}
-    constraints_manager/include
+        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
+        qros_robot_dynamics_provider
-                    dqrobotics
+        dqrobotics
-                    robot_interface_franka)
+        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
-                       dqrobotics-interface-vrep)
+        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
+        sas_robot_driver_coppelia
-                      ${catkin_LIBRARIES})
+        ${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
+        sas_robot_driver_franka
-                      ${catkin_LIBRARIES})
+        ${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/main.cpp
-  qt/configuration_window/mainwindow.cpp
+        qt/configuration_window/mainwindow.cpp
-  qt/configuration_window/mainwindow.ui
+        qt/configuration_window/mainwindow.ui
-  )
+)
-
-target_link_libraries(JuankaEmika  PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
-    dqrobotics
-    ${catkin_LIBRARIES}
-    robot_interface_franka
-    )
 
+target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
+        dqrobotics
+        ${catkin_LIBRARIES}
+        robot_interface_franka
+)
 
 
 #####################################################################################
@@ -182,33 +178,31 @@ 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}
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
+        FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
+        #   PATTERN ".svn" EXCLUDE
 )
 
 

include/robot_dynamic/qros_robot_dynamics_provider.h

@@ -36,12 +36,14 @@
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros//static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <geometry_msgs/Pose.h>
 #include <std_msgs/Header.h>
 #include <dqrobotics/DQ.h>
 
 #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_;}

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);
 };
 
 

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

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()

src/robot_dynamics/qros_robot_dynamic_py.cpp

@@ -53,6 +53,7 @@ PYBIND11_MODULE(_qros_robot_dynamic, m)
     py::class_<RDP>(m, "RobotDynamicsProvider")
         .def(py::init<const std::string&>())
         .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);
 

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());

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<geometry_msgs::WrenchStamped>(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)

src/sas_robot_driver_franka_node.cpp

@@ -35,7 +35,7 @@
 
 #include <exception>
 #include <dqrobotics/utils/DQ_Math.h>
-//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
@@ -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<DQ_SerialManipulatorDH>(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
         {