more optimization..

CMakeLists.txt

@@ -26,14 +26,11 @@ find_package(catkin REQUIRED COMPONENTS
         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
+        CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin
-        pybind11_catkin
-
 )
 
 find_package(Franka REQUIRED)
@@ -121,6 +118,7 @@ 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(sas_robot_driver_franka_hand src/hand/sas_robot_driver_franka_hand.cpp)
@@ -154,7 +152,6 @@ target_link_libraries(sas_robot_driver_franka_hand_node
         ${catkin_LIBRARIES})
 
 
-
 add_executable(JuankaEmika
         qt/configuration_window/main.cpp
         qt/configuration_window/mainwindow.cpp
@@ -168,7 +165,6 @@ target_link_libraries(JuankaEmika  PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
 )
 
 
-
 #####################################################################################
 # python binding
 include_directories(
@@ -182,8 +178,6 @@ 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)
     qt_finalize_executable(JuankaEmika)
 endif ()

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,10 +59,14 @@ 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);
 
+    void _publish_base_static_tf();
+
 public:
     RobotDynamicProvider() = delete;
     RobotDynamicProvider(const 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);
 
 }

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
         {