From 0b848204972f6f43e2ea0fd2e8375fe69ca57ae9 Mon Sep 17 00:00:00 2001 From: QuentinLin Date: Wed, 14 Aug 2024 14:25:40 +0900 Subject: [PATCH] bug fix and tested coppelia mirror node --- CMakeLists.txt | 1 + src/coppelia/sas_robot_driver_coppelia.cpp | 75 ++++++++++++++-------- src/coppelia/sas_robot_driver_coppelia.h | 28 ++++---- src/sas_robot_driver_coppelia_node.cpp | 13 ++-- 4 files changed, 77 insertions(+), 40 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 27ebbce..2a47558 100644 --- a/src/coppelia/sas_robot_driver_coppelia.cpp +++ b/src/coppelia/sas_robot_driver_coppelia.cpp @@ -4,16 +4,21 @@ namespace sas { +RobotDriverCoppelia::~RobotDriverCoppelia() { + deinitialize(); + disconnect(); +} 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()), - joint_limits_(configuration.q_min, configuration.q_max) + vi_(std::make_shared()) { // 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) { ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using real robot, Instantiating robot interface to real driver."); @@ -34,23 +39,27 @@ RobotDriverCoppelia::RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCo } -void RobotDriverCoppelia::_update_vrep_position(VectorXd joint_positions){ +void RobotDriverCoppelia::_update_vrep_position(const VectorXd &joint_positions, const bool& force_update) const{ if(configuration_.vrep_dynamically_enabled){ - vi_->set_joint_target_positions(configuration_.vrep_robot_joint_names, joint_positions); + 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_robot_joint_names, joint_positions); + vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions); } } -void RobotDriverCoppelia::_update_vrep_velocity(VectorXd joint_velocity){ +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_robot_joint_names, joint_velocity); + 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(); @@ -62,21 +71,25 @@ void RobotDriverCoppelia::_start_control_loop(){ }else{ // robot_provider_ VectorXd target_joint_positions; - auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_robot_joint_names); + auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names); VectorXd current_joint_velocity; - 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 (...){} + 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{ - ROS_WARN_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()); @@ -119,7 +132,7 @@ int RobotDriverCoppelia::start_control_loop(){ void RobotDriverCoppelia::connect(){ - auto ret = vi_->connect(configuration_.ip, configuration_.port, 100, 10); + 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"); } @@ -133,19 +146,31 @@ void RobotDriverCoppelia::initialize(){ if(configuration_.using_real_robot) { 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::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()); + _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_robot_joint_names); + 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());} @@ -191,8 +216,8 @@ void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){ current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model // cap joint limit - auto q_min = joint_limits_.first; - auto q_max = joint_limits_.second; + 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); diff --git a/src/coppelia/sas_robot_driver_coppelia.h b/src/coppelia/sas_robot_driver_coppelia.h index 8b0840b..23bcabd 100644 --- a/src/coppelia/sas_robot_driver_coppelia.h +++ b/src/coppelia/sas_robot_driver_coppelia.h @@ -40,12 +40,17 @@ #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; @@ -65,8 +70,9 @@ struct RobotDriverCoppeliaConfiguration std::string robot_mode; bool using_real_robot; std::string robot_topic_prefix; - VectorXd q_min; - VectorXd q_max; + std::string robot_parameter_file_path; + // VectorXd q_min; + // VectorXd q_max; }; class RobotDriverCoppelia @@ -79,8 +85,8 @@ private: RobotDriverCoppeliaConfiguration configuration_; - sas::Clock clock_; - atomic_bool* break_loops_; + Clock clock_; + std::atomic_bool* break_loops_; bool _should_shutdown() const {return (*break_loops_);} ControlMode robot_mode_; @@ -97,20 +103,20 @@ private: VectorXd simulated_joint_velocities_; void start_simulation_thread(); // thread entry point void _velocity_control_simulation_thread_main(); - atomic_bool simulation_thread_started_ = false; + std::atomic_bool simulation_thread_started_{false}; bool initialized_ = false; - inline void _assert_initialized(){ + inline void _assert_initialized() const{ if (!initialized_){throw std::runtime_error("[RobotDriverCoppelia] Robot driver not initialized");} } - inline void _assert_is_alive(){ + 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(VectorXd joint_positions); - inline void _update_vrep_velocity(VectorXd joint_velocity); + 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; diff --git a/src/sas_robot_driver_coppelia_node.cpp b/src/sas_robot_driver_coppelia_node.cpp index e4defee..bab80ec 100644 --- a/src/sas_robot_driver_coppelia_node.cpp +++ b/src/sas_robot_driver_coppelia_node.cpp @@ -54,6 +54,7 @@ void sig_int_handler(int) } + int main(int argc, char **argv) { if(signal(SIGINT, sig_int_handler) == SIG_ERR) @@ -78,15 +79,19 @@ int main(int argc, char **argv) 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,"/q_min", robot_driver_coppelia_configuration.q_min); - sas::get_ros_param(nh,"/q_max", robot_driver_coppelia_configuration.q_max); + sas::get_ros_param(nh,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path); + + // 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); - 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); return robot_driver_coppelia.start_control_loop();