initial package cleanup

2024-08-21 16:30:44 -07:00
parent a16c4fa5a6
commit dc2dcf51bc
25 changed files with 24 additions and 27 deletions
--- a/src/deprecated/robot_coppelia_ros_interface_node.cpp
+++ b/src/deprecated/robot_coppelia_ros_interface_node.cpp
@@ -39,7 +39,7 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "sas_robot_driver_franka.h"
+#include "../../include/sas_robot_driver_franka/sas_robot_driver_franka.h"
 #include "robot_coppelia_ros_interface.h"

 /*********************************************
--- a/src/generator/custom_motion_generation.cpp
+++ b/src/generator/custom_motion_generation.cpp
@@ -28,7 +28,7 @@
 #
 # ################################################################
 */
-#include "generator/custom_motion_generation.h"
+#include <sas_robot_driver_franka/generator/custom_motion_generation.h>

 /**
 * @brief CustomMotionGeneration::CustomMotionGeneration
--- a/src/generator/motion_generator.cpp
+++ b/src/generator/motion_generator.cpp
@@ -1,6 +1,6 @@
 // Copyright (c) 2017 Franka Emika GmbH
 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include "generator/motion_generator.h"
+#include <sas_robot_driver_franka/generator/motion_generator.h>
 #include <algorithm>
 #include <array>
 #include <cmath>
--- a/src/generator/quadratic_program_motion_generator.cpp
+++ b/src/generator/quadratic_program_motion_generator.cpp
@@ -1,4 +1,4 @@
-#include "generator/quadratic_program_motion_generator.h"
+#include <sas_robot_driver_franka/generator/quadratic_program_motion_generator.h>


 QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor,
--- a/src/hand/qros_effector_driver_franka_hand.cpp
+++ b/src/hand/qros_effector_driver_franka_hand.cpp
@@ -27,9 +27,8 @@
 #
 # ################################################################
 */
-#include "hand/qros_effector_driver_franka_hand.h"
+#include "qros_effector_driver_franka_hand.h"

-#include <franka/exception.h>


 namespace qros
--- a/src/hand/qros_effector_driver_franka_hand.h
+++ b/src/hand/qros_effector_driver_franka_hand.h
@@ -38,6 +38,7 @@
 #include <thread>
 #include <mutex>

+#include <franka/exception.h>
 // #define BLOCK_READ_IN_USED
 // #define IN_TESTING

--- a/src/hand/robot_interface_hand.cpp
+++ b/src/hand/robot_interface_hand.cpp
@@ -1,4 +1,4 @@
-#include "robot_interface_hand.h"
+#include <sas_robot_driver_franka/robot_interface_hand.h>



--- a/src/joint/robot_interface_franka.cpp
+++ b/src/joint/robot_interface_franka.cpp
@@ -30,7 +30,7 @@
 */


-#include "robot_interface_franka.h"
+#include <sas_robot_driver_franka/robot_interface_franka.h>

 #include <ros/this_node.h>
 #include <rosconsole/macros_generated.h>
--- a/src/joint/robot_interface_franka.h
+++ b/src/joint/robot_interface_franka.h
@@ -1,257 +0,0 @@
-/*
-# 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);
-    */
-
-
-};
-
-
--- a/src/joint/sas_robot_driver_franka.cpp
+++ b/src/joint/sas_robot_driver_franka.cpp
@@ -30,12 +30,8 @@
 # ################################################################
 */

+#include <sas_robot_driver_franka/sas_robot_driver_franka.h>

-#include "../../include/sas_robot_driver_franka.h"
-#include "sas_clock/sas_clock.h"
-#include <dqrobotics/utils/DQ_Math.h>
-#include <ros/this_node.h>
-#include <rosconsole/macros_generated.h>

 namespace sas
 {
--- a/src/robot_dynamics/qros_robot_dynamic_py.cpp
+++ b/src/robot_dynamics/qros_robot_dynamic_py.cpp
@@ -32,8 +32,8 @@
 #include <pybind11/stl.h>
 #include <pybind11/eigen.h>

-#include <robot_dynamic/qros_robot_dynamics_provider.h>
-#include <robot_dynamic/qros_robot_dynamics_interface.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h>

 namespace py = pybind11;
 using RDI = qros::RobotDynamicInterface;
--- a/src/robot_dynamics/qros_robot_dynamics_interface.cpp
+++ b/src/robot_dynamics/qros_robot_dynamics_interface.cpp
@@ -27,7 +27,7 @@
 #
 # ################################################################
 */
-#include <robot_dynamic/qros_robot_dynamics_interface.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h>

 using namespace qros;

--- a/src/robot_dynamics/qros_robot_dynamics_provider.cpp
+++ b/src/robot_dynamics/qros_robot_dynamics_provider.cpp
@@ -27,7 +27,7 @@
 #
 # ################################################################
 */
-#include <robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>

 using namespace qros;

--- a/src/sas_robot_driver_coppelia_node.cpp
+++ b/src/sas_robot_driver_coppelia_node.cpp
@@ -41,7 +41,7 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "coppelia/sas_robot_driver_coppelia.h"
+#include "../../src/coppelia/sas_robot_driver_coppelia.h"

 /*********************************************
 * SIGNAL HANDLER
--- a/src/sas_robot_driver_franka_hand_node.cpp
+++ b/src/sas_robot_driver_franka_hand_node.cpp
@@ -36,7 +36,7 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "hand/qros_effector_driver_franka_hand.h"
+#include "../../src/hand/qros_effector_driver_franka_hand.h"


 /*********************************************
--- a/src/sas_robot_driver_franka_node.cpp
+++ b/src/sas_robot_driver_franka_node.cpp
@@ -39,8 +39,8 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "sas_robot_driver_franka.h"
-#include <robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>


 /*********************************************