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

attempt to merge newer changes

Browse Source
This commit is contained in:
qlin1806
2024-08-21 16:35:22 -07:00
parent 0358b851df fe5bb31873
commit 35131ed59e
5 changed files with 87 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
CMakeLists.txt
View File

@@ -244,22 +244,22 @@ install(TARGETS
##### PYBIND11 LIBRARY ROBOT_DYNAMICS #####
ament_python_install_package(${PROJECT_NAME})
pybind11_add_module(_qros_franka_robot_dynamic SHARED
pybind11_add_module(_qros_franka_robot_dynamics_py SHARED
src/robot_dynamics/qros_robot_dynamics_py.cpp
src/robot_dynamics/qros_robot_dynamics_client.cpp
src/robot_dynamics/qros_robot_dynamics_server.cpp
)
target_include_directories(_qros_franka_robot_dynamic
target_include_directories(_qros_franka_robot_dynamics_py
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/robot_dynamics>
$<INSTALL_INTERFACE:include/robot_dynamics>)
target_compile_definitions(_qros_franka_robot_dynamic PRIVATE IS_SAS_PYTHON_BUILD)
target_compile_definitions(_qros_franka_robot_dynamics_py PRIVATE IS_SAS_PYTHON_BUILD)
# https://github.com/pybind/pybind11/issues/387
target_link_libraries(_qros_franka_robot_dynamic PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
target_link_libraries(_qros_franka_robot_dynamics_py PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
install(TARGETS _qros_franka_robot_dynamic
install(TARGETS _qros_franka_robot_dynamics_py
DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
)
##END## PYBIND11 LIBRARY ROBOT_DYNAMICS #####

105
sas_robot_driver_franka/__init__.py
View File

@@ -1,11 +1,14 @@
"""
"""
from sas_robot_driver_franka._qros_franka_robot_dynamic import RobotDynamicsClient, RobotDynamicsServer
from typing import Union
from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamicsClient, RobotDynamicsServer
import rclpy
from rclpy.node import Node
# from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
from sas_robot_driver_franka_interfaces.srv import Move, Move_Request, Move_Response, Grasp, Grasp_Request, Grasp_Response
from rclpy.client import Client
from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
from sas_robot_driver_franka_interfaces.srv import Move, Grasp
from sas_robot_driver_franka_interfaces.msg import GripperState
import os
import threading
@@ -31,9 +34,9 @@ class FrankaGripperInterface:
self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
self._grasping = False
self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
self._status_callback, 5)
self._status_callback, 10)
self.result_queue = Queue()
self.service_future: Union[rclpy.Future, None] = None
# gripper state
self.state_width = None
@@ -45,14 +48,11 @@ class FrankaGripperInterface:
def _default_spin_handler(self):
rclpy.spin_once(self.node)
def set_spin_handler(self, spin_handler):
self.spin_handler = spin_handler
def _is_service_ready(self, service):
def _is_service_ready(self, service: Client):
try:
self.node.get_logger().info("Waiting for service: " + service.service_name)
service.wait_for_service(timeout_sec=0.1)
return True
# self.node.get_logger().info("Waiting for service: " + service.service_name)
ret = service.wait_for_service(timeout_sec=0.1)
return ret
except Exception as e:
self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
return False
@@ -81,7 +81,7 @@ class FrankaGripperInterface:
"""
if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
threading.Thread(target=self._move, args=(width, speed)).start()
self._move(width, speed)
def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
"""
@@ -95,7 +95,7 @@ class FrankaGripperInterface:
"""
if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
threading.Thread(target=self._grasp, args=(width, force, speed, epsilon_inner, epsilon_outer)).start()
self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
def get_max_width(self):
""" Get the maximum width of the gripper """
@@ -131,7 +131,7 @@ class FrankaGripperInterface:
def is_busy(self):
""" Check if the gripper is currently moving or grasping """
return self._moving or self._grasping
return self._moving or self._grasping or self.service_future is not None
def is_done(self):
""" Check if the gripper is done moving or grasping """
@@ -139,7 +139,9 @@ class FrankaGripperInterface:
self.node.get_logger().warn("Gripper is not moving or grasping")
return False
else:
if self.result_queue.empty():
if self.service_future is not None:
if self.service_future.done():
return True
return False
else:
return True
@@ -149,11 +151,19 @@ class FrankaGripperInterface:
Get the result of the last action
:return:
"""
if not self.result_queue.empty():
response = self.result_queue.get()
self._moving = False
self._grasping = False
return response.success
if self.service_future is not None:
if self.service_future.done():
response = self.service_future.result()
if isinstance(response, Move.Response):
self._moving = False
elif isinstance(response, Grasp.Response):
self._grasping = False
else:
raise Exception("Invalid response type")
self.service_future = None
return response.success
else:
raise Exception("No result available")
else:
raise Exception("No result available")
@@ -164,40 +174,33 @@ class FrankaGripperInterface:
"""
if not self.is_enabled():
raise Exception("Gripper is not enabled")
if not self._moving and not self._grasping:
raise Exception("Gripper is not moving or grasping")
if not self._moving or self._grasping:
raise Exception("Gripper is not moving or grasping")
while self._moving or self._grasping:
if not self.is_busy():
return
while not self.is_done():
self.spin_handler()
time.sleep(0.1)
if not self.result_queue.empty():
response = self.result_queue.get()
if isinstance(response, Move_Response):
self._moving = False
elif isinstance(response, Grasp_Response):
self._grasping = False
else:
raise Exception("Invalid response type")
break
return response.success
time.sleep(0.01)
def _move(self, width, speed):
self._moving = True
request = Move_Request()
request.width = width
request.speed = speed
response = self.move_service.call(request)
self.result_queue.put(response)
# self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
request = Move.Request()
request.width = float(width)
request.speed = float(speed)
# self.node.get_logger().info("Calling move service")
self.service_future = self.move_service.call_async(request)
def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
self._grasping = True
request = Grasp_Request()
request.width = width
request.force = force
request.speed = speed
request.epsilon_inner = epsilon_inner
request.epsilon_outer = epsilon_outer
response = self.grasp_service.call(request)
self.result_queue.put(response)
# self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
request = Grasp.Request()
request.width = float(width)
request.force = float(force)
request.speed = float(speed)
request.epsilon_inner = float(epsilon_inner)
request.epsilon_outer = float(epsilon_outer)
# self.node.get_logger().info("Calling grasp service")
self.service_future = self.grasp_service.call_async(request)
def set_spin_handler(self, spin_handler):
self.spin_handler = spin_handler

22
src/hand/qros_effector_driver_franka_hand.cpp
View File

@@ -78,12 +78,11 @@ namespace qros
}
void EffectorDriverFrankaHand::start_control_loop()
{
void EffectorDriverFrankaHand::start_control_loop() {
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
clock.init();
RCLCPP_INFO_STREAM(node_->get_logger(),
"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
RCLCPP_WARN_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::Gripper READY.");
while (!(*break_loops_))
{
if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
@@ -141,7 +140,7 @@ namespace qros
{
throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
}
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
}
void EffectorDriverFrankaHand::initialize()
@@ -151,6 +150,14 @@ namespace qros
gripper_homing();
// start gripper status loop
status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
// check status loop with timeout
auto time_now = std::chrono::system_clock::now();
auto time_out = time_now + std::chrono::seconds(5);
while (!status_loop_running_)
{
if (std::chrono::system_clock::now() > time_out){throw std::runtime_error("[" + std::string(node_->get_name()) + "]::[EffectorDriverFrankaHand]::initialize::Could not start status loop.");}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
void EffectorDriverFrankaHand::deinitialize()
@@ -243,13 +250,12 @@ namespace qros
void EffectorDriverFrankaHand::_gripper_status_loop()
{
status_loop_running_ = true;
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.")
;
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
clock.init();
try
{
status_loop_running_ = true;
while (status_loop_running_)
{
#ifdef BLOCK_READ_IN_USED

2
src/robot_dynamics/qros_robot_dynamics_py.cpp
View File

@@ -40,7 +40,7 @@ using RDC = qros::RobotDynamicsClient;
using RDS = qros::RobotDynamicsServer;
PYBIND11_MODULE(_qros_franka_robot_dynamic, m)
PYBIND11_MODULE(_qros_franka_robot_dynamics_py, m)
{
py::class_<RDC>(m, "RobotDynamicsClient")
.def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())

28
src/sas_robot_driver_franka_node.cpp
View File

@@ -118,35 +118,33 @@ int main(int argc, char **argv)
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"Force upper limits scaling factor is not set.");
}
try{node->declare_parameter<std::vector<double>>("upper_torque_threshold");}catch (...){}
if(node->has_parameter("upper_torque_threshold")) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
try {
std::vector<double> upper_torque_threshold_std_vec;
sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
}else {
}catch(...) {
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);
}
try{node->declare_parameter<std::vector<double>>("upper_force_threshold");}catch (...){}
if(node->has_parameter("upper_force_threshold")) {
try {
std::vector<double> upper_force_threshold_std_vec;
sas::get_ros_parameter(node,"upper_force_threshold",upper_force_threshold_std_vec);
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_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 {
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 = std_vec_to_std_array<double,6>(upper_force_threshold_std_vec);
}catch(...) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque 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);
}
try{node->declare_parameter<std::string>("robot_parameter_file_path");}catch (...){}
if(node->has_parameter("robot_parameter_file_path"))
{
try {
std::string 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{RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");}
}catch(...) {
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;