bug fix and tested coppelia mirror node

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})

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<DQ_VrepInterface>()),
+        vi_(std::make_shared<DQ_VrepInterface>())
-        joint_limits_(configuration.q_min, configuration.q_max)
 {
     // should initialize robot driver interface to real robot
+    DQ_SerialManipulatorDH smdh = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(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,8 +71,9 @@ 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_provider_->is_enabled()) {
                 if(robot_mode_ == ControlMode::Velocity)
                 {
                     if(robot_provider_->is_enabled(sas::RobotDriver::Functionality::VelocityControl)) {
@@ -72,12 +82,15 @@ void RobotDriverCoppelia::_start_control_loop(){
                         //                    try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){}
                     }
                     else{
-                    ROS_WARN_STREAM("[" + ros::this_node::getName() + "]::Velocity control is not enabled.");
+                        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_);
@@ -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_min = std::get<0>(joint_limits_);
-            auto q_max = joint_limits_.second;
+            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);

src/coppelia/sas_robot_driver_coppelia.h

@@ -40,12 +40,17 @@
 #include <dqrobotics/DQ.h>
 #include <sas_robot_driver/sas_robot_driver.h>
 #include <sas_clock/sas_clock.h>
-#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
+// #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_interface.h>
+#include <sas_robot_driver/sas_robot_driver_provider.h>
 
 #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;
+    std::string robot_parameter_file_path;
-    VectorXd q_max;
+    // VectorXd q_min;
+    // VectorXd q_max;
 };
 
 class RobotDriverCoppelia
@@ -79,8 +85,8 @@ private:
 
     RobotDriverCoppeliaConfiguration configuration_;
 
-    sas::Clock clock_;
+    Clock clock_;
-    atomic_bool* break_loops_;
+    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_position(const VectorXd &joint_positions, const bool& force_update=false) const;
-    inline void _update_vrep_velocity(VectorXd joint_velocity);
+    inline void _update_vrep_velocity(const VectorXd & joint_velocity) const;
 
 public:
     RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;

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,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
-    sas::get_ros_param(nh,"/q_max", robot_driver_coppelia_configuration.q_max);
+
+    // std::vector<double> 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();