Added source files

src/sas_robot_driver_franka.cpp

@@ -0,0 +1,120 @@
+#include "sas_robot_driver_franka.h"
+#include "sas_clock/sas_clock.h"
+#include <dqrobotics/utils/DQ_Math.h>
+
+namespace sas
+{
+    RobotDriverFranka::RobotDriverFranka(const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
+    RobotDriver(break_loops),
+    configuration_(configuration)
+   {
+        joint_positions_.resize(7);
+        joint_velocities_.resize(7);
+        joint_forces_.resize(7);
+        end_effector_pose_.resize(7);
+        joint_positions_buffer_.resize(8,0);
+        end_effector_pose_euler_buffer_.resize(7,0);
+        end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
+        std::cout<<configuration.ip_address<<std::endl;
+
+        robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(configuration.ip_address,
+                                                                        RobotInterfaceFranka::MODE::VelocityControl, //None, PositionControl
+                                                                        RobotInterfaceFranka::HAND::ON);
+    }
+
+    RobotDriverFranka::~RobotDriverFranka()=default;
+
+
+    /**
+     * @brief 
+     * 
+     */
+    void RobotDriverFranka::connect() 
+    {
+        robot_driver_interface_sptr_ ->connect();
+    }
+
+
+    /**
+     * @brief 
+     * 
+     */
+    void RobotDriverFranka::initialize()
+    {
+        robot_driver_interface_sptr_->initialize();
+    }
+
+
+    /**
+     * @brief 
+     * 
+     */
+    void RobotDriverFranka::deinitialize()
+    {
+        robot_driver_interface_sptr_->deinitialize();
+    }
+
+
+    /**
+     * @brief 
+     * 
+     */
+    void RobotDriverFranka::disconnect()
+    {
+        robot_driver_interface_sptr_->disconnect();
+    }
+
+
+    /**
+     * @brief 
+     * 
+     * @return VectorXd 
+     */
+    VectorXd  RobotDriverFranka::get_joint_positions()
+    {
+        return robot_driver_interface_sptr_->get_joint_positions();
+    }
+
+
+    /**
+     * @brief 
+     * 
+     * @param desired_joint_positions_rad 
+     */
+    void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
+    {
+        robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad);
+    }
+
+    /**
+     * @brief RobotDriverFranka::get_joint_velocities
+     * @return
+     */
+    VectorXd RobotDriverFranka::get_joint_velocities()
+    {
+        joint_velocities_ = robot_driver_interface_sptr_->get_joint_velocities();
+        return joint_velocities_;
+    }
+
+
+    /**
+     * @brief RobotDriverFranka::set_target_joint_velocities
+     * @param desired_joint_velocities
+     */
+    void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
+    {
+        desired_joint_velocities_ = desired_joint_velocities;
+        robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities);
+    }
+
+    /**
+     * @brief RobotDriverFranka::get_joint_forces
+     * @return
+     */
+    VectorXd RobotDriverFranka::get_joint_forces()
+    {
+        joint_forces_ = robot_driver_interface_sptr_->get_joint_forces();
+        return joint_forces_;
+    }
+
+}