diff --git a/CMakeLists.txt b/CMakeLists.txt index c082786..40d521b 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp b/include/sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp new file mode 100644 index 0000000..337f2a0 --- /dev/null +++ b/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 . +# +# ################################################################ +# +# 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 +#include +#include +#include +#include +#include +#include +// #include +#include +#include +// #include +#include +#include +#include +#include + +#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 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_sptr_; + + sas::Clock clock_; + std::atomic_bool* break_loops_; + bool _should_shutdown() const {return (*break_loops_);} + ControlMode robot_mode_; + + std::shared_ptr vi_; + std::shared_ptr real_robot_interface_ = nullptr; + std::shared_ptr 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_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 joint_limits_; + +}; +} diff --git a/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.h b/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp similarity index 100% rename from include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.h rename to include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp diff --git a/include/sas_robot_driver_franka/interfaces/robot_interface_franka.h b/include/sas_robot_driver_franka/interfaces/robot_interface_franka.hpp similarity index 100% rename from include/sas_robot_driver_franka/interfaces/robot_interface_franka.h rename to include/sas_robot_driver_franka/interfaces/robot_interface_franka.hpp diff --git a/include/sas_robot_driver_franka/robot_interface_hand.h b/include/sas_robot_driver_franka/robot_interface_hand.hpp similarity index 100% rename from include/sas_robot_driver_franka/robot_interface_hand.h rename to include/sas_robot_driver_franka/robot_interface_hand.hpp diff --git a/include/sas_robot_driver_franka/sas_robot_driver_franka.h b/include/sas_robot_driver_franka/sas_robot_driver_franka.hpp similarity index 99% rename from include/sas_robot_driver_franka/sas_robot_driver_franka.h rename to include/sas_robot_driver_franka/sas_robot_driver_franka.hpp index 0f6aeaf..b6fda2c 100644 --- a/include/sas_robot_driver_franka/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka/sas_robot_driver_franka.hpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include diff --git a/launch/sas_robot_driver_franka_example.py b/launch/sas_robot_driver_franka_example.py index 8820acd..003a843 100644 --- a/launch/sas_robot_driver_franka_example.py +++ b/launch/sas_robot_driver_franka_example.py @@ -13,16 +13,33 @@ 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_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 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], "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], "robot_mode": "VelocityControl", "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"] }], - output="screen" + 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" ), ]) - diff --git a/launch/sas_robot_driver_franka_simulation_example.py b/launch/sas_robot_driver_franka_simulation_example.py new file mode 100644 index 0000000..a68756b --- /dev/null +++ b/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" + ), + + ]) diff --git a/qt/configuration_window/mainwindow.h b/qt/configuration_window/mainwindow.h index 7b801be..5293af5 100644 --- a/qt/configuration_window/mainwindow.h +++ b/qt/configuration_window/mainwindow.h @@ -7,7 +7,7 @@ #include #include "qspinbox.h" -#include +#include QT_BEGIN_NAMESPACE namespace Ui { class MainWindow; } diff --git a/src/coppelia/sas_robot_driver_coppelia.cpp b/src/coppelia/sas_robot_driver_coppelia.cpp index 552442c..5789291 100644 --- a/src/coppelia/sas_robot_driver_coppelia.cpp +++ b/src/coppelia/sas_robot_driver_coppelia.cpp @@ -1,252 +1,246 @@ -#include "sas_robot_driver_coppelia.h" +#include -namespace sas +namespace qros { - -RobotDriverCoppelia::RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops): -RobotDriver(break_loops), - configuration_(configuration), - robot_mode_(configuration.robot_mode), - jointnames_(configuration.jointnames), - mirror_mode_(configuration.mirror_mode), - dim_configuration_space_(configuration.jointnames.size()), - real_robot_topic_prefix_(configuration.real_robot_topic_prefix) -{ - vi_ = std::make_shared(); - desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_); - auto nodehandle = ros::NodeHandle(); - std::cout<<"RobotDriverCoppelia::Rostopic: "<<"/"+real_robot_topic_prefix_<(nodehandle, "/"+real_robot_topic_prefix_); +RobotDriverCoppelia::~RobotDriverCoppelia() { + deinitialize(); + disconnect(); } -VectorXd RobotDriverCoppelia::get_joint_positions() +RobotDriverCoppelia::RobotDriverCoppelia(const std::shared_ptr &node_sptr, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops): + configuration_(configuration), + node_sptr_(node_sptr), + clock_(configuration.thread_sampling_time_sec), + break_loops_(break_loops), + robot_mode_(ControlMode::Position), + vi_(std::make_shared()) { - 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..."<disconnect(); - if (joint_velocity_control_mode_thread_.joinable()) + // should initialize robot driver interface to real robot + DQ_SerialManipulatorDH smdh = DQ_JsonReader::get_from_json(configuration_.robot_parameter_file_path); + joint_limits_ = {smdh.get_lower_q_limit(),smdh.get_upper_q_limit()}; + if(configuration_.using_real_robot) { - joint_velocity_control_mode_thread_.join(); - } - if (joint_velocity_control_mirror_mode_thread_.joinable()) - { - joint_velocity_control_mirror_mode_thread_.join(); - } - std::cout<<"RobotDriverCoppelia::Disconnected."<start_simulation(); - if (mirror_mode_ == false) - { - _start_joint_velocity_control_thread(); + 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(node_sptr_, configuration_.robot_topic_prefix); }else{ - _start_joint_velocity_control_mirror_thread(); - } - std::cout<<"RobotDriverCoppelia::Velocity loop running..."<set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_)); - vi_->stop_simulation(); - finish_motion(); - std::cout<<"RobotDriverCoppelia::Deinitialized."<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); - - desired_joint_positions_ = q; - 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; - } + RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode); + robot_provider_ = std::make_shared(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'"); } } + +} + +void RobotDriverCoppelia::_update_vrep_position(const VectorXd &joint_positions, const bool& force_update) const{ + if(configuration_.vrep_dynamically_enabled){ + if(force_update){ + vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions); + } + 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"); + } + vi_->set_joint_target_velocities(configuration_.vrep_joint_names, joint_velocity); +} + +void RobotDriverCoppelia::_start_control_loop(){ + clock_.init(); + 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) + { + if(robot_provider_->is_enabled(sas::RobotDriver::Functionality::VelocityControl)) { + simulated_joint_velocities_ = robot_provider_->get_target_joint_velocities(); + current_joint_velocity = simulated_joint_velocities_; + // try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){} + } + 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_); + + } + } + + +} + +int RobotDriverCoppelia::start_control_loop(){ + try{ + RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Waiting to connect with coppelia..."); + connect(); + RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Connected to coppelia."); + RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Initializing..."); + initialize(); + RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::initialized."); + + _start_control_loop(); + } catch(const std::exception& e) { - std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<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 " <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() -{ - finish_motion_ = false; - if (joint_velocity_control_mode_thread_.joinable()) - { - joint_velocity_control_mode_thread_.join(); + +void RobotDriverCoppelia::connect(){ + auto ret = vi_->connect(configuration_.vrep_ip, configuration_.vrep_port, 100, 10); + if(!ret){ + throw std::runtime_error("[RobotDriverCoppelia]::connect::Could not connect to Vrep"); } - if (joint_velocity_control_mirror_mode_thread_.joinable()) - { - joint_velocity_control_mirror_mode_thread_.join(); - } - joint_velocity_control_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mode, this); + RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::connect::Connected to Vrep"); +} +void RobotDriverCoppelia::disconnect(){ + vi_->disconnect_all(); } -void RobotDriverCoppelia::_start_joint_velocity_control_mirror_thread() -{ - finish_motion_ = false; - if (joint_velocity_control_mode_thread_.joinable()) +void RobotDriverCoppelia::initialize(){ + if(configuration_.using_real_robot) { - joint_velocity_control_mode_thread_.join(); - } - if (joint_velocity_control_mirror_mode_thread_.joinable()) - { - joint_velocity_control_mirror_mode_thread_.join(); - } - joint_velocity_control_mirror_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode, this); -} - -void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode() -{ - - try{ - finish_motion_ = false; - std::cout<<"RobotDriverCoppelia::Waiting for real robot topics..."<is_enabled()) - { - q = franka1_ros_->get_joint_positions(); - break; + 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..."< 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(!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."); } - ros::spinOnce(); } - std::cout<<"RobotDriverCoppelia::Done!"<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); - - desired_joint_positions_ = q_vrep; - - - while(ros::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) { - 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")) - { - vi_->set_joint_target_positions(jointnames_, q); - } - if (finish_motion_) { - finish_motion_ = false; - return; - } - } + simulated_joint_positions_ = current_joint_positions; + simulated_joint_velocities_ = current_joint_velocity; + start_simulation_thread(); } } - catch(const std::exception& e) - { - std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<get_logger(), "::RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started."); + sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC); + double tau = VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC; + auto current_joint_positions = simulated_joint_positions_; + clock.init(); + while (!(*break_loops_) && rclcpp::ok()) { + + current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model + // cap joint limit + auto q_min = std::get<0>(joint_limits_); + auto q_max = std::get<1>(joint_limits_); + for (int i = 0; i < current_joint_positions.size(); i++) { + if (current_joint_positions(i) < q_min(i)) { + current_joint_positions(i) = q_min(i); + } + if (current_joint_positions(i) > q_max(i)) { + current_joint_positions(i) = q_max(i); + } + } + 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; + } + break_loops_->store(true); + +} diff --git a/src/coppelia/sas_robot_driver_coppelia.h b/src/coppelia/sas_robot_driver_coppelia.h deleted file mode 100644 index cabdd9f..0000000 --- a/src/coppelia/sas_robot_driver_coppelia.h +++ /dev/null @@ -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 . -# -# ################################################################ -# -# 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace DQ_robotics; -using namespace Eigen; - -namespace sas -{ - - - -struct RobotDriverCoppeliaConfiguration -{ - - int thread_sampling_time_nsec; - int port; - std::string ip; - std::vector 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 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 franka1_ros_; - - -protected: - std::shared_ptr vi_; - std::vector 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; - - -}; -} diff --git a/src/hand/qros_effector_driver_franka_hand.cpp b/src/hand/qros_effector_driver_franka_hand.cpp index 0a49057..b04e624 100644 --- a/src/hand/qros_effector_driver_franka_hand.cpp +++ b/src/hand/qros_effector_driver_franka_hand.cpp @@ -27,7 +27,7 @@ # # ################################################################ */ -#include +#include #include diff --git a/src/hand/robot_interface_hand.cpp b/src/hand/robot_interface_hand.cpp index fd5bc1a..c1a8d3f 100644 --- a/src/hand/robot_interface_hand.cpp +++ b/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 . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ +#include "sas_robot_driver_franka/robot_interface_hand.hpp" diff --git a/src/joint/robot_interface_franka.cpp b/src/joint/robot_interface_franka.cpp index 89e0a99..9b7071f 100644 --- a/src/joint/robot_interface_franka.cpp +++ b/src/joint/robot_interface_franka.cpp @@ -30,7 +30,7 @@ */ -#include +#include /** diff --git a/src/joint/sas_robot_driver_franka.cpp b/src/joint/sas_robot_driver_franka.cpp index d96cbad..426433d 100644 --- a/src/joint/sas_robot_driver_franka.cpp +++ b/src/joint/sas_robot_driver_franka.cpp @@ -31,7 +31,7 @@ */ -#include +#include #include #include @@ -42,8 +42,8 @@ namespace sas const std::shared_ptr &node, const std::shared_ptr &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<get_logger(), "Mode: " << configuration_.mode); if (configuration_.mode == std::string("None")) { diff --git a/src/sas_robot_driver_coppelia_node.cpp b/src/sas_robot_driver_coppelia_node.cpp index 04c4654..22b42d0 100644 --- a/src/sas_robot_driver_coppelia_node.cpp +++ b/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 #include //#include -#include -#include -#include -#include "coppelia/sas_robot_driver_coppelia.h" +#include +#include +#include /********************************************* * 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); - ROS_WARN("====================================================================="); - ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------"); - ROS_WARN("====================================================================="); - ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server."); + rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None); + auto context = rclcpp::contexts::get_global_default_context(); + + auto node = std::make_shared("sas_robot_driver_franka_coppelia_node"); + + 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; - sas::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration; - - sas::get_ros_param(nh,"/robot_ip_address",robot_driver_coppelia_configuration.ip); - sas::get_ros_param(nh,"/robot_mode", robot_driver_coppelia_configuration.robot_mode); - sas::get_ros_param(nh,"/vrep_port", robot_driver_coppelia_configuration.port); - sas::get_ros_param(nh,"/vrep_robot_joint_names", robot_driver_coppelia_configuration.jointnames); - sas::get_ros_param(nh,"/mirror_mode", robot_driver_coppelia_configuration.mirror_mode); - sas::get_ros_param(nh, "/real_robot_topic_prefix", robot_driver_coppelia_configuration.real_robot_topic_prefix); - sas::RobotDriverROSConfiguration robot_driver_ros_configuration; - - 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(); + qros::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_parameter(node,"vrep_port",robot_driver_coppelia_configuration.vrep_port); + sas::get_ros_parameter(node,"vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names); + sas::get_ros_parameter(node,"vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled); + sas::get_ros_parameter(node,"robot_mode", robot_driver_coppelia_configuration.robot_mode); + sas::get_ros_parameter(node,"using_real_robot", robot_driver_coppelia_configuration.using_real_robot); + sas::get_ros_parameter(node,"robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix); + sas::get_ros_parameter(node,"robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path); try { - ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot."); - sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration, + RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"::Instantiating Coppelia robot mirror."); + 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, - &robot_driver_coppelia, - robot_driver_ros_configuration, - &kill_this_process); - robot_driver_ros.control_loop(); + + return robot_driver_coppelia.start_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; } diff --git a/src/sas_robot_driver_franka_hand_node.cpp b/src/sas_robot_driver_franka_hand_node.cpp index b924621..12ede61 100644 --- a/src/sas_robot_driver_franka_hand_node.cpp +++ b/src/sas_robot_driver_franka_hand_node.cpp @@ -35,7 +35,7 @@ #include #include // #include -#include +#include /********************************************* diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index ff5fe45..eb13751 100644 --- a/src/sas_robot_driver_franka_node.cpp +++ b/src/sas_robot_driver_franka_node.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include @@ -76,6 +76,15 @@ std::array std_vec_to_std_array(const std::vector& vector) } return array; } +VectorXd std_vec_to_eigen_vector(const std::vector& 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 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,