135 lines
3.7 KiB
C++
135 lines
3.7 KiB
C++
/*
|
|
# 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;
|
|
|
|
|
|
};
|
|
}
|