minor renaming of file and working coppelia node

CMakeLists.txt

@@ -197,6 +197,24 @@ install(TARGETS
         sas_robot_driver_franka_hand_node
         DESTINATION lib/${PROJECT_NAME})
 
+##### Coppelia Mirror Node #####
+
+add_executable(sas_robot_driver_franka_coppelia_node
+    src/sas_robot_driver_coppelia_node.cpp
+    src/coppelia/sas_robot_driver_coppelia.cpp
+)
+ament_target_dependencies(sas_robot_driver_franka_coppelia_node
+        rclcpp sas_common sas_core sas_conversions sas_robot_driver
+)
+
+target_link_libraries(sas_robot_driver_franka_coppelia_node
+        dqrobotics
+        dqrobotics-interface-json11
+        dqrobotics-interface-vrep
+)
+install(TARGETS
+        sas_robot_driver_franka_coppelia_node
+        DESTINATION lib/${PROJECT_NAME})
 
 ##### JuanEmika
 

include/sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp

@@ -0,0 +1,139 @@
+/*
+# 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)
+#         - Adapted from sas_robot_driver_denso.cpp
+#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp)
+#
+# ################################################################
+*/
+
+
+#pragma once
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+#include <dqrobotics/DQ.h>
+#include <sas_core/sas_clock.hpp>
+// #include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
+#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
+#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+// #include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
+#include <thread>
+#include <atomic>
+#include <sas_robot_driver/sas_robot_driver_server.hpp>
+#include <sas_robot_driver/sas_robot_driver_client.hpp>
+
+#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC 0.002  // 2ms, 500Hz
+#define REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT 500
+using namespace DQ_robotics;
+using namespace Eigen;
+
+namespace qros
+{
+
+
+
+struct RobotDriverCoppeliaConfiguration
+{
+
+    double thread_sampling_time_sec;  // frontend vrep update rate
+    int vrep_port;
+    std::string vrep_ip;
+    std::vector<std::string> vrep_joint_names;
+    bool vrep_dynamically_enabled = false;
+    std::string robot_mode;
+    bool using_real_robot;
+    std::string robot_topic_prefix;
+    std::string robot_parameter_file_path;
+    // VectorXd q_min;
+    // VectorXd q_max;
+};
+
+class RobotDriverCoppelia
+{
+private:
+    enum ControlMode{
+        Position=0,
+        Velocity
+    };
+
+    RobotDriverCoppeliaConfiguration configuration_;
+    std::shared_ptr<Node> node_sptr_;
+
+    sas::Clock clock_;
+    std::atomic_bool* break_loops_;
+    bool _should_shutdown() const {return (*break_loops_);}
+    ControlMode robot_mode_;
+
+    std::shared_ptr<DQ_VrepInterface> vi_;
+    std::shared_ptr<sas::RobotDriverClient> real_robot_interface_ = nullptr;
+    std::shared_ptr<sas::RobotDriverServer> robot_provider_ = nullptr;
+
+    // backend thread for simulaton
+    /**
+     * Current simulation mechanism is not accounting for any robot dynamics, just the joint limits
+     */
+    std::thread velocity_control_simulation_thread_;
+    VectorXd simulated_joint_positions_;
+    VectorXd simulated_joint_velocities_;
+    void start_simulation_thread();  // thread entry point
+    void _velocity_control_simulation_thread_main();
+    std::atomic_bool simulation_thread_started_{false};
+
+    bool initialized_ = false;
+    inline void _assert_initialized() const{
+        if (!initialized_){throw std::runtime_error("[RobotDriverCoppelia] Robot driver not initialized");}
+    }
+    inline void _assert_is_alive() const{
+        if(!configuration_.using_real_robot && !simulation_thread_started_){throw std::runtime_error("[RobotDriverCoppelia] Robot Simulation is not alive");}
+    }
+
+    void _start_control_loop();
+protected:
+    inline void _update_vrep_position(const VectorXd &joint_positions, const bool& force_update=false) const;
+    inline void _update_vrep_velocity(const VectorXd & joint_velocity) const;
+
+public:
+    RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
+    RobotDriverCoppelia()=delete;
+    ~RobotDriverCoppelia();
+
+    RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
+
+    int start_control_loop();  // public entry point
+
+    void connect();
+    void disconnect();
+
+    void initialize();
+    void deinitialize();
+
+    std::tuple<VectorXd, VectorXd> joint_limits_;
+
+};
+}

include/sas_robot_driver_franka/sas_robot_driver_franka.hpp

@@ -41,7 +41,7 @@
 #include <dqrobotics/DQ.h>
 
 #include <sas_core/sas_robot_driver.hpp>
-#include <sas_robot_driver_franka/interfaces/robot_interface_franka.h>
+#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
 

launch/sas_robot_driver_franka_example.py

@@ -13,8 +13,8 @@ def generate_launch_description():
                 "robot_ip_address": "172.16.0.4",
                 "thread_sampling_time_sec": 0.004,
                 # "thread_sampling_time_nsec": 4000000,
-                "q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895],
+                "q_min": [-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895],
-                "q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094,  2.6895],
+                "q_max": [2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895],
                 "force_upper_limits_scaling_factor": 4.0,
                 "upper_torque_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0],
                 "upper_force_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0],
@@ -23,6 +23,23 @@ def generate_launch_description():
             }],
             output="screen"
         ),
+        Node(
+            package='sas_robot_driver_franka',
+            executable='sas_robot_driver_franka_coppelia_node',
+            name='arm3_coppelia',
+            parameters=[{
+                "thread_sampling_time_sec": 0.008,
+                "vrep_ip": os.environ["VREP_IP"],
+                "vrep_port": 20012,
+                "vrep_dynamically_enabled": True,
+                "using_real_robot": True,
+                "vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
+                                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"],
+                "robot_topic_prefix": "/arm3",
+                "robot_mode": "VelocityControl",
+                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
+            }],
+            output="screen"
+        ),
 
     ])
-

launch/sas_robot_driver_franka_simulation_example.py

@@ -0,0 +1,27 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='sas_robot_driver_franka',
+            executable='sas_robot_driver_franka_coppelia_node',
+            name='arm3_coppelia',
+            parameters=[{
+                "thread_sampling_time_sec": 0.008,
+                "vrep_ip": os.environ["VREP_IP"],
+                "vrep_port": 20012,
+                "vrep_dynamically_enabled": True,
+                "using_real_robot": False,
+                "vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
+                                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"],
+                "robot_topic_prefix": "/arm3",
+                "robot_mode": "VelocityControl",
+                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
+            }],
+            output="screen"
+        ),
+
+    ])

qt/configuration_window/mainwindow.h

@@ -7,7 +7,7 @@
 
 #include <QMainWindow>
 #include "qspinbox.h"
-#include <sas_robot_driver_franka/interfaces/robot_interface_franka.h>
+#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
 
 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }

src/coppelia/sas_robot_driver_coppelia.cpp

@@ -1,253 +1,247 @@
-#include "sas_robot_driver_coppelia.h"
+#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
 
 
-namespace sas
+namespace qros
 {
 
+RobotDriverCoppelia::~RobotDriverCoppelia() {
+    deinitialize();
+    disconnect();
+}
 
-RobotDriverCoppelia::RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
+RobotDriverCoppelia::RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
-RobotDriver(break_loops),
         configuration_(configuration),
-    robot_mode_(configuration.robot_mode),
+        node_sptr_(node_sptr),
-    jointnames_(configuration.jointnames),
+        clock_(configuration.thread_sampling_time_sec),
-    mirror_mode_(configuration.mirror_mode),
+        break_loops_(break_loops),
-    dim_configuration_space_(configuration.jointnames.size()),
+        robot_mode_(ControlMode::Position),
-    real_robot_topic_prefix_(configuration.real_robot_topic_prefix)
+        vi_(std::make_shared<DQ_VrepInterface>())
 {
-    vi_ = std::make_shared<DQ_VrepInterface>();
+    // should initialize robot driver interface to real robot
-    desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_);
+    DQ_SerialManipulatorDH smdh = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(configuration_.robot_parameter_file_path);
-    auto nodehandle = ros::NodeHandle();
+    joint_limits_ = {smdh.get_lower_q_limit(),smdh.get_upper_q_limit()};
-    std::cout<<"RobotDriverCoppelia::Rostopic: "<<"/"+real_robot_topic_prefix_<<std::endl;
+    if(configuration_.using_real_robot)
-    franka1_ros_ = std::make_shared<sas::RobotDriverInterface>(nodehandle, "/"+real_robot_topic_prefix_);
-}
-
-VectorXd RobotDriverCoppelia::get_joint_positions()
-{
-    return current_joint_positions_;
-}
-
-void RobotDriverCoppelia::set_target_joint_positions(const VectorXd &desired_joint_positions_rad)
-{
-    desired_joint_positions_ = desired_joint_positions_rad;
-}
-
-VectorXd RobotDriverCoppelia::get_joint_velocities()
-{
-    return current_joint_velocities_;
-}
-
-void RobotDriverCoppelia::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
-{
-    desired_joint_velocities_ = desired_joint_velocities;
-}
-
-VectorXd RobotDriverCoppelia::get_joint_forces()
-{
-    return current_joint_forces_;
-}
-
-RobotDriverCoppelia::~RobotDriverCoppelia()=default;
-
-void RobotDriverCoppelia::connect()
-{
-
-    vi_->connect(configuration_.ip, configuration_.port, 500, 10);
-    vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
-    std::cout<<"RobotDriverCoppelia::Connecting..."<<std::endl;
-}
-
-void RobotDriverCoppelia::disconnect()
-{
-    vi_->disconnect();
-    if (joint_velocity_control_mode_thread_.joinable())
     {
-        joint_velocity_control_mode_thread_.join();
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using real robot, Instantiating robot interface to real driver at ["+configuration_.robot_topic_prefix+"].");
-    }
+        real_robot_interface_ = std::make_shared<sas::RobotDriverClient>(node_sptr_, configuration_.robot_topic_prefix);
-    if (joint_velocity_control_mirror_mode_thread_.joinable())
-    {
-        joint_velocity_control_mirror_mode_thread_.join();
-    }
-    std::cout<<"RobotDriverCoppelia::Disconnected."<<std::endl;
-}
-
-void RobotDriverCoppelia::initialize()
-{
-    vi_->start_simulation();
-    if (mirror_mode_ == false)
-    {
-        _start_joint_velocity_control_thread();
     }else{
-        _start_joint_velocity_control_mirror_thread();
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode);
+        robot_provider_ = std::make_shared<sas::RobotDriverServer>(node_sptr_, configuration_.robot_topic_prefix);
+        std::string _mode_upper = configuration_.robot_mode;
+        std::transform(_mode_upper.begin(), _mode_upper.end(), _mode_upper.begin(), ::toupper);
+        if(_mode_upper == "POSITIONCONTROL"){
+            robot_mode_ = ControlMode::Position;
+        }else if(_mode_upper == "VELOCITYCONTROL"){
+            robot_mode_ = ControlMode::Velocity;
+        }else{
+            throw std::invalid_argument("[" + std::string(node_sptr_->get_name()) + "]::Robot mode must be either 'position' or 'velocity'");
         }
-    std::cout<<"RobotDriverCoppelia::Velocity loop running..."<<std::endl;
+    }
+
 }
 
-void RobotDriverCoppelia::deinitialize()
+void RobotDriverCoppelia::_update_vrep_position(const VectorXd &joint_positions, const bool& force_update) const{
-{
+    if(configuration_.vrep_dynamically_enabled){
-    vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
+        if(force_update){
-    vi_->stop_simulation();
+            vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
-    finish_motion();
+        }
-    std::cout<<"RobotDriverCoppelia::Deinitialized."<<std::endl;
+        vi_->set_joint_target_positions(configuration_.vrep_joint_names, joint_positions);
+    }else{
+        vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
+    }
 }
 
-
+void RobotDriverCoppelia::_update_vrep_velocity(const VectorXd & joint_velocity) const{
-
+    if(!configuration_.vrep_dynamically_enabled){
-
+        throw std::runtime_error("[RobotDriverCoppelia]::[_update_vrep_velocity]::Vrep is not dynamically enabled");
-void RobotDriverCoppelia::_update_robot_state(const VectorXd &q, const VectorXd &q_dot, const VectorXd &forces)
+    }
-{
+    vi_->set_joint_target_velocities(configuration_.vrep_joint_names, joint_velocity);
-    current_joint_positions_  = q;
-    current_joint_velocities_ = q_dot;
-    current_joint_forces_     =  forces;
 }
 
-void RobotDriverCoppelia::finish_motion()
+void RobotDriverCoppelia::_start_control_loop(){
-{
+    clock_.init();
-    for (int i=0;i<1000;i++)
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Starting control loop...");
+    while(!_should_shutdown()){
+        clock_.update_and_sleep();
+        rclcpp::spin_some(node_sptr_);
+        if(!rclcpp::ok()){break_loops_->store(true);}
+        if(configuration_.using_real_robot){
+            // real_robot_interface_
+            auto joint_position = real_robot_interface_->get_joint_positions();
+            _update_vrep_position(joint_position);
+        }else{
+            // robot_provider_
+            VectorXd target_joint_positions;
+            auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
+            VectorXd current_joint_velocity;
+            if(robot_provider_->is_enabled()) {
+                if(robot_mode_ == ControlMode::Velocity)
                 {
-        set_target_joint_positions(current_joint_positions_);
+                    if(robot_provider_->is_enabled(sas::RobotDriver::Functionality::VelocityControl)) {
-        set_target_joint_velocities(VectorXd::Zero(dim_configuration_space_));
+                        simulated_joint_velocities_ = robot_provider_->get_target_joint_velocities();
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+                        current_joint_velocity = simulated_joint_velocities_;
+                        //                    try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){}
                     }
-    finish_motion_ = true;
+                    else{
+                        RCLCPP_DEBUG_STREAM(node_sptr_->get_logger(), "::Velocity control is not enabled.");
+                    }
+                    target_joint_positions = simulated_joint_positions_;
+                }else{
+                    target_joint_positions = robot_provider_->get_target_joint_positions();
+                }
+            }else {
+                target_joint_positions = current_joint_positions;
+            }
+            _update_vrep_position(target_joint_positions);
+            robot_provider_->send_joint_states(current_joint_positions, current_joint_velocity, VectorXd());
+            robot_provider_->send_joint_limits(joint_limits_);
+
+        }
+    }
+
+
 }
 
-void RobotDriverCoppelia::_start_joint_velocity_control_mode()
+int RobotDriverCoppelia::start_control_loop(){
-{
     try{
-        finish_motion_ = false;
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Waiting to connect with coppelia...");
-        VectorXd q = vi_->get_joint_positions(jointnames_);
+        connect();
-        VectorXd q_dot =  vi_->get_joint_velocities(jointnames_);
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Connected to coppelia.");
-        VectorXd forces = vi_->get_joint_torques(jointnames_);
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Initializing...");
-        _update_robot_state(q, q_dot, forces);
+        initialize();
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::initialized.");
 
-        desired_joint_positions_ = q;
+        _start_control_loop();
-        while(true)
-        {
-
-            VectorXd q = vi_->get_joint_positions(jointnames_);
-            VectorXd q_dot =  vi_->get_joint_velocities(jointnames_);
-            VectorXd forces = vi_->get_joint_torques(jointnames_);
-            _update_robot_state(q, q_dot, forces);
-
-
-            if (robot_mode_  == std::string("VelocityControl"))
-            {              
-                    vi_->set_joint_target_velocities(jointnames_, desired_joint_velocities_); 
-            }
-            else if (robot_mode_  == std::string("PositionControl"))
-            {
-                    vi_->set_joint_target_positions(jointnames_, desired_joint_positions_);
-            }
-            if (finish_motion_) {
-                    finish_motion_ = false;
-                    return;
-            }
-        }
     }
     catch(const std::exception& e)
     {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<<std::endl;
+        RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Error or exception caught::" << e.what());
     }
     catch(...)
     {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
+        RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Unexpected error or exception caught");
     }
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Deinitializing...");
+    deinitialize();
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::deinitialized.");
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnecting from coppelia...");
+    disconnect();
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnected from coppelia.");
 
-
+    return 0;
 }
 
-void RobotDriverCoppelia::_start_joint_velocity_control_thread()
+
-{
+void RobotDriverCoppelia::connect(){
-    finish_motion_ = false;
+    auto ret = vi_->connect(configuration_.vrep_ip, configuration_.vrep_port, 100, 10);
-    if (joint_velocity_control_mode_thread_.joinable())
+    if(!ret){
-    {
+        throw std::runtime_error("[RobotDriverCoppelia]::connect::Could not connect to Vrep");
-        joint_velocity_control_mode_thread_.join();
     }
-    if (joint_velocity_control_mirror_mode_thread_.joinable())
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::connect::Connected to Vrep");
-    {
+}
-        joint_velocity_control_mirror_mode_thread_.join();
+void RobotDriverCoppelia::disconnect(){
-    }
+    vi_->disconnect_all();
-    joint_velocity_control_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mode, this);
 }
 
-void RobotDriverCoppelia::_start_joint_velocity_control_mirror_thread()
+void RobotDriverCoppelia::initialize(){
-{
+    if(configuration_.using_real_robot)
-    finish_motion_ = false;
-    if (joint_velocity_control_mode_thread_.joinable())
     {
-        joint_velocity_control_mode_thread_.join();
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
+        rclcpp::spin_some(node_sptr_);
+        int count = 0;
+        while (!real_robot_interface_->is_enabled(sas::RobotDriver::Functionality::PositionControl)) {
+            rclcpp::spin_some(node_sptr_);
+            // std::cout<<"Waiting for real robot interface to initialize..."<<std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            rclcpp::spin_some(node_sptr_);
+            count++;
+            if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){
+                RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
+                throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
             }
-    if (joint_velocity_control_mirror_mode_thread_.joinable())
+            if(!rclcpp::ok()) {
+                RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::ROS shutdown received. Exiting...");
+                throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::ROS shutdown received, not OK.");
+            }
+        }
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Real robot interface initialized.");
+        joint_limits_ = real_robot_interface_->get_joint_limits();
+        _update_vrep_position(real_robot_interface_->get_joint_positions(), true);
+    }else{
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Simulation mode.");
+        // initialization information for robot driver provider
+        /**
+        * TODO: check for making sure real robot is not actually connected
+        */
+        auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
+        VectorXd current_joint_velocity;
+        if(robot_mode_ == ControlMode::Velocity)
+        {current_joint_velocity = VectorXd::Zero(current_joint_positions.size());}
+        robot_provider_->send_joint_states(current_joint_positions, current_joint_velocity, VectorXd());
+        robot_provider_->send_joint_limits(joint_limits_);
+        // start velocity control simulation thread if needed
+        if(robot_mode_ == ControlMode::Velocity)
         {
-        joint_velocity_control_mirror_mode_thread_.join();
+            simulated_joint_positions_ = current_joint_positions;
+            simulated_joint_velocities_ = current_joint_velocity;
+            start_simulation_thread();
         }
-    joint_velocity_control_mirror_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode, this);
+    }
+}
+void RobotDriverCoppelia::deinitialize(){
+    // nothing to do
 }
 
-void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode()
+void RobotDriverCoppelia::start_simulation_thread(){ // thread entry point
-{
+    if(simulation_thread_started_){
+        throw std::runtime_error("[RobotDriverCoppelia]::start_simulation_thread::Simulation thread already started");
+    }
+    if(velocity_control_simulation_thread_.joinable()){
+        velocity_control_simulation_thread_.join();
+    }
 
+    velocity_control_simulation_thread_ = std::thread(&RobotDriverCoppelia::_velocity_control_simulation_thread_main, this);
+}
+
+void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){
+    /**
+     * This thread should not access vrep
+     */
+    simulation_thread_started_ = true;
     try{
-        finish_motion_ = false;
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
-        std::cout<<"RobotDriverCoppelia::Waiting for real robot topics..."<<std::endl;
+        sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC);
-        VectorXd q;
+        double tau = VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC;
-        while (ros::ok()) {
+        auto current_joint_positions = simulated_joint_positions_;
-            if (franka1_ros_->is_enabled())
+        clock.init();
-            {
+        while (!(*break_loops_) && rclcpp::ok()) {
-                    q = franka1_ros_->get_joint_positions();
-                    break;
-            }
-            ros::spinOnce();
-        }
-        std::cout<<"RobotDriverCoppelia::Done!"<<std::endl;
 
-        VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
+            current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model
-        VectorXd q_dot_vrep =  vi_->get_joint_velocities(jointnames_);
+            // cap joint limit
-        VectorXd forces_vrep = vi_->get_joint_torques(jointnames_);
+            auto q_min = std::get<0>(joint_limits_);
-        _update_robot_state(q_vrep, q_dot_vrep, forces_vrep);
+            auto q_max = std::get<1>(joint_limits_);
-
+            for (int i = 0; i < current_joint_positions.size(); i++) {
-        desired_joint_positions_ = q_vrep;
+                if (current_joint_positions(i) < q_min(i)) {
-
+                    current_joint_positions(i) = q_min(i);
-
-        while(ros::ok())
-        {
-            q = franka1_ros_->get_joint_positions();
-            if (q.size() == dim_configuration_space_)
-            {
-                VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
-                VectorXd q_dot_vrep =  vi_->get_joint_velocities(jointnames_);
-                VectorXd forces_vrep = vi_->get_joint_torques(jointnames_);
-                _update_robot_state(q_vrep, q_dot_vrep, forces_vrep);
-
-
-                if (robot_mode_  == std::string("VelocityControl"))
-                {
-                    vi_->set_joint_target_velocities(jointnames_, gain_*(q-q_vrep));
                 }
-                else if (robot_mode_  == std::string("PositionControl"))
+                if (current_joint_positions(i) > q_max(i)) {
-                {
+                    current_joint_positions(i) = q_max(i);
-                        vi_->set_joint_target_positions(jointnames_, q);
-                }
-                if (finish_motion_) {
-                        finish_motion_ = false;
-                        return;
                 }
             }
+            simulated_joint_positions_ = current_joint_positions;
+            clock.update_and_sleep();
         }
+    }catch(std::exception &e){
+        RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" << e.what());
+        simulation_thread_started_ = false;
+    }catch(...){
+        RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
+        simulation_thread_started_ = false;
     }
-    catch(const std::exception& e)
+    break_loops_->store(true);
-    {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<<std::endl;
-    }
-    catch(...)
-    {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl;
-    }
-
 
 }
 
 
 
-
 } // sas namespace

src/coppelia/sas_robot_driver_coppelia.h

@@ -1,134 +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)
-#         - Adapted from sas_robot_driver_denso.cpp
-#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp)
-#
-# ################################################################
-*/
-
-
-#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 <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
-#include <thread>
-#include <sas_robot_driver/sas_robot_driver_interface.h>
-
-using namespace DQ_robotics;
-using namespace Eigen;
-
-namespace sas
-{
-
-
-
-struct RobotDriverCoppeliaConfiguration
-{
-
-    int thread_sampling_time_nsec;
-    int port;
-    std::string ip;
-    std::vector<std::string> jointnames;
-    std::string robot_mode;
-    bool mirror_mode;
-    std::string real_robot_topic_prefix;
-};
-
-class RobotDriverCoppelia: public RobotDriver
-{
-private:
-    RobotDriverCoppeliaConfiguration configuration_;
-
-    std::string robot_mode_ = std::string("VelocityControl");   // PositionControl
-    bool mirror_mode_ = false;
-    double gain_ = 3.0;
-    std::string real_robot_topic_prefix_;
-
-    VectorXd current_joint_positions_;
-    VectorXd current_joint_velocities_;
-    VectorXd current_joint_forces_;
-
-
-    VectorXd desired_joint_velocities_;
-    VectorXd desired_joint_positions_;
-
-    std::atomic<bool> finish_motion_;
-
-    int dim_configuration_space_;
-
-    void _update_robot_state(const VectorXd& q, const VectorXd& q_dot, const VectorXd& forces);
-
-    void finish_motion();
-
-    void _start_joint_velocity_control_mode();
-    std::thread joint_velocity_control_mode_thread_;
-    void _start_joint_velocity_control_thread();
-
-
-    void _start_joint_velocity_control_mirror_mode();
-    std::thread joint_velocity_control_mirror_mode_thread_;
-    void _start_joint_velocity_control_mirror_thread();
-
-    std::shared_ptr<sas::RobotDriverInterface> franka1_ros_;
-
-
-protected:
-    std::shared_ptr<DQ_VrepInterface> vi_;
-    std::vector<std::string> jointnames_;
-
-
-
-public:
-    RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
-    RobotDriverCoppelia()=delete;
-    ~RobotDriverCoppelia();
-
-    RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration& 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;
-
-
-};
-}

src/hand/qros_effector_driver_franka_hand.cpp

@@ -27,7 +27,7 @@
 #
 # ################################################################
 */
-#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.h>
+#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
 
 #include <franka/exception.h>
 

src/hand/robot_interface_hand.cpp

@@ -1,4 +1,33 @@
-#include "sas_robot_driver_franka/robot_interface_hand.h"
+/*
+# 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. Quenitin Lin
+#
+# ################################################################
+*/
+#include "sas_robot_driver_franka/robot_interface_hand.hpp"
 
 
 

src/joint/robot_interface_franka.cpp

@@ -30,7 +30,7 @@
 */
 
 
-#include <sas_robot_driver_franka/interfaces/robot_interface_franka.h>
+#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
 
 
 /**

src/joint/sas_robot_driver_franka.cpp

@@ -31,7 +31,7 @@
 */
 
 
-#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
+#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
 #include <sas_core/sas_clock.hpp>
 #include <dqrobotics/utils/DQ_Math.h>
 
@@ -42,8 +42,8 @@ namespace sas
         const std::shared_ptr<Node> &node,
         const std::shared_ptr<qros::RobotDynamicsServer> &robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops
     ):
-    node_(node),
     RobotDriver(break_loops),
+    node_(node),
     configuration_(configuration),
     robot_dynamic_provider_sptr_(robot_dynamic_provider),
     break_loops_(break_loops)
@@ -55,11 +55,10 @@ namespace sas
         //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;
+        // std::cout<<configuration.ip_address<<std::endl;
 
         RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
-
+        RCLCPP_INFO_STREAM(node_->get_logger(), "Mode: " << configuration_.mode);
-        std::cout<<configuration.mode<<std::endl;
 
         if (configuration_.mode == std::string("None"))
         {

src/sas_robot_driver_coppelia_node.cpp

@@ -26,6 +26,9 @@
 #      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
 #         - Adapted from sas_robot_driver_denso_node.cpp
 #           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
+#      2. Quentin Lin (qlin1806@g.ecc.u-tokyo.ac.jp)
+#         - Adapted for simplify operation
+#         - porting to ROS2
 #
 # ################################################################
 */
@@ -36,10 +39,9 @@
 #include <exception>
 #include <dqrobotics/utils/DQ_Math.h>
 //#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
-#include <sas_common/sas_common.h>
+#include <sas_common/sas_common.hpp>
-#include <sas_conversions/eigen3_std_conversions.h>
+#include <sas_conversions/eigen3_std_conversions.hpp>
-#include <sas_robot_driver/sas_robot_driver_ros.h>
+#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
-#include "coppelia/sas_robot_driver_coppelia.h"
 
 /*********************************************
  * SIGNAL HANDLER
@@ -52,52 +54,51 @@ void sig_int_handler(int)
 }
 
 
+
 int main(int argc, char **argv)
 {
     if(signal(SIGINT, sig_int_handler) == SIG_ERR)
     {
-        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
+        throw std::runtime_error("Error setting the signal int handler.");
     }
 
-    ros::init(argc, argv, "sas_robot_driver_coppelia_node", ros::init_options::NoSigintHandler);
+    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
-    ROS_WARN("=====================================================================");
+    auto context = rclcpp::contexts::get_global_default_context();
-    ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
+
-    ROS_WARN("=====================================================================");
+    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_coppelia_node");
-    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
+
+    const auto node_name = std::string(node->get_name());
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_WARN(node->get_logger(),"-----------------Adapted by Quentin Lin ------------------------");
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_INFO_STREAM_ONCE(node->get_logger(),":Loading parameters from parameter server.");
 
 
-    ros::NodeHandle nh;
+    qros::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
-    sas::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
+    sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_coppelia_configuration.thread_sampling_time_sec);
-
+    sas::get_ros_parameter(node,"vrep_ip",robot_driver_coppelia_configuration.vrep_ip);
-    sas::get_ros_param(nh,"/robot_ip_address",robot_driver_coppelia_configuration.ip);
+    sas::get_ros_parameter(node,"vrep_port",robot_driver_coppelia_configuration.vrep_port);
-    sas::get_ros_param(nh,"/robot_mode",      robot_driver_coppelia_configuration.robot_mode);
+    sas::get_ros_parameter(node,"vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names);
-    sas::get_ros_param(nh,"/vrep_port", robot_driver_coppelia_configuration.port);
+    sas::get_ros_parameter(node,"vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled);
-    sas::get_ros_param(nh,"/vrep_robot_joint_names", robot_driver_coppelia_configuration.jointnames);
+    sas::get_ros_parameter(node,"robot_mode", robot_driver_coppelia_configuration.robot_mode);
-    sas::get_ros_param(nh,"/mirror_mode", robot_driver_coppelia_configuration.mirror_mode);
+    sas::get_ros_parameter(node,"using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
-    sas::get_ros_param(nh, "/real_robot_topic_prefix", robot_driver_coppelia_configuration.real_robot_topic_prefix);
+    sas::get_ros_parameter(node,"robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
-    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
+    sas::get_ros_parameter(node,"robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
-
-    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
-    sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
-    sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
-
-    robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
 
     try
     {
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot.");
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"::Instantiating Coppelia robot mirror.");
-        sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration,
+        qros::RobotDriverCoppelia robot_driver_coppelia(node, robot_driver_coppelia_configuration,
                                                        &kill_this_process);
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
+
-        sas::RobotDriverROS robot_driver_ros(nh,
+        return robot_driver_coppelia.start_control_loop();
-                                             &robot_driver_coppelia,
-                                             robot_driver_ros_configuration,
-                                             &kill_this_process);
-        robot_driver_ros.control_loop();
     }
     catch (const std::exception& e)
     {
-        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
+        kill_this_process = true;
+        RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
+        shutdown(context, "Exception in main function: " + std::string(e.what()));
+        return -1;
     }
 
 

src/sas_robot_driver_franka_hand_node.cpp

@@ -35,7 +35,7 @@
 #include <dqrobotics/utils/DQ_Math.h>
 #include <sas_common/sas_common.hpp>
 // #include <sas_conversions/eigen3_std_conversions.hpp>
-#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.h>
+#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
 
 
 /*********************************************

src/sas_robot_driver_franka_node.cpp

@@ -41,7 +41,7 @@
 #include <sas_common/sas_common.hpp>
 #include <sas_conversions/eigen3_std_conversions.hpp>
 #include <sas_robot_driver/sas_robot_driver_ros.hpp>
-#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
+#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
 #include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
 
 
@@ -76,6 +76,15 @@ std::array<T, N> std_vec_to_std_array(const std::vector<T>& vector)
     }
     return array;
 }
+VectorXd std_vec_to_eigen_vector(const std::vector<double>& vector)
+{
+    VectorXd eigen_vector(vector.size());
+    for(std::size_t i = 0; i < vector.size(); i++)
+    {
+        eigen_vector(i) = vector[i];
+    }
+    return eigen_vector;
+}
 
 
 int main(int argc, char **argv)
@@ -164,7 +173,8 @@ int main(int argc, char **argv)
                 robot_driver_franka_configuration,
                 &kill_this_process
             );
-            //robot_driver_franka.set_joint_limits({qmin, qmax});
+            std::tuple<VectorXd, VectorXd> joint_limits = {std_vec_to_eigen_vector(robot_driver_ros_configuration.q_min), std_vec_to_eigen_vector(robot_driver_ros_configuration.q_max)};
+            robot_driver_franka -> set_joint_limits(joint_limits);
             RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating RobotDriverROS.");
             sas::RobotDriverROS robot_driver_ros(node,
                                                 robot_driver_franka,