Added source files

include/sas_robot_driver_franka.h

@@ -0,0 +1,89 @@
+#pragma once
+
+
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+
+#include <dqrobotics/DQ.h>
+
+#include <sas_robot_driver/sas_robot_driver.h>
+#include "robot_interface_franka.h"
+
+using namespace DQ_robotics;
+using namespace Eigen;
+
+namespace sas
+{
+
+
+struct RobotDriverFrankaConfiguration
+{
+    std::string ip_address;
+    int port;
+    double speed;
+};
+
+
+class RobotDriverFranka: public RobotDriver
+{
+private:
+    RobotDriverFrankaConfiguration configuration_;
+
+    std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
+
+    //Joint positions
+    VectorXd joint_positions_;
+    //VectorXd joint_velocities_;
+    VectorXd end_effector_pose_;
+    std::vector<double> joint_positions_buffer_;
+    std::vector<double> end_effector_pose_euler_buffer_;
+    std::vector<double> end_effector_pose_homogenous_transformation_buffer_;
+
+    //DQ _homogenous_vector_to_dq(const VectorXd& homogenousvector) const;
+    //VectorXd _dq_to_homogenous_vector(const DQ& pose) const;
+    //VectorXd _get_end_effector_pose_homogenous_transformation();
+    //double _sign(const double &a) const;
+
+  //  void _connect(); //Throws std::runtime_error()
+
+   // void _motor_on(); //Throws std::runtime_error()
+   // void _motor_off() noexcept; //No exceptions should be thrown in the path to turn off the robot
+
+   // void _set_speed(const float& speed, const float& acceleration, const float& deacceleration); //Throws std::runtime_error()
+
+   // void _slave_mode_on(int mode); //Throws std::runtime_error()
+    //void _slave_mode_off() noexcept; //No exceptions should be thrown in the path to turn off the robot
+
+public:
+    const static int SLAVE_MODE_JOINT_CONTROL;
+    const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
+
+    RobotDriverFranka(const RobotDriverFranka&)=delete;
+    RobotDriverFranka()=delete;
+    ~RobotDriverFranka();
+
+    RobotDriverFranka(const RobotDriverFrankaConfiguration& configuration, std::atomic_bool* break_loops);
+
+
+    VectorXd get_joint_positions() override;
+    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
+
+    VectorXd get_joint_velocities() override;
+    void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
+
+    VectorXd get_joint_forces() override;
+
+    void connect() override;
+    void disconnect() override;
+
+    void initialize() override;
+    void deinitialize() override;
+
+    bool set_end_effector_pose_dq(const DQ& pose);
+    DQ get_end_effector_pose_dq();
+    
+};
+}