From d1bc7d5c9f442ea6d8486e95f07ba1b2962a5899 Mon Sep 17 00:00:00 2001 From: QuentinLin Date: Wed, 14 Aug 2024 14:27:29 +0900 Subject: [PATCH] working coppelia mirror node (#1) Reviewed-on: https://gitea.qlin.me/qlin/sas_robot_driver_franka/pulls/1 Co-authored-by: QuentinLin Co-committed-by: QuentinLin --- CMakeLists.txt | 1 + src/coppelia/sas_robot_driver_coppelia.cpp | 425 ++++++++++----------- src/coppelia/sas_robot_driver_coppelia.h | 115 +++--- src/sas_robot_driver_coppelia_node.cpp | 41 +- 4 files changed, 290 insertions(+), 292 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fe8804..ab9e211 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,6 +158,7 @@ add_dependencies(qros_effector_driver_franka_hand ${catkin_EXPORTED_TARGETS}) add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp) target_link_libraries(sas_robot_driver_coppelia dqrobotics + dqrobotics-interface-json11 dqrobotics-interface-vrep) add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/src/coppelia/sas_robot_driver_coppelia.cpp b/src/coppelia/sas_robot_driver_coppelia.cpp index 552442c..2a47558 100644 --- a/src/coppelia/sas_robot_driver_coppelia.cpp +++ b/src/coppelia/sas_robot_driver_coppelia.cpp @@ -4,249 +4,240 @@ namespace sas { - -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(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops): + configuration_(configuration), + clock_(configuration.thread_sampling_time_nsec), + 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(); + ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using real robot, Instantiating robot interface to real driver."); + real_robot_interface_ = std::make_shared(nh, 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; - } + ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode); + robot_provider_ = std::make_shared(nh, 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("[" + ros::this_node::getName() + "]::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(); + ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Starting control loop..."); + while(!_should_shutdown()){ + clock_.update_and_sleep(); + ros::spinOnce(); + if(!ros::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{ + ROS_DEBUG_STREAM("[" + ros::this_node::getName() + "]::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{ + ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with coppelia..."); + connect(); + ROS_INFO_STREAM(ros::this_node::getName() << "::Connected to coppelia."); + ROS_INFO_STREAM(ros::this_node::getName() << "::Initializing..."); + initialize(); + ROS_INFO_STREAM(ros::this_node::getName() << "::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()<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); + ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::[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; - } + ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize..."); + ros::spinOnce(); + int count = 0; + while (!real_robot_interface_->is_enabled()) { 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()) - { - 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; - } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + count++; + if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){ + ROS_ERROR_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT..."); + throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized."); + } + if(!ros::ok()) { + ROS_WARN_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved. Exiting..."); + throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved, not OK."); } } + ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[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{ + ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[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) + { + 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()<(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC) * 1e-9; + auto current_joint_positions = simulated_joint_positions_; + clock.init(); + while (!(*break_loops_) && ros::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){ + ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" + e.what()); + simulation_thread_started_ = false; + }catch(...){ + ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown"); + simulation_thread_started_ = false; + } + +} diff --git a/src/coppelia/sas_robot_driver_coppelia.h b/src/coppelia/sas_robot_driver_coppelia.h index cabdd9f..23bcabd 100644 --- a/src/coppelia/sas_robot_driver_coppelia.h +++ b/src/coppelia/sas_robot_driver_coppelia.h @@ -39,10 +39,18 @@ #include #include #include -#include +#include +// #include +#include +#include +// #include #include +#include #include +#include +#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC 2000000 // 2ms, 500Hz +#define REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT 500 using namespace DQ_robotics; using namespace Eigen; @@ -54,81 +62,78 @@ namespace sas struct RobotDriverCoppeliaConfiguration { - int thread_sampling_time_nsec; - int port; - std::string ip; - std::vector jointnames; + int thread_sampling_time_nsec; // 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 mirror_mode; - std::string real_robot_topic_prefix; + bool using_real_robot; + std::string robot_topic_prefix; + std::string robot_parameter_file_path; + // VectorXd q_min; + // VectorXd q_max; }; -class RobotDriverCoppelia: public RobotDriver +class RobotDriverCoppelia { private: + enum ControlMode{ + Position=0, + Velocity + }; + RobotDriverCoppeliaConfiguration configuration_; - std::string robot_mode_ = std::string("VelocityControl"); // PositionControl - bool mirror_mode_ = false; - double gain_ = 3.0; - std::string real_robot_topic_prefix_; + Clock clock_; + std::atomic_bool* break_loops_; + bool _should_shutdown() const {return (*break_loops_);} + ControlMode robot_mode_; - 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_; + 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 RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops); + RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops); + int start_control_loop(); // public entry point - VectorXd get_joint_positions() override; - void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override; + void connect(); + void disconnect(); - 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; + void initialize(); + void deinitialize(); + std::tuple joint_limits_; }; } diff --git a/src/sas_robot_driver_coppelia_node.cpp b/src/sas_robot_driver_coppelia_node.cpp index 04c4654..bab80ec 100644 --- a/src/sas_robot_driver_coppelia_node.cpp +++ b/src/sas_robot_driver_coppelia_node.cpp @@ -26,6 +26,8 @@ # 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 # # ################################################################ */ @@ -52,6 +54,7 @@ void sig_int_handler(int) } + int main(int argc, char **argv) { if(signal(SIGINT, sig_int_handler) == SIG_ERR) @@ -61,39 +64,37 @@ int main(int argc, char **argv) ros::init(argc, argv, "sas_robot_driver_coppelia_node", ros::init_options::NoSigintHandler); ROS_WARN("====================================================================="); - ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------"); + ROS_WARN("--------------------- Quentin Lin -----------------------------------"); ROS_WARN("====================================================================="); ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server."); ros::NodeHandle nh; sas::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration; + sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_coppelia_configuration.thread_sampling_time_nsec); + sas::get_ros_param(nh,"/vrep_ip",robot_driver_coppelia_configuration.vrep_ip); + sas::get_ros_param(nh,"/vrep_port",robot_driver_coppelia_configuration.vrep_port); + sas::get_ros_param(nh,"/vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names); + sas::get_ros_param(nh,"/vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled); + sas::get_ros_param(nh,"/robot_mode", robot_driver_coppelia_configuration.robot_mode); + sas::get_ros_param(nh,"/using_real_robot", robot_driver_coppelia_configuration.using_real_robot); + sas::get_ros_param(nh,"/robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix); + sas::get_ros_param(nh,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path); - 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; + // std::vector q_min_vec, q_max_vec; + // sas::get_ros_param(nh,"/q_min", q_min_vec); + // robot_driver_coppelia_configuration.q_min = sas::std_vector_double_to_vectorxd(q_min_vec); + // sas::get_ros_param(nh,"/q_max", q_max_vec); + // robot_driver_coppelia_configuration.q_max = sas::std_vector_double_to_vectorxd(q_max_vec); - 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."); - sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration, + sas::RobotDriverCoppelia robot_driver_coppelia(nh, 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) {