Added a new robot coppelia driver

CMakeLists.txt

@@ -103,9 +103,18 @@ target_link_libraries(sas_robot_driver_franka
                       robot_interface_franka)
 
 
+add_library(sas_robot_driver_coppelia src/sas_robot_driver_coppelia.cpp)
+target_link_libraries(sas_robot_driver_coppelia
+                       dqrobotics
+                       dqrobotics-interface-vrep)
 
+add_dependencies(sas_robot_driver_franka   ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})                        
+add_executable(sas_robot_driver_coppelia_node src/sas_robot_driver_coppelia_node.cpp)
+target_link_libraries(sas_robot_driver_coppelia_node
+                      sas_robot_driver_coppelia
+                      ${catkin_LIBRARIES})
 
 add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp)                        
 target_link_libraries(sas_robot_driver_franka_node
@@ -153,6 +162,10 @@ install(TARGETS sas_robot_driver_franka_node
 DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+install(TARGETS sas_robot_driver_coppelia_node
+DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 ## Mark cpp header files for installation
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

cfg/sas_robot_driver_coppelia_franka_1.yaml

@@ -0,0 +1,10 @@
+"robot_ip_address": "10.198.113.159"
+"robot_mode": "VelocityControl"
+"real_robot_topic_prefix": "franka_1"
+"vrep_port": 20011
+"mirror_mode": true
+"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]
+"vrep_robot_joint_names": ["Franka_joint1","Franka_joint2","Franka_joint3","Franka_joint4","Franka_joint5","Franka_joint6","Franka_joint7"]
+

include/robot_coppelia_ros_interface.h

@@ -60,17 +60,35 @@ protected:
 
 
 public:
+
+
+    RobotCoppeliaRosInterface()=delete;
+
+    ~RobotCoppeliaRosInterface();
+
+
+    RobotCoppeliaRosInterface(const RobotCoppeliaRosInterface&) = delete;
+    RobotCoppeliaRosInterface& operator= (const RobotCoppeliaRosInterface&) = delete;
+
     RobotCoppeliaRosInterface(const ros::NodeHandle& nh,
                               const std::string& topic_prefix,
                               const RobotCoppeliaROSConfiguration& configuration,
                               std::atomic_bool* kill_this_node);
 
-    RobotCoppeliaRosInterface()=delete;
-
-    ~RobotCoppeliaRosInterface();
-    //VectorXd  get_joint_positions() const;
+    //VectorXd get_joint_positions() const;
+    //VectorXd get_joint_velocities() const;
     //void set_joint_target_positions(const VectorXd& target_joint_positions);
 
     //void set_joint_target_positions_(const VectorXd& target_joint_positions);
+
+    void connect();
+    void disconnect();
+
+    void initialize();
+    void deinitialize();
     int control_loop();
+
+
+
+
 };

include/sas_robot_driver_coppelia.h

@@ -0,0 +1,134 @@
+/*
+# 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_ = 0.5;
+    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;
+
+
+};
+}

launch/sas_robot_driver_coppelia_franka_1.launch

@@ -0,0 +1,17 @@
+<launch>
+
+
+
+<node pkg="sas_robot_driver_franka" type="sas_robot_driver_coppelia_node" name="franka_1_sim">
+<rosparam file="$(find sas_robot_driver_franka)/cfg/sas_robot_driver_coppelia_franka_1.yaml" command="load" 
+/>
+
+
+
+</node>
+
+
+
+
+
+</launch>

src/robot_coppelia_ros_interface.cpp

@@ -25,24 +25,52 @@ RobotCoppeliaRosInterface::RobotCoppeliaRosInterface(const ros::NodeHandle& nh,
     subscriber_target_joint_velocities_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_velocities", 1, &RobotCoppeliaRosInterface::_callback_target_joint_velocities, this);
     publisher_joint_states_ = nh_.advertise<sensor_msgs::JointState>(topic_prefix_+ "/get/joint_states", 1);
 
-    ROS_INFO_STREAM(ros::this_node::getName() << "::Connecting with CoppeliaSim...");
-    vi_ = std::make_shared<DQ_VrepInterface>();
-    vi_->connect(configuration_.ip, configuration_.port, 500, 10);
-    vi_->start_simulation();
-    ROS_INFO_STREAM(ros::this_node::getName() << "::Connection ok!");
+    connect();
+    initialize();
+
 }
 
 RobotCoppeliaRosInterface::~RobotCoppeliaRosInterface()
 {
-    vi_->disconnect();
+    deinitialize();
+    disconnect();
 }
 
-/*
-VectorXd RobotCoppeliaRosInterface::get_joint_positions() const
+
+
+void RobotCoppeliaRosInterface::connect()
 {
-    return joint_positions_;
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Connecting with CoppeliaSim...");
+    vi_ = std::make_shared<DQ_VrepInterface>();
+    vi_->connect(configuration_.ip, configuration_.port, 500, 10);
+
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Connection ok!");
 }
 
+void RobotCoppeliaRosInterface::disconnect()
+{
+    vi_->disconnect();
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Disconnecting simuation.");
+}
+
+void RobotCoppeliaRosInterface::initialize()
+{
+    vi_->start_simulation();
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Starting simuation...");
+}
+
+void RobotCoppeliaRosInterface::deinitialize()
+{
+    vi_->stop_simulation();
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Stopping simuation...");
+}
+
+
+
+
+/*
+
+
 
 /*
 void RobotCoppeliaRosInterface::set_joint_target_positions(const VectorXd &target_joint_positions)

src/sas_robot_driver_coppelia.cpp

@@ -0,0 +1,253 @@
+#include "sas_robot_driver_coppelia.h"
+
+
+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<DQ_VrepInterface>();
+    desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_);
+    auto nodehandle = ros::NodeHandle();
+    std::cout<<"Rostopic: "<<"/"+real_robot_topic_prefix_<<std::endl;
+    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<<"Connecting..."<<std::endl;
+}
+
+void RobotDriverCoppelia::disconnect()
+{
+    vi_->disconnect();
+    if (joint_velocity_control_mode_thread_.joinable())
+    {
+        joint_velocity_control_mode_thread_.join();
+    }
+    if (joint_velocity_control_mirror_mode_thread_.joinable())
+    {
+        joint_velocity_control_mirror_mode_thread_.join();
+    }
+    std::cout<<"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();
+    }
+    std::cout<<"Velocity loop running..."<<std::endl;
+}
+
+void RobotDriverCoppelia::deinitialize()
+{
+    vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
+    vi_->stop_simulation();
+    finish_motion();
+    std::cout<<"Deinitialized."<<std::endl;
+}
+
+
+
+
+void RobotDriverCoppelia::_update_robot_state(const VectorXd &q, const VectorXd &q_dot, const VectorXd &forces)
+{
+    current_joint_positions_  = q;
+    current_joint_velocities_ = q_dot;
+    current_joint_forces_     =  forces;
+}
+
+void RobotDriverCoppelia::finish_motion()
+{
+    for (int i=0;i<1000;i++)
+    {
+        set_target_joint_positions(current_joint_positions_);
+        set_target_joint_velocities(VectorXd::Zero(dim_configuration_space_));
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    finish_motion_ = true;
+}
+
+void RobotDriverCoppelia::_start_joint_velocity_control_mode()
+{
+    try{
+        finish_motion_ = false;
+        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);
+
+        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;
+            }
+        }
+    }
+    catch(const std::exception& e)
+    {
+        std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<<std::endl;
+    }
+    catch(...)
+    {
+        std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
+    }
+
+
+}
+
+void RobotDriverCoppelia::_start_joint_velocity_control_thread()
+{
+    finish_motion_ = false;
+    if (joint_velocity_control_mode_thread_.joinable())
+    {
+        joint_velocity_control_mode_thread_.join();
+    }
+    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);
+}
+
+void RobotDriverCoppelia::_start_joint_velocity_control_mirror_thread()
+{
+    finish_motion_ = false;
+    if (joint_velocity_control_mode_thread_.joinable())
+    {
+        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<<"Waiting for real robot topics..."<<std::endl;
+        VectorXd q;
+        while (ros::ok()) {
+            if (franka1_ros_->is_enabled())
+            {
+                    q = franka1_ros_->get_joint_positions();
+                    break;
+            }
+            ros::spinOnce();
+        }
+        std::cout<<"Done!"<<std::endl;
+
+        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);
+
+        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;
+                }
+            }
+        }
+    }
+    catch(const std::exception& e)
+    {
+        std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<<std::endl;
+    }
+    catch(...)
+    {
+        std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl;
+    }
+
+
+}
+
+
+
+
+} // sas namespace

src/sas_robot_driver_coppelia_node.cpp

@@ -0,0 +1,105 @@
+/*
+# 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_node.cpp
+#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
+#
+# ################################################################
+*/
+
+
+#include <sstream>
+
+#include <exception>
+#include <dqrobotics/utils/DQ_Math.h>
+//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+#include <sas_common/sas_common.h>
+#include <sas_conversions/eigen3_std_conversions.h>
+#include <sas_robot_driver/sas_robot_driver_ros.h>
+#include "sas_robot_driver_coppelia.h"
+
+/*********************************************
+ * SIGNAL HANDLER
+ * *******************************************/
+#include<signal.h>
+static std::atomic_bool kill_this_process(false);
+void sig_int_handler(int)
+{
+    kill_this_process = true;
+}
+
+
+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.");
+    }
+
+    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.");
+
+
+    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();
+
+    try
+    {
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot.");
+        sas::RobotDriverCoppelia robot_driver_coppelia(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();
+    }
+    catch (const std::exception& e)
+    {
+        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
+    }
+
+
+    return 0;
+}