initial package cleanup

include/sas_robot_driver_franka/robot_interface_franka.h

@@ -0,0 +1,257 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Original implementation
+#
+# ################################################################
+*/
+
+
+#pragma once
+
+#include <dqrobotics/DQ.h>
+#include <memory.h>
+#include <tuple>
+#include <exception>
+#include <vector>
+#include <franka/robot.h>
+#include <franka/gripper.h>
+#include <franka/exception.h>
+#include "generator/motion_generator.h"
+#include <thread>
+#include "generator/quadratic_program_motion_generator.h"
+#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
+#include <atomic>
+#include "generator/custom_motion_generation.h"
+
+using namespace DQ_robotics;
+using namespace Eigen;
+
+/**
+ * @brief get homgenious_tf_to_DQ
+ * @param pose_vector (column major)
+ * @return pose
+ */
+inline DQ homgenious_tf_to_DQ(const VectorXd& pose_vector) {
+    // VectorXd column major
+    const auto t = DQ(0,  pose_vector(12), pose_vector(13), pose_vector(14));
+    Eigen::Matrix<double, 3, 3, Eigen::ColMajor> rot_mat;
+    rot_mat << pose_vector(0), pose_vector(4), pose_vector(8),
+               pose_vector(1), pose_vector(5), pose_vector(9),
+               pose_vector(2), pose_vector(6), pose_vector(10);
+    Quaternion<double> quaternion(rot_mat);
+    const auto r = normalize(DQ(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()));
+    return r + 0.5 * E_ * t * r;
+}
+
+class RobotInterfaceFranka
+{
+public:
+    struct FrankaInterfaceConfiguration {
+        std::array<double, 7> lower_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 7> upper_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 6> lower_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 6> upper_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 7> joint_impedance={3000, 3000, 3000, 2500, 2500, 2000, 2000};  // K_theta (Nm/rad)
+        std::array<double, 6> cartesian_impedance={3000, 3000, 3000, 300, 300, 300}; // K_x (N/m)
+    };
+
+    enum class MODE{
+        None=0,
+        PositionControl,
+        VelocityControl,
+        ForceControl,
+        Homing,
+        ClearPositions,
+    };
+
+private:
+    using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
+    std::string ip_ = "172.16.0.2";
+    FrankaInterfaceConfiguration franka_configuration_;
+    double speed_factor_joint_position_controller_ = 0.2;
+    double speed_gripper_ = 0.02;
+    VectorXd desired_joint_positions_;
+    VectorXd desired_joint_velocities_ = VectorXd::Zero(7);
+
+    VectorXd current_joint_positions_;
+    std::array<double, 7> current_joint_positions_array_;
+
+    VectorXd current_joint_velocities_;
+    std::array<double, 7> current_joint_velocities_array_;
+
+    VectorXd current_joint_forces_;
+    std::array<double, 7> current_joint_forces_array_;
+
+    Vector3d current_stiffness_force_;
+    Vector3d current_stiffness_torque_;
+    std::array<double, 6> current_stiffness_force_torque_array_;
+
+    VectorXd current_stiffness_pose_;
+    VectorXd current_effeector_pose_;
+    std::array<double, 16> current_stiffness_pose_array_;
+    std::array<double, 16> current_effector_pose_array_;
+
+    franka::RobotMode robot_mode_;
+
+    double time_ = 0;
+    bool initialize_flag_ = false;
+    void _check_if_robot_is_connected();
+    void _check_if_hand_is_connected();
+
+    RobotInterfaceFranka::MODE mode_;
+    void _set_driver_mode(const RobotInterfaceFranka::MODE& mode);
+
+    void _restart_default_parameters();
+
+
+    void _update_robot_state(const franka::RobotState& robot_state, const double& time);
+
+    std::shared_ptr<franka::Robot> robot_sptr_;
+    std::shared_ptr<franka::Gripper> gripper_sptr_;
+    std::unique_ptr<QuadraticProgramMotionGenerator> trajectory_generator_sptr_;
+    std::unique_ptr<CustomMotionGeneration> custom_generator_sptr_;
+
+    void _setDefaultRobotBehavior();
+    //bool hand_enabled_ = false;
+    const VectorXd q_home_configuration_ =(VectorXd(7)<<0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4).finished();
+
+    const VectorXd q_max_ = ((VectorXd(7) <<  2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094,  2.6895).finished());
+    const VectorXd q_min_ = ((VectorXd(7) << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished());
+    const VectorXd q_min_dot_ = ((VectorXd(7) << -2, -1, -1.5, -1.25, -3, -1.5, -3).finished());
+    const VectorXd q_max_dot_ = ((VectorXd(7) <<  2,  1,  1.5,  1.25,  3,  1.5,  3).finished());
+
+    const double samples_ = 20;
+
+    VectorXd desired_mean_joint_positions_;
+
+    bool verbose_ = true;
+    std::string status_message_ = " ";
+    void _update_status_message(const std::string& message, const bool& verbose = true);
+
+
+    //-------To handle the threads-----------------
+    std::atomic<bool> exit_on_err_={false};
+
+
+    void _start_joint_position_control_mode();
+    std::thread joint_position_control_mode_thread_;
+    void _start_joint_position_control_thread();
+    std::atomic<bool> finish_motion_;
+
+    void _start_echo_robot_state_mode();
+    std::thread echo_robot_state_mode_thread_;
+    void _start_echo_robot_state_mode_thread();
+    std::atomic<bool> finish_echo_robot_state_;
+    void _finish_echo_robot_state();
+
+    void _start_joint_velocity_control_mode();
+    std::thread joint_velocity_control_mode_thread_;
+    void _start_joint_velocity_control_thread();
+    //----------------------------------------------
+
+
+
+    VectorXd _compute_recursive_mean(const double& samples, const VectorXd& q);
+    VectorXd _read_once_smooth_initial_positions(const double& samples);
+
+
+public:
+
+
+    enum HAND{ON=0, OFF};
+    enum CONNECTION{AUTOMATIC};
+    enum GRIPPER_STATES{WIDTH=0, MAX_WIDTH=1};
+    RobotInterfaceFranka() = delete;
+    RobotInterfaceFranka(const RobotInterfaceFranka&) = delete;
+    RobotInterfaceFranka& operator= (const RobotInterfaceFranka&) = delete;
+    RobotInterfaceFranka(
+        const FrankaInterfaceConfiguration &configuration,
+        const std::string &ROBOT_IP,
+        const MODE& mode,
+        const HAND& hand = ON);
+
+
+
+    VectorXd read_once_initial_positions();
+
+    VectorXd get_joint_target_positions();
+
+    void move_robot_to_target_joint_positions(const VectorXd& q_target);
+
+
+
+
+    double read_gripper(const GRIPPER_STATES& gripper_state = WIDTH);
+    bool read_grasped_status();
+    void set_gripper(const double& width);
+    void gripper_homing();
+    VectorXd get_home_robot_configuration();
+
+    std::string get_status_message();
+
+    bool get_err_state() const {return exit_on_err_.load();}
+
+    std::string get_robot_mode();
+
+
+    void finish_motion();
+
+    std::shared_ptr<franka::Robot> get_robot_pointer();
+
+
+    //--------sas compatible methods----------//
+    VectorXd get_joint_positions();
+    VectorXd get_joint_velocities();
+    VectorXd get_joint_forces();
+
+    std::tuple<Vector3d, Vector3d> get_stiffness_force_torque();
+    DQ get_stiffness_pose();
+
+    double get_time();
+
+    void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad);
+    void connect();
+    void initialize();
+    void deinitialize();
+    void disconnect();
+
+    void set_target_joint_velocities(const VectorXd& target_joint_velocities);
+
+
+
+
+
+
+    /* To be implemented
+    std::tuple<VectorXd, VectorXd> get_joint_limits() const;
+    void set_joint_limits(const std::tuple<VectorXd, VectorXd>& joint_limits);
+    */
+
+
+};
+
+

include/sas_robot_driver_franka/sas_robot_driver_franka.h

@@ -43,13 +43,15 @@
 #include <sas_robot_driver/sas_robot_driver.h>
 #include "robot_interface_franka.h"
 #include <ros/common.h>
-#include <robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_clock/sas_clock.h>
+#include <dqrobotics/utils/DQ_Math.h>
+#include <ros/this_node.h>
+#include <rosconsole/macros_generated.h>
 
 using namespace DQ_robotics;
 using namespace Eigen;
 
-struct RobotInterfaceFranka::FrankaInterfaceConfiguration;  // Forward declaration
-
 namespace sas
 {