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

migration to ros2 done

Browse Source
This commit is contained in:
qlin960618
2024-07-27 13:07:37 +09:00
parent f5552be8d6
commit 7dfd4d40b8
94 changed files with 13216 additions and 2224 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/generator/custom_motion_generation.cpp
View File

@@ -28,7 +28,7 @@
#
# ################################################################
*/
#include "generator/custom_motion_generation.h"
#include <sas_robot_driver_franka/generator/custom_motion_generation.h>
/**
* @brief CustomMotionGeneration::CustomMotionGeneration

2
src/generator/motion_generator.cpp
View File

@@ -1,6 +1,6 @@
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include "generator/motion_generator.h"
#include <sas_robot_driver_franka/generator/motion_generator.h>
#include <algorithm>
#include <array>
#include <cmath>

2
src/generator/quadratic_program_motion_generator.cpp
View File

@@ -1,4 +1,4 @@
#include "generator/quadratic_program_motion_generator.h"
#include <sas_robot_driver_franka/generator/quadratic_program_motion_generator.h>
QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor,

151
src/hand/qros_effector_driver_franka_hand.cpp
View File

@@ -27,10 +27,13 @@
#
# ################################################################
*/
#include "hand/qros_effector_driver_franka_hand.h"
#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.h>
#include <franka/exception.h>
using namespace std::placeholders;
using namespace sas_robot_driver_franka_interfaces;
namespace qros
{
@@ -48,19 +51,19 @@ namespace qros
EffectorDriverFrankaHand::EffectorDriverFrankaHand(
const std::string& driver_node_prefix,
const EffectorDriverFrankaHandConfiguration& configuration,
ros::NodeHandle& node_handel,
std::shared_ptr<Node> node,
std::atomic_bool* break_loops):
driver_node_prefix_(driver_node_prefix),
configuration_(configuration),
node_handel_(node_handel),
node_(node),
break_loops_(break_loops)
{
gripper_sptr_ = nullptr;
grasp_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/grasp",
&EffectorDriverFrankaHand::_grasp_srv_callback, this);
move_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/move",
&EffectorDriverFrankaHand::_move_srv_callback, this);
gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::GripperState>(
grasp_srv_ = node->create_service<srv::Grasp>(driver_node_prefix_ + "/grasp",
std::bind(&EffectorDriverFrankaHand::_grasp_srv_callback, this, _1, _2));
move_srv_ = node->create_service<srv::Move>(driver_node_prefix_ + "/move",
std::bind(&EffectorDriverFrankaHand::_move_srv_callback, this, _1, _2));
gripper_status_publisher_ = node->create_publisher<msg::GripperState>(
driver_node_prefix_ + "/gripper_status", 1);
}
@@ -77,54 +80,47 @@ namespace qros
void EffectorDriverFrankaHand::start_control_loop()
{
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
clock.init();
ROS_INFO_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
RCLCPP_INFO_STREAM(node_->get_logger(),
"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
while (!(*break_loops_))
{
if (!_is_connected()) throw std::runtime_error(
"[" + ros::this_node::getName() +
"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
if (!status_loop_running_)
{
ROS_WARN_STREAM(
"["+ ros::this_node::getName()+
"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
break_loops_->store(true);
break;
}
clock.update_and_sleep();
ros::spinOnce();
spin_some(node_);
}
ROS_INFO_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
}
void EffectorDriverFrankaHand::connect()
{
#ifdef IN_TESTING
ROS_WARN_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
return;
#endif
gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
if (!_is_connected()) throw std::runtime_error(
"[" + ros::this_node::getName() +
"[" + std::string(node_->get_name())+
"]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot.");
}
void EffectorDriverFrankaHand::disconnect() noexcept
{
#ifdef IN_TESTING
ROS_WARN_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
return;
#endif
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnecting...");
RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::disconnecting...");
gripper_sptr_->~Gripper();
gripper_sptr_ = nullptr;
}
@@ -135,26 +131,23 @@ namespace qros
void EffectorDriverFrankaHand::gripper_homing()
{
#ifdef IN_TESTING
ROS_WARN_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
return;
#endif
if (!_is_connected()) throw std::runtime_error(
"[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
"[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
auto ret = gripper_sptr_->homing();
if (!ret)
{
throw std::runtime_error(
"[" + ros::this_node::getName() +
"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
}
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
}
void EffectorDriverFrankaHand::initialize()
{
if (!_is_connected()) throw std::runtime_error(
"[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
"[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
gripper_homing();
// start gripper status loop
status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
@@ -163,7 +156,7 @@ namespace qros
void EffectorDriverFrankaHand::deinitialize()
{
#ifdef IN_TESTING
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
return;
#endif
if (_is_connected())
@@ -177,20 +170,19 @@ namespace qros
}
bool EffectorDriverFrankaHand::_grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req,
sas_robot_driver_franka::Grasp::Response& res)
bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> req, std::shared_ptr<srv::Grasp::Response> res)
{
ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
auto force = req.force;
auto speed = req.speed;
auto epsilon_inner = req.epsilon_inner;
auto epsilon_outer = req.epsilon_outer;
RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
auto force = req->force;
auto speed = req->speed;
auto epsilon_inner = req->epsilon_inner;
auto epsilon_outer = req->epsilon_outer;
if (force <= 0.0) force = configuration_.default_force;
if (speed <= 0.0) speed = configuration_.default_speed;
if (epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner;
if (epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer;
ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req.width));
ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req->width));
RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
bool ret = false;
bool function_ret = true;
gripper_in_use_.lock();
@@ -200,30 +192,29 @@ namespace qros
#else
try
{
ret = gripper_sptr_->grasp(req.width, speed, force, epsilon_inner, epsilon_outer);
ret = gripper_sptr_->grasp(req->width, speed, force, epsilon_inner, epsilon_outer);
}catch(franka::CommandException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + e.what());
RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + e.what());
function_ret = false;
}catch(franka::NetworkException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + e.what());
RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + e.what());
function_ret = false;
}
#endif
gripper_in_use_.unlock();
res.success = ret;
res->set__success(ret);
return function_ret;
}
bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req,
sas_robot_driver_franka::Move::Response& res)
bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> req, std::shared_ptr<srv::Move::Response> res)
{
ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
auto speed = req.speed;
RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
auto speed = req->speed;
if (speed <= 0.0) speed = configuration_.default_speed;
ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req.width));
RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req->width));
bool ret = false;
bool function_ret = true;
gripper_in_use_.lock();
@@ -233,19 +224,19 @@ namespace qros
#else
try
{
ret = gripper_sptr_->move(req.width, speed);
ret = gripper_sptr_->move(req->width, speed);
}catch(franka::CommandException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + e.what());
RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + e.what());
function_ret = false;
}catch(franka::NetworkException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + e.what());
RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + e.what());
function_ret = false;
}
#endif
gripper_in_use_.unlock();
res.success = ret;
res->set__success(ret);
return function_ret;
}
@@ -253,28 +244,25 @@ namespace qros
void EffectorDriverFrankaHand::_gripper_status_loop()
{
status_loop_running_ = true;
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
ROS_INFO_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
;
clock.init();
try
{
while (status_loop_running_)
{
#ifdef BLOCK_READ_IN_USED
bool should_unlock = false;
#endif
if (!_is_connected()) throw std::runtime_error(
"[" + ros::this_node::getName() +
"[" + std::string(node_->get_name())+
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
try
{
#ifdef IN_TESTING
ROS_WARN_STREAM(
"["+ ros::this_node::getName()+
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
throw std::runtime_error(
"[" + ros::this_node::getName() +
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
#endif
#ifdef BLOCK_READ_IN_USED
gripper_in_use_.lock();
@@ -284,23 +272,23 @@ namespace qros
#ifdef BLOCK_READ_IN_USED
gripper_in_use_.unlock();
#endif
sas_robot_driver_franka::GripperState msg;
msg.width = static_cast<float>(gripper_state.width);
msg.max_width = static_cast<float>(gripper_state.max_width);
msg.is_grasped = gripper_state.is_grasped;
msg.temperature = gripper_state.temperature;
msg.duration_ms = gripper_state.time.toMSec();
gripper_status_publisher_.publish(msg);
msg::GripperState msg;
msg.set__width(static_cast<float>(gripper_state.width));
msg.set__max_width(static_cast<float>(gripper_state.max_width));
msg.set__is_grasped(gripper_state.is_grasped);
msg.set__temperature(gripper_state.temperature);
msg.set__duration_ms(gripper_state.time.toMSec());
gripper_status_publisher_->publish(msg);
}
catch (...)
{
#ifdef BLOCK_READ_IN_USED
if (should_unlock) gripper_in_use_.unlock();
#endif
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
sas_robot_driver_franka::GripperState msg;
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
msg::GripperState msg;
msg.width = 0.0;
gripper_status_publisher_.publish(msg);
gripper_status_publisher_->publish(msg);
}
clock.update_and_sleep();
@@ -309,19 +297,16 @@ namespace qros
}
catch (std::exception& e)
{
ROS_ERROR_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.
RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.
what());
status_loop_running_ = false;
}
catch (...)
{
ROS_ERROR_STREAM(
"["+ ros::this_node::getName()+
RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
status_loop_running_ = false;
}
ROS_INFO_STREAM(
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
}
} // qros

126
src/hand/qros_effector_driver_franka_hand.h
View File

@@ -1,126 +0,0 @@
#pragma once
/*
# 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 <exception>
#include <tuple>
#include <atomic>
#include <vector>
#include <memory>
#include <franka/robot.h>
#include <franka/gripper.h>
#include <thread>
#include <mutex>
// #define BLOCK_READ_IN_USED
// #define IN_TESTING
// #include <sas_robot_driver/sas_robot_driver.h>
#include <sas_clock/sas_clock.h>
#include <ros/ros.h>
#include <ros/service.h>
#include <sas_robot_driver_franka/Grasp.h>
#include <sas_robot_driver_franka/Move.h>
#include <sas_robot_driver_franka/GripperState.h>
#ifdef IN_TESTING
#include <Move.h> // dummy include
#include <Grasp.h> // dummy include
#include <GripperState.h> // dummy include
#endif
// using namespace DQ_robotics;
// using namespace Eigen;
namespace qros {
struct EffectorDriverFrankaHandConfiguration
{
std::string robot_ip;
int thread_sampeling_time_ns = 1e8; // 10Hz
double default_force = 3.0;
double default_speed = 0.1;
double default_epsilon_inner = 0.005;
double default_epsilon_outer = 0.005;
};
class EffectorDriverFrankaHand{
private:
std::string driver_node_prefix_;
EffectorDriverFrankaHandConfiguration configuration_;
ros::NodeHandle& node_handel_;
std::shared_ptr<franka::Gripper> gripper_sptr_;
std::atomic_bool* break_loops_;
bool _is_connected() const;
// thread specific functions
void _gripper_status_loop();
std::thread status_loop_thread_;
std::atomic_bool status_loop_running_{false};
ros::Publisher gripper_status_publisher_;
std::mutex gripper_in_use_;
ros::ServiceServer grasp_server_;
ros::ServiceServer move_server_;
public:
bool _grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res);
bool _move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res);
EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
EffectorDriverFrankaHand()=delete;
~EffectorDriverFrankaHand();
EffectorDriverFrankaHand(
const std::string &driver_node_prefix,
const EffectorDriverFrankaHandConfiguration& configuration,
ros::NodeHandle& node_handel,
std::atomic_bool* break_loops);
void start_control_loop();
void gripper_homing();
void connect() ;
void disconnect() noexcept;
void initialize() ;
void deinitialize() ;
};
} // qros

2
src/hand/robot_interface_hand.cpp
View File

@@ -1,4 +1,4 @@
#include "robot_interface_hand.h"
#include "sas_robot_driver_franka/robot_interface_hand.h"

8
src/joint/robot_interface_franka.cpp
View File

@@ -30,10 +30,7 @@
*/
#include "robot_interface_franka.h"
#include <ros/this_node.h>
#include <rosconsole/macros_generated.h>
#include <sas_robot_driver_franka/interfaces/robot_interface_franka.h>
/**
@@ -313,7 +310,8 @@ void RobotInterfaceFranka::initialize()
initialize_flag_ = true;
// initialize and set the robot to default behavior (collision behavior, impedance, etc)
setDefaultBehavior(*robot_sptr_);
// robot_sptr_->setDefaultBehavior();
// setDefaultBehavior(*robot_sptr_);
robot_sptr_->setCollisionBehavior(
franka_configuration_.lower_torque_threshold,
franka_configuration_.upper_torque_threshold,

257
src/joint/robot_interface_franka.h
View File

@@ -1,257 +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)
# - Original implementation
#
# ################################################################
*/
#pragma once
#include <dqrobotics/DQ.h>
#include <memory.h>
#include <tuple>
#include <exception>
#include <vector>
#include <franka/robot.h>
#include <franka/gripper.h>
#include <franka/exception.h>
#include "generator/motion_generator.h"
#include <thread>
#include "generator/quadratic_program_motion_generator.h"
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
#include <atomic>
#include "generator/custom_motion_generation.h"
using namespace DQ_robotics;
using namespace Eigen;
/**
* @brief get homgenious_tf_to_DQ
* @param pose_vector (column major)
* @return pose
*/
inline DQ homgenious_tf_to_DQ(const VectorXd& pose_vector) {
// VectorXd column major
const auto t = DQ(0, pose_vector(12), pose_vector(13), pose_vector(14));
Eigen::Matrix<double, 3, 3, Eigen::ColMajor> rot_mat;
rot_mat << pose_vector(0), pose_vector(4), pose_vector(8),
pose_vector(1), pose_vector(5), pose_vector(9),
pose_vector(2), pose_vector(6), pose_vector(10);
Quaternion<double> quaternion(rot_mat);
const auto r = normalize(DQ(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()));
return r + 0.5 * E_ * t * r;
}
class RobotInterfaceFranka
{
public:
struct FrankaInterfaceConfiguration {
std::array<double, 7> lower_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
std::array<double, 7> upper_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
std::array<double, 6> lower_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
std::array<double, 6> upper_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
std::array<double, 7> joint_impedance={3000, 3000, 3000, 2500, 2500, 2000, 2000}; // K_theta (Nm/rad)
std::array<double, 6> cartesian_impedance={3000, 3000, 3000, 300, 300, 300}; // K_x (N/m)
};
enum class MODE{
None=0,
PositionControl,
VelocityControl,
ForceControl,
Homing,
ClearPositions,
};
private:
using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
std::string ip_ = "172.16.0.2";
FrankaInterfaceConfiguration franka_configuration_;
double speed_factor_joint_position_controller_ = 0.2;
double speed_gripper_ = 0.02;
VectorXd desired_joint_positions_;
VectorXd desired_joint_velocities_ = VectorXd::Zero(7);
VectorXd current_joint_positions_;
std::array<double, 7> current_joint_positions_array_;
VectorXd current_joint_velocities_;
std::array<double, 7> current_joint_velocities_array_;
VectorXd current_joint_forces_;
std::array<double, 7> current_joint_forces_array_;
Vector3d current_stiffness_force_;
Vector3d current_stiffness_torque_;
std::array<double, 6> current_stiffness_force_torque_array_;
VectorXd current_stiffness_pose_;
VectorXd current_effeector_pose_;
std::array<double, 16> current_stiffness_pose_array_;
std::array<double, 16> current_effector_pose_array_;
franka::RobotMode robot_mode_;
double time_ = 0;
bool initialize_flag_ = false;
void _check_if_robot_is_connected();
void _check_if_hand_is_connected();
RobotInterfaceFranka::MODE mode_;
void _set_driver_mode(const RobotInterfaceFranka::MODE& mode);
void _restart_default_parameters();
void _update_robot_state(const franka::RobotState& robot_state, const double& time);
std::shared_ptr<franka::Robot> robot_sptr_;
std::shared_ptr<franka::Gripper> gripper_sptr_;
std::unique_ptr<QuadraticProgramMotionGenerator> trajectory_generator_sptr_;
std::unique_ptr<CustomMotionGeneration> custom_generator_sptr_;
void _setDefaultRobotBehavior();
//bool hand_enabled_ = false;
const VectorXd q_home_configuration_ =(VectorXd(7)<<0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4).finished();
const VectorXd q_max_ = ((VectorXd(7) << 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895).finished());
const VectorXd q_min_ = ((VectorXd(7) << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished());
const VectorXd q_min_dot_ = ((VectorXd(7) << -2, -1, -1.5, -1.25, -3, -1.5, -3).finished());
const VectorXd q_max_dot_ = ((VectorXd(7) << 2, 1, 1.5, 1.25, 3, 1.5, 3).finished());
const double samples_ = 20;
VectorXd desired_mean_joint_positions_;
bool verbose_ = true;
std::string status_message_ = " ";
void _update_status_message(const std::string& message, const bool& verbose = true);
//-------To handle the threads-----------------
std::atomic<bool> exit_on_err_={false};
void _start_joint_position_control_mode();
std::thread joint_position_control_mode_thread_;
void _start_joint_position_control_thread();
std::atomic<bool> finish_motion_;
void _start_echo_robot_state_mode();
std::thread echo_robot_state_mode_thread_;
void _start_echo_robot_state_mode_thread();
std::atomic<bool> finish_echo_robot_state_;
void _finish_echo_robot_state();
void _start_joint_velocity_control_mode();
std::thread joint_velocity_control_mode_thread_;
void _start_joint_velocity_control_thread();
//----------------------------------------------
VectorXd _compute_recursive_mean(const double& samples, const VectorXd& q);
VectorXd _read_once_smooth_initial_positions(const double& samples);
public:
enum HAND{ON=0, OFF};
enum CONNECTION{AUTOMATIC};
enum GRIPPER_STATES{WIDTH=0, MAX_WIDTH=1};
RobotInterfaceFranka() = delete;
RobotInterfaceFranka(const RobotInterfaceFranka&) = delete;
RobotInterfaceFranka& operator= (const RobotInterfaceFranka&) = delete;
RobotInterfaceFranka(
const FrankaInterfaceConfiguration &configuration,
const std::string &ROBOT_IP,
const MODE& mode,
const HAND& hand = ON);
VectorXd read_once_initial_positions();
VectorXd get_joint_target_positions();
void move_robot_to_target_joint_positions(const VectorXd& q_target);
double read_gripper(const GRIPPER_STATES& gripper_state = WIDTH);
bool read_grasped_status();
void set_gripper(const double& width);
void gripper_homing();
VectorXd get_home_robot_configuration();
std::string get_status_message();
bool get_err_state() const {return exit_on_err_.load();}
std::string get_robot_mode();
void finish_motion();
std::shared_ptr<franka::Robot> get_robot_pointer();
//--------sas compatible methods----------//
VectorXd get_joint_positions();
VectorXd get_joint_velocities();
VectorXd get_joint_forces();
std::tuple<Vector3d, Vector3d> get_stiffness_force_torque();
DQ get_stiffness_pose();
double get_time();
void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad);
void connect();
void initialize();
void deinitialize();
void disconnect();
void set_target_joint_velocities(const VectorXd& target_joint_velocities);
/* To be implemented
std::tuple<VectorXd, VectorXd> get_joint_limits() const;
void set_joint_limits(const std::tuple<VectorXd, VectorXd>& joint_limits);
*/
};

25
src/joint/sas_robot_driver_franka.cpp
View File

@@ -31,15 +31,18 @@
*/
#include "../../include/sas_robot_driver_franka.h"
#include "sas_clock/sas_clock.h"
#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
#include <sas_core/sas_clock.hpp>
#include <dqrobotics/utils/DQ_Math.h>
#include <ros/this_node.h>
#include <rosconsole/macros_generated.h>
namespace sas
{
RobotDriverFranka::RobotDriverFranka(qros::RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
RobotDriverFranka::RobotDriverFranka(
const std::shared_ptr<Node> &node,
qros::RobotDynamicsServer* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops
):
node_(node),
RobotDriver(break_loops),
configuration_(configuration),
robot_dynamic_provider_(robot_dynamic_provider),
@@ -143,7 +146,9 @@ namespace sas
VectorXd RobotDriverFranka::get_joint_positions()
{
if(robot_driver_interface_sptr_->get_err_state()) {
ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
RCLCPP_ERROR_STREAM(node_->get_logger(),
"["+std::string(node_->get_name())+"]::driver interface "
"error on:"+robot_driver_interface_sptr_->get_status_message());
break_loops_->store(true);
}
_update_stiffness_contact_and_pose();
@@ -160,7 +165,9 @@ namespace sas
{
robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad);
if(robot_driver_interface_sptr_->get_err_state()) {
ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
RCLCPP_ERROR_STREAM(node_->get_logger(),
"["+std::string(node_->get_name())+"]::driver interface "
"error on:"+robot_driver_interface_sptr_->get_status_message());
break_loops_->store(true);
}
}
@@ -185,7 +192,9 @@ namespace sas
desired_joint_velocities_ = desired_joint_velocities;
robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities);
if(robot_driver_interface_sptr_->get_err_state()) {
ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
RCLCPP_ERROR_STREAM(node_->get_logger(),
"["+std::string(node_->get_name())+"]::driver interface "
"error on:"+robot_driver_interface_sptr_->get_status_message());
break_loops_->store(true);
}
}

102
src/robot_dynamics/qros_robot_dynamics_client.cpp Normal file
View File

@@ -0,0 +1,102 @@
/*
# Copyright (c) 2024 Quenitin Lin
#
# 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: Quenitin Lin
#
# ################################################################
#
# Contributors:
# 1. Quenitin Lin
#
# ################################################################
*/
#include <memory>
#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp>
using namespace qros;
RobotDynamicsClient::RobotDynamicsClient(const std::shared_ptr<Node> &node, const std::string& topic_prefix):
node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix),
child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"),
last_stiffness_force_(Vector3d::Zero()),
last_stiffness_torque_(Vector3d::Zero()),
last_stiffness_frame_pose_(0)
{
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
// Strip potential leading slash
if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
RCLCPP_INFO_STREAM(node_->get_logger(),
"["+ std::string(node_->get_name()) + "]::Initializing RobotDynamicsClient with prefix " + topic_prefix);
subscriber_cartesian_stiffness_ = node_->create_subscription<geometry_msgs::msg::WrenchStamped>(topic_prefix + "/get/cartesian_stiffness", 1,
std::bind(&RobotDynamicsClient::_callback_cartesian_stiffness, this, std::placeholders::_1)
);
}
void RobotDynamicsClient::_callback_cartesian_stiffness(const geometry_msgs::msg::WrenchStamped &msg)
{
last_stiffness_force_(0) = msg.wrench.force.x;
last_stiffness_force_(1) = msg.wrench.force.y;
last_stiffness_force_(2) = msg.wrench.force.z;
last_stiffness_torque_(0) = msg.wrench.torque.x;
last_stiffness_torque_(1) = msg.wrench.torque.y;
last_stiffness_torque_(2) = msg.wrench.torque.z;
try {
const auto transform_stamped = tf_buffer_->lookupTransform( parent_frame_id_, child_frame_id_, tf2::TimePointZero);
last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
} catch (tf2::TransformException &ex) {
RCLCPP_WARN_STREAM(node_->get_logger(), "["+ std::string(node_->get_name()) + "]::" + ex.what());
}
}
DQ RobotDynamicsClient::_geometry_msgs_transform_to_dq(const geometry_msgs::msg::Transform& transform)
{
const auto t = DQ(0,transform.translation.x,transform.translation.y,transform.translation.z);
const auto r = DQ(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
return r + 0.5 * E_ * t * r;
}
VectorXd RobotDynamicsClient::get_stiffness_force()
{
if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_force on uninitialized topic");
return last_stiffness_force_;
}
VectorXd RobotDynamicsClient::get_stiffness_torque()
{
if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_torque on uninitialized topic");
return last_stiffness_torque_;
}
DQ RobotDynamicsClient::get_stiffness_frame_pose()
{
if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_frame_pose on uninitialized Interface");
return last_stiffness_frame_pose_;
}
bool RobotDynamicsClient::is_enabled() const
{
if(last_stiffness_frame_pose_==0) return false;
return true;
}

103
src/robot_dynamics/qros_robot_dynamics_interface.cpp
View File

@@ -1,103 +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. Quenitin Lin
#
# ################################################################
*/
#include <robot_dynamic/qros_robot_dynamics_interface.h>
using namespace qros;
RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &nodehandle, const std::string &node_prefix):
RobotDynamicInterface(nodehandle, nodehandle, node_prefix)
{
//Delegated
}
RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
node_prefix_(node_prefix),
child_frame_id_(node_prefix + "_stiffness_frame"),
parent_frame_id_(node_prefix + "_base"),
tf_buffer_(ros::Duration(BUFFER_DURATION_DEFAULT_S)),
tf_listener_(tf_buffer_, subscriber_nodehandle),
last_stiffness_force_(Vector3d::Zero()),
last_stiffness_torque_(Vector3d::Zero()),
last_stiffness_frame_pose_(0)
{
// Strip potential leading slash
if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicInterface with prefix " + node_prefix);
subscriber_cartesian_stiffness_ = subscriber_nodehandle.subscribe(node_prefix_ + "/get/cartesian_stiffness", 1, &RobotDynamicInterface::_callback_cartesian_stiffness, this);
}
void RobotDynamicInterface::_callback_cartesian_stiffness(const geometry_msgs::WrenchStampedConstPtr &msg)
{
last_stiffness_force_(0) = msg->wrench.force.x;
last_stiffness_force_(1) = msg->wrench.force.y;
last_stiffness_force_(2) = msg->wrench.force.z;
last_stiffness_torque_(0) = msg->wrench.torque.x;
last_stiffness_torque_(1) = msg->wrench.torque.y;
last_stiffness_torque_(2) = msg->wrench.torque.z;
try {
const auto transform_stamped = tf_buffer_.lookupTransform( parent_frame_id_, child_frame_id_, ros::Time(0));
last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
} catch (tf2::TransformException &ex) {
ROS_WARN_STREAM(ros::this_node::getName() + "::" + ex.what());
}
}
DQ RobotDynamicInterface::_geometry_msgs_transform_to_dq(const geometry_msgs::Transform& transform)
{
const auto t = DQ(0,transform.translation.x,transform.translation.y,transform.translation.z);
const auto r = DQ(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
return r + 0.5 * E_ * t * r;
}
VectorXd RobotDynamicInterface::get_stiffness_force()
{
if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_force on uninitialized topic");
return last_stiffness_force_;
}
VectorXd RobotDynamicInterface::get_stiffness_torque()
{
if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_torque on uninitialized topic");
return last_stiffness_torque_;
}
DQ RobotDynamicInterface::get_stiffness_frame_pose()
{
if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_frame_pose on uninitialized Interface");
return last_stiffness_frame_pose_;
}
bool RobotDynamicInterface::is_enabled() const
{
if(last_stiffness_frame_pose_==0) return false;
return true;
}

126
src/robot_dynamics/qros_robot_dynamics_provider.cpp
View File

@@ -1,126 +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. Quenitin Lin
#
# ################################################################
*/
#include <robot_dynamic/qros_robot_dynamics_provider.h>
using namespace qros;
RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const std::string &node_prefix):
RobotDynamicProvider(nodehandle, nodehandle, node_prefix)
{
//Delegated
}
RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
node_prefix_(node_prefix),
child_frame_id_(node_prefix + "_stiffness_frame"),
parent_frame_id_(node_prefix + "_base"),
world_to_base_tf_(0)
{
// Strip potential leading slash
if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix);
publisher_cartesian_stiffness_ = publisher_nodehandle.advertise<geometry_msgs::WrenchStamped>(node_prefix + "/get/cartesian_stiffness", 1);
}
geometry_msgs::Transform RobotDynamicProvider::_dq_to_geometry_msgs_transform(const DQ& pose)
{
geometry_msgs::Transform tf_msg;
const auto t = translation(pose);
const auto r = rotation(pose);
tf_msg.translation.x = t.q(1);
tf_msg.translation.y = t.q(2);
tf_msg.translation.z = t.q(3);
tf_msg.rotation.w = r.q(0);
tf_msg.rotation.x = r.q(1);
tf_msg.rotation.y = r.q(2);
tf_msg.rotation.z = r.q(3);
return tf_msg;
}
void RobotDynamicProvider::set_world_to_base_tf(const DQ& world_to_base_tf)
{
if(world_to_base_tf_==0)
{
world_to_base_tf_ = world_to_base_tf;
_publish_base_static_tf();
}else
{
throw std::runtime_error("The world to base transform has already been set");
}
}
void RobotDynamicProvider::_publish_base_static_tf()
{
geometry_msgs::TransformStamped base_tf;
base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_);
base_tf.header.stamp = ros::Time::now();
base_tf.header.frame_id = WORLD_FRAME_ID;
base_tf.child_frame_id = parent_frame_id_;
static_base_tf_broadcaster_.sendTransform(base_tf);
}
void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)
{
std_msgs::Header header;
header.stamp = ros::Time::now();
header.seq = seq_++;
geometry_msgs::WrenchStamped msg;
msg.header = header;
msg.header.frame_id = child_frame_id_;
msg.wrench.force.x = force(0);
msg.wrench.force.y = force(1);
msg.wrench.force.z = force(2);
msg.wrench.torque.x = torque(0);
msg.wrench.torque.y = torque(1);
msg.wrench.torque.z = torque(2);
publisher_cartesian_stiffness_.publish(msg);
if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
{
geometry_msgs::TransformStamped tf_msg;
tf_msg.transform = _dq_to_geometry_msgs_transform(base_to_stiffness);
tf_msg.header = header;
tf_msg.header.frame_id = parent_frame_id_;
tf_msg.child_frame_id = child_frame_id_;
tf_broadcaster_.sendTransform(tf_msg);
}
}
bool RobotDynamicProvider::is_enabled() const
{
return true; //Always enabled
}

34
src/robot_dynamics/qros_robot_dynamic_py.cpp → src/robot_dynamics/qros_robot_dynamics_py.cpp
View File

@@ -32,29 +32,29 @@
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <robot_dynamic/qros_robot_dynamics_provider.h>
#include <robot_dynamic/qros_robot_dynamics_interface.h>
#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp>
#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
namespace py = pybind11;
using RDI = qros::RobotDynamicInterface;
using RDP = qros::RobotDynamicProvider;
using RDC = qros::RobotDynamicsClient;
using RDS = qros::RobotDynamicsServer;
PYBIND11_MODULE(_qros_robot_dynamic, m)
{
py::class_<RDI>(m, "RobotDynamicsInterface")
.def(py::init<const std::string&>())
.def("get_stiffness_force",&RDI::get_stiffness_force)
.def("get_stiffness_torque",&RDI::get_stiffness_torque)
.def("get_stiffness_frame_pose",&RDI::get_stiffness_frame_pose)
.def("is_enabled",&RDI::is_enabled,"Returns true if the RobotDynamicInterface is enabled.")
.def("get_topic_prefix",&RDI::get_topic_prefix);
py::class_<RDC>(m, "RobotDynamicsClient")
.def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())
.def("get_stiffness_force",&RDC::get_stiffness_force)
.def("get_stiffness_torque",&RDC::get_stiffness_torque)
.def("get_stiffness_frame_pose",&RDC::get_stiffness_frame_pose)
.def("is_enabled",&RDC::is_enabled,"Returns true if the RobotDynamicInterface is enabled.")
.def("get_topic_prefix",&RDC::get_topic_prefix);
py::class_<RDP>(m, "RobotDynamicsProvider")
.def(py::init<const std::string&>())
.def("publish_stiffness",&RDP::publish_stiffness)
.def("set_world_to_base_tf", &RDP::set_world_to_base_tf)
.def("is_enabled",&RDP::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
.def("get_topic_prefix",&RDP::get_topic_prefix);
py::class_<RDS>(m, "RobotDynamicsServer")
.def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())
.def("publish_stiffness",&RDS::publish_stiffness)
.def("set_world_to_base_tf", &RDS::set_world_to_base_tf)
.def("is_enabled",&RDS::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
.def("get_topic_prefix",&RDS::get_topic_prefix);
}

116
src/robot_dynamics/qros_robot_dynamics_server.cpp Normal file
View File

@@ -0,0 +1,116 @@
/*
# Copyright (c) 2024 Quenitin Lin
#
# 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: Quenitin Lin
#
# ################################################################
#
# Contributors:
# 1. Quenitin Lin
#
# ################################################################
*/
#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
using namespace qros;
RobotDynamicsServer::RobotDynamicsServer(const std::shared_ptr<Node> &node, const std::string& topic_prefix):
node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix),
child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"),
world_to_base_tf_(0)
{
// Strip potential leading slash
if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
RCLCPP_INFO_STREAM(node_->get_logger(),
"["+ std::string(node_->get_name()) + "]::Initializing RobotDynamicsServer with prefix " + topic_prefix);
publisher_cartesian_stiffness_ = node_->create_publisher<geometry_msgs::msg::WrenchStamped>(topic_prefix + "/get/cartesian_stiffness", 1);
}
geometry_msgs::msg::Transform RobotDynamicsServer::_dq_to_geometry_msgs_transform(const DQ& pose)
{
geometry_msgs::msg::Transform tf_msg;
const auto t = translation(pose);
const auto r = rotation(pose);
tf_msg.translation.x = t.q(1);
tf_msg.translation.y = t.q(2);
tf_msg.translation.z = t.q(3);
tf_msg.rotation.w = r.q(0);
tf_msg.rotation.x = r.q(1);
tf_msg.rotation.y = r.q(2);
tf_msg.rotation.z = r.q(3);
return tf_msg;
}
void RobotDynamicsServer::set_world_to_base_tf(const DQ& world_to_base_tf)
{
if(world_to_base_tf_==0)
{
world_to_base_tf_ = world_to_base_tf;
_publish_base_static_tf();
}else
{
throw std::runtime_error("["+ std::string(node_->get_name()) + "]::[RobotDynamicsServer]::The world to base transform has already been set");
}
}
void RobotDynamicsServer::_publish_base_static_tf()
{
geometry_msgs::msg::TransformStamped base_tf;
base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_);
base_tf.header.stamp = node_->now();
base_tf.header.frame_id = WORLD_FRAME_ID;
base_tf.child_frame_id = parent_frame_id_;
static_base_tf_broadcaster_->sendTransform(base_tf);
}
void RobotDynamicsServer::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)
{
std_msgs::msg::Header header;
header.set__stamp(node_->now());
geometry_msgs::msg::WrenchStamped msg;
msg.header = header;
msg.header.set__frame_id(child_frame_id_);
msg.wrench.force.set__x(force(0));
msg.wrench.force.set__y(force(1));
msg.wrench.force.set__z(force(2));
msg.wrench.torque.set__x(torque(0));
msg.wrench.torque.set__y(torque(1));
msg.wrench.torque.set__z(torque(2));
publisher_cartesian_stiffness_->publish(msg);
if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
{
geometry_msgs::msg::TransformStamped tf_msg;
tf_msg.transform = _dq_to_geometry_msgs_transform(base_to_stiffness);
tf_msg.set__header(header);
tf_msg.header.set__frame_id(parent_frame_id_);
tf_msg.set__child_frame_id(child_frame_id_);
tf_broadcaster_->sendTransform(tf_msg);
}
}
bool RobotDynamicsServer::is_enabled() const
{
return true; //Always enabled
}

65
src/sas_robot_driver_franka_hand_node.cpp
View File

@@ -33,10 +33,9 @@
#include <exception>
#include <dqrobotics/utils/DQ_Math.h>
#include <sas_common/sas_common.h>
#include <sas_conversions/eigen3_std_conversions.h>
#include <sas_robot_driver/sas_robot_driver_ros.h>
#include "hand/qros_effector_driver_franka_hand.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>
/*********************************************
@@ -52,14 +51,14 @@ void sig_int_handler(int)
template<typename T>
void get_optional_parameter(const std::string &node_prefix, ros::NodeHandle &nh, const std::string &param_name, T &param)
void get_optional_parameter(std::shared_ptr<Node> node, const std::string &param_name, T &param)
{
if(nh.hasParam(node_prefix + param_name))
if(node->has_parameter(param_name))
{
sas::get_ros_param(nh,param_name,param);
sas::get_ros_parameter(node,param_name,param);
}else
{
ROS_INFO_STREAM(ros::this_node::getName() + "::Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
RCLCPP_INFO_STREAM(node->get_logger(), "["+std::string(node->get_name())+"]:Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
}
}
@@ -69,55 +68,53 @@ 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_franka_hand_node", ros::init_options::NoSigintHandler);
ROS_WARN("=====================================================================");
ROS_WARN("---------------------------Quentin Lin-------------------------------");
ROS_WARN("=====================================================================");
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
const std::string& effector_driver_provider_prefix = ros::this_node::getName();
rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_hand_node");
const auto node_name = std::string(node->get_name());
RCLCPP_WARN(node->get_logger(),"=====================================================================");
RCLCPP_WARN(node->get_logger(),"---------------------------Quentin Lin-------------------------------");
RCLCPP_WARN(node->get_logger(),"=====================================================================");
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
ros::NodeHandle nh;
qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
get_optional_parameter(effector_driver_provider_prefix,nh,"/thread_sampling_time_nsec",robot_driver_franka_hand_configuration.thread_sampeling_time_ns);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_force",robot_driver_franka_hand_configuration.default_force);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_speed",robot_driver_franka_hand_configuration.default_speed);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_franka_hand_configuration.thread_sampeling_time_s);
sas::get_ros_parameter(node,"default_force",robot_driver_franka_hand_configuration.default_force);
sas::get_ros_parameter(node,"default_speed",robot_driver_franka_hand_configuration.default_speed);
sas::get_ros_parameter(node,"default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
sas::get_ros_parameter(node,"default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
qros::EffectorDriverFrankaHand franka_hand_driver(
effector_driver_provider_prefix,
node_name,
robot_driver_franka_hand_configuration,
nh,
node,
&kill_this_process
);
try
{
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka hand.");
RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Instantiating Franka hand.");
franka_hand_driver.connect();
franka_hand_driver.initialize();
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating control loop.");
RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Starting control loop.");
franka_hand_driver.start_control_loop();
}
catch (const std::exception& e)
{
ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
RCLCPP_ERROR_STREAM(node->get_logger(), "["+node_name+"]::Exception::" + e.what());
}catch (...)
{
ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::Unknown exception.");
RCLCPP_ERROR_STREAM(node->get_logger(), "["+node_name+"]::Exception::Unknown exception.");
}
ROS_INFO_STREAM(ros::this_node::getName()+"::Exiting...");
RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Exiting...");
franka_hand_driver.deinitialize();
ROS_INFO_STREAM(ros::this_node::getName()+"::deinitialized.");
RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Deinitialized.");
franka_hand_driver.disconnect();
ROS_INFO_STREAM(ros::this_node::getName()+"::disconnected.");
RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Disconnected.");
return 0;

86
src/sas_robot_driver_franka_node.cpp
View File

@@ -26,6 +26,8 @@
# 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
# - Adaption to ROS2
#
# ################################################################
*/
@@ -36,11 +38,11 @@
#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_franka.h"
#include <robot_dynamic/qros_robot_dynamics_provider.h>
#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/robot_dynamic/qros_robot_dynamics_server.hpp>
/*********************************************
@@ -80,67 +82,68 @@ 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_franka_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 node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_node");
ros::NodeHandle nh;
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(),"["+node_name+"]::Loading parameters from parameter server.");
sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
RobotInterfaceFranka::FrankaInterfaceConfiguration franka_interface_configuration;
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
sas::get_ros_param(nh,"/robot_mode", robot_driver_franka_configuration.mode);
sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_configuration.ip_address);
sas::get_ros_parameter(node,"robot_mode", robot_driver_franka_configuration.mode);
double upper_scale_factor = 1.0;
std::vector<std::string> all_params;
if(nh.hasParam(ros::this_node::getName()+"/force_upper_limits_scaling_factor"))
if(node->has_parameter("force_upper_limits_scaling_factor"))
{
sas::get_ros_param(nh,"/force_upper_limits_scaling_factor",upper_scale_factor);
ROS_WARN_STREAM(ros::this_node::getName()+"::Set force upper limits scaling factor: " << upper_scale_factor);
sas::get_ros_parameter(node,"force_upper_limits_scaling_factor",upper_scale_factor);
RCLCPP_WARN_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Set force upper limits scaling factor: " << upper_scale_factor);
}
if(nh.hasParam(ros::this_node::getName()+"/upper_torque_threshold")) {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold override and set.");
if(node->has_parameter("upper_torque_threshold")) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
std::vector<double> upper_torque_threshold_std_vec;
sas::get_ros_param(nh,"/upper_torque_threshold",upper_torque_threshold_std_vec);
sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
}else {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold not set. Using default with value scalling.");
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
}
if(nh.hasParam(ros::this_node::getName()+"/upper_force_threshold")) {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold override and set.");
if(node->has_parameter("upper_force_threshold")) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold override and set.");
std::vector<double> upper_torque_threshold_std_vec;
sas::get_ros_param(nh,"/upper_force_threshold",upper_torque_threshold_std_vec);
sas::get_ros_parameter(node,"upper_force_threshold",upper_torque_threshold_std_vec);
franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_torque_threshold_std_vec);
}else {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling.");
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold not set. Using default with value scalling.");
franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
}
if(nh.hasParam(ros::this_node::getName()+"/robot_parameter_file_path"))
if(node->has_parameter("robot_parameter_file_path"))
{
std::string robot_parameter_file_path;
sas::get_ros_param(nh,"/robot_parameter_file_path",robot_parameter_file_path);
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading robot parameters from file: " + robot_parameter_file_path);
sas::get_ros_parameter(node,"robot_parameter_file_path",robot_parameter_file_path);
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading robot parameters from file: " + robot_parameter_file_path);
const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
}else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");}
}else{RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");}
robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
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);
sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_ros_configuration.thread_sampling_time_sec);
sas::get_ros_parameter(node,"q_min",robot_driver_ros_configuration.q_min);
sas::get_ros_parameter(node,"q_max",robot_driver_ros_configuration.q_max);
// initialize the robot dynamic provider
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
qros::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
robot_driver_ros_configuration.robot_driver_provider_prefix = node_name;
qros::RobotDynamicsServer robot_dynamic_provider(node, robot_driver_ros_configuration.robot_driver_provider_prefix);
if(robot_driver_franka_configuration.robot_reference_frame!=0)
{
robot_dynamic_provider.set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
@@ -148,16 +151,17 @@ int main(int argc, char **argv)
try
{
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot.");
sas::RobotDriverFranka robot_driver_franka(
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating Franka robot.");
auto robot_driver_franka = std::make_shared<sas::RobotDriverFranka>(
node,
&robot_dynamic_provider,
robot_driver_franka_configuration,
&kill_this_process
);
//robot_driver_franka.set_joint_limits({qmin, qmax});
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
sas::RobotDriverROS robot_driver_ros(nh,
&robot_driver_franka,
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating RobotDriverROS.");
sas::RobotDriverROS robot_driver_ros(node,
robot_driver_franka,
robot_driver_ros_configuration,
&kill_this_process);
robot_driver_ros.control_loop();
@@ -165,7 +169,7 @@ int main(int argc, char **argv)
catch (const std::exception& e)
{
kill_this_process = true;
ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
}