qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

minor renaming of file and working coppelia node

Browse Source
This commit is contained in:
qlin960618
2024-08-16 11:30:58 +09:00
parent 94a32a0f0c
commit 900f6aa2e1
18 changed files with 504 additions and 404 deletions
Download Patch File Download Diff File Expand all files Collapse all files

430
src/coppelia/sas_robot_driver_coppelia.cpp
View File

@@ -1,252 +1,246 @@
#include "sas_robot_driver_coppelia.h"
#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
namespace sas
namespace qros
{
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<<"RobotDriverCoppelia::Rostopic: "<<"/"+real_robot_topic_prefix_<<std::endl;
franka1_ros_ = std::make_shared<sas::RobotDriverInterface>(nodehandle, "/"+real_robot_topic_prefix_);
RobotDriverCoppelia::~RobotDriverCoppelia() {
deinitialize();
disconnect();
}
VectorXd RobotDriverCoppelia::get_joint_positions()
RobotDriverCoppelia::RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
configuration_(configuration),
node_sptr_(node_sptr),
clock_(configuration.thread_sampling_time_sec),
break_loops_(break_loops),
robot_mode_(ControlMode::Position),
vi_(std::make_shared<DQ_VrepInterface>())
{
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..."<<std::endl;
}
void RobotDriverCoppelia::disconnect()
{
vi_->disconnect();
if (joint_velocity_control_mode_thread_.joinable())
// 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)
{
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."<<std::endl;
}
void RobotDriverCoppelia::initialize()
{
vi_->start_simulation();
if (mirror_mode_ == false)
{
_start_joint_velocity_control_thread();
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using real robot, Instantiating robot interface to real driver at ["+configuration_.robot_topic_prefix+"].");
real_robot_interface_ = std::make_shared<sas::RobotDriverClient>(node_sptr_, configuration_.robot_topic_prefix);
}else{
_start_joint_velocity_control_mirror_thread();
}
std::cout<<"RobotDriverCoppelia::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<<"RobotDriverCoppelia::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;
}
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode);
robot_provider_ = std::make_shared<sas::RobotDriverServer>(node_sptr_, 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("[" + std::string(node_sptr_->get_name()) + "]::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();
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Starting control loop...");
while(!_should_shutdown()){
clock_.update_and_sleep();
rclcpp::spin_some(node_sptr_);
if(!rclcpp::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{
RCLCPP_DEBUG_STREAM(node_sptr_->get_logger(), "::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{
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Waiting to connect with coppelia...");
connect();
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Connected to coppelia.");
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Initializing...");
initialize();
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::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()<<std::endl;
RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Error or exception caught::" << e.what());
}
catch(...)
{
std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Unexpected error or exception caught");
}
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Deinitializing...");
deinitialize();
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::deinitialized.");
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnecting from coppelia...");
disconnect();
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnected from coppelia.");
return 0;
}
void RobotDriverCoppelia::_start_joint_velocity_control_thread()
{
finish_motion_ = false;
if (joint_velocity_control_mode_thread_.joinable())
{
joint_velocity_control_mode_thread_.join();
void RobotDriverCoppelia::connect(){
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");
}
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);
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[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..."<<std::endl;
VectorXd q;
while (ros::ok()) {
if (franka1_ros_->is_enabled())
{
q = franka1_ros_->get_joint_positions();
break;
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
rclcpp::spin_some(node_sptr_);
int count = 0;
while (!real_robot_interface_->is_enabled(sas::RobotDriver::Functionality::PositionControl)) {
rclcpp::spin_some(node_sptr_);
// std::cout<<"Waiting for real robot interface to initialize..."<<std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(100));
rclcpp::spin_some(node_sptr_);
count++;
if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){
RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
}
if(!rclcpp::ok()) {
RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::ROS shutdown received. Exiting...");
throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::ROS shutdown received, not OK.");
}
ros::spinOnce();
}
std::cout<<"RobotDriverCoppelia::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())
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[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{
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[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)
{
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;
}
}
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()<<std::endl;
}
catch(...)
{
std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl;
}
}
void RobotDriverCoppelia::deinitialize(){
// nothing to do
}
void RobotDriverCoppelia::start_simulation_thread(){ // thread entry point
if(simulation_thread_started_){
throw std::runtime_error("[RobotDriverCoppelia]::start_simulation_thread::Simulation thread already started");
}
if(velocity_control_simulation_thread_.joinable()){
velocity_control_simulation_thread_.join();
}
velocity_control_simulation_thread_ = std::thread(&RobotDriverCoppelia::_velocity_control_simulation_thread_main, this);
}
void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){
/**
* This thread should not access vrep
*/
simulation_thread_started_ = true;
try{
RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC);
double tau = VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC;
auto current_joint_positions = simulated_joint_positions_;
clock.init();
while (!(*break_loops_) && rclcpp::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){
RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" << e.what());
simulation_thread_started_ = false;
}catch(...){
RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
simulation_thread_started_ = false;
}
break_loops_->store(true);
}

134
src/coppelia/sas_robot_driver_coppelia.h
View File

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

2
src/hand/qros_effector_driver_franka_hand.cpp
View File

@@ -27,7 +27,7 @@
#
# ################################################################
*/
#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.h>
#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
#include <franka/exception.h>

31
src/hand/robot_interface_hand.cpp
View File

@@ -1,4 +1,33 @@
#include "sas_robot_driver_franka/robot_interface_hand.h"
/*
# 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. Quenitin Lin
#
# ################################################################
*/
#include "sas_robot_driver_franka/robot_interface_hand.hpp"

2
src/joint/robot_interface_franka.cpp
View File

@@ -30,7 +30,7 @@
*/
#include <sas_robot_driver_franka/interfaces/robot_interface_franka.h>
#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
/**

9
src/joint/sas_robot_driver_franka.cpp
View File

@@ -31,7 +31,7 @@
*/
#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
#include <sas_core/sas_clock.hpp>
#include <dqrobotics/utils/DQ_Math.h>
@@ -42,8 +42,8 @@ namespace sas
const std::shared_ptr<Node> &node,
const std::shared_ptr<qros::RobotDynamicsServer> &robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops
):
node_(node),
RobotDriver(break_loops),
node_(node),
configuration_(configuration),
robot_dynamic_provider_sptr_(robot_dynamic_provider),
break_loops_(break_loops)
@@ -55,11 +55,10 @@ namespace sas
//joint_positions_buffer_.resize(8,0);
//end_effector_pose_euler_buffer_.resize(7,0);
//end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
std::cout<<configuration.ip_address<<std::endl;
// std::cout<<configuration.ip_address<<std::endl;
RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
std::cout<<configuration.mode<<std::endl;
RCLCPP_INFO_STREAM(node_->get_logger(), "Mode: " << configuration_.mode);
if (configuration_.mode == std::string("None"))
{

71
src/sas_robot_driver_coppelia_node.cpp
View File

@@ -26,6 +26,9 @@
# 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
# - porting to ROS2
#
# ################################################################
*/
@@ -36,10 +39,9 @@
#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 "coppelia/sas_robot_driver_coppelia.h"
#include <sas_common/sas_common.hpp>
#include <sas_conversions/eigen3_std_conversions.hpp>
#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
/*********************************************
* SIGNAL HANDLER
@@ -52,52 +54,51 @@ void sig_int_handler(int)
}
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.");
throw std::runtime_error("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.");
rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
auto context = rclcpp::contexts::get_global_default_context();
auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_coppelia_node");
const auto node_name = std::string(node->get_name());
RCLCPP_WARN(node->get_logger(),"=====================================================================");
RCLCPP_WARN(node->get_logger(),"-----------------Adapted by Quentin Lin ------------------------");
RCLCPP_WARN(node->get_logger(),"=====================================================================");
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),":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();
qros::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_coppelia_configuration.thread_sampling_time_sec);
sas::get_ros_parameter(node,"vrep_ip",robot_driver_coppelia_configuration.vrep_ip);
sas::get_ros_parameter(node,"vrep_port",robot_driver_coppelia_configuration.vrep_port);
sas::get_ros_parameter(node,"vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names);
sas::get_ros_parameter(node,"vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled);
sas::get_ros_parameter(node,"robot_mode", robot_driver_coppelia_configuration.robot_mode);
sas::get_ros_parameter(node,"using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
sas::get_ros_parameter(node,"robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
sas::get_ros_parameter(node,"robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
try
{
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot.");
sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration,
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"::Instantiating Coppelia robot mirror.");
qros::RobotDriverCoppelia robot_driver_coppelia(node, 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)
{
ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
kill_this_process = true;
RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
shutdown(context, "Exception in main function: " + std::string(e.what()));
return -1;
}

2
src/sas_robot_driver_franka_hand_node.cpp
View File

@@ -35,7 +35,7 @@
#include <dqrobotics/utils/DQ_Math.h>
#include <sas_common/sas_common.hpp>
// #include <sas_conversions/eigen3_std_conversions.hpp>
#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.h>
#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
/*********************************************

14
src/sas_robot_driver_franka_node.cpp
View File

@@ -41,7 +41,7 @@
#include <sas_common/sas_common.hpp>
#include <sas_conversions/eigen3_std_conversions.hpp>
#include <sas_robot_driver/sas_robot_driver_ros.hpp>
#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
@@ -76,6 +76,15 @@ std::array<T, N> std_vec_to_std_array(const std::vector<T>& vector)
}
return array;
}
VectorXd std_vec_to_eigen_vector(const std::vector<double>& vector)
{
VectorXd eigen_vector(vector.size());
for(std::size_t i = 0; i < vector.size(); i++)
{
eigen_vector(i) = vector[i];
}
return eigen_vector;
}
int main(int argc, char **argv)
@@ -164,7 +173,8 @@ int main(int argc, char **argv)
robot_driver_franka_configuration,
&kill_this_process
);
//robot_driver_franka.set_joint_limits({qmin, qmax});
std::tuple<VectorXd, VectorXd> joint_limits = {std_vec_to_eigen_vector(robot_driver_ros_configuration.q_min), std_vec_to_eigen_vector(robot_driver_ros_configuration.q_max)};
robot_driver_franka -> set_joint_limits(joint_limits);
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating RobotDriverROS.");
sas::RobotDriverROS robot_driver_ros(node,
robot_driver_franka,