added cartesian interface

include/robot_interface_franka.h

@@ -78,6 +78,13 @@ private:
     VectorXd current_joint_forces_;
     std::array<double, 7> current_joint_forces_array_;
 
+    Vector3d current_cartesian_force_;
+    Vector3d current_cartesian_torque_;
+    std::array<double, 6> current_cartesian_force_torque_array_;
+
+    VectorXd current_cartesian_pose_;
+    std::array<double, 16> current_cartesian_pose_array_;
+
     franka::RobotMode robot_mode_;
 
     double time_ = 0;
@@ -187,6 +194,9 @@ public:
     VectorXd get_joint_velocities();
     VectorXd get_joint_forces();
 
+    std::tuple<Vector3d, Vector3d> get_cartesian_force_torque();
+    DQ get_cartesian_pose();
+
     double get_time();
 
     void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad);

include/sas_robot_driver_franka.h

@@ -43,6 +43,7 @@
 #include <sas_robot_driver/sas_robot_driver.h>
 #include "robot_interface_franka.h"
 #include <ros/common.h>
+#include "sas_robot_dynamic_provider.h"
 
 using namespace DQ_robotics;
 using namespace Eigen;
@@ -67,6 +68,8 @@ private:
 
     std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
 
+    RobotDynamicProvider* robot_dynamic_provider_;
+
     //Joint positions
     VectorXd joint_positions_;
     //VectorXd joint_velocities_;
@@ -77,6 +80,9 @@ private:
 
     std::atomic_bool* break_loops_;
 
+    // hotfix function to update cartesian contact and pose information
+    void _update_cartesian_contact_and_pose() const;
+
 
 public:
     //const static int SLAVE_MODE_JOINT_CONTROL;
@@ -86,7 +92,11 @@ public:
     RobotDriverFranka()=delete;
     ~RobotDriverFranka();
 
-    RobotDriverFranka(const RobotDriverFrankaConfiguration& configuration, std::atomic_bool* break_loops);
+    RobotDriverFranka(
+        RobotDynamicProvider* robot_dynamic_provider,
+        const RobotDriverFrankaConfiguration& configuration,
+        std::atomic_bool* break_loops
+        );
 
 
     VectorXd get_joint_positions() override;