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

updated minor optimization and bug fix for python gripper interface

Browse Source
This commit is contained in:
qlin960618
2025-07-05 18:02:13 +09:00
parent a54ddb14ea
commit 89e25af2d2
1 changed files with 28 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
sas_robot_driver_franka/__init__.py
View File

@@ -7,7 +7,7 @@ from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamics
import rclpy
from rclpy.node import Node
from rclpy.client import Client
from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
# from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
from sas_robot_driver_franka_interfaces.srv import Move, Grasp
from std_srvs.srv import Trigger
from sas_robot_driver_franka_interfaces.msg import GripperState
@@ -31,12 +31,10 @@ class FrankaGripperInterface:
self.last_message = None
self.robot_prefix = robot_prefix
self.node = node
self._current_action = None # between moving, grasping, and homing
self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX))
self._moving = False
self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
self._grasping = False
self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing"))
self._homing = False
self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
self._status_callback, 10)
@@ -50,7 +48,7 @@ class FrankaGripperInterface:
self.spin_handler = self._default_spin_handler
def _default_spin_handler(self):
rclpy.spin_once(self.node)
rclpy.spin_once(self.node, timeout_sec=0.01)
def _is_service_ready(self, service: Client):
try:
@@ -75,6 +73,7 @@ class FrankaGripperInterface:
self.state_max_width = msg.max_width
self.state_temperature = msg.temperature
self.state_is_grasped = msg.is_grasped
def home(self):
"""
Reinitialize the gripper
@@ -93,7 +92,7 @@ class FrankaGripperInterface:
"""
if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
self._move(width, speed)
return self._move(width, speed)
def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
"""
@@ -107,7 +106,7 @@ class FrankaGripperInterface:
"""
if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
return self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
def get_max_width(self):
""" Get the maximum width of the gripper """
@@ -135,21 +134,25 @@ class FrankaGripperInterface:
def is_moving(self):
""" Check if the gripper is currently moving """
return self._moving
return self._current_action == "moving"
def is_grasping(self):
""" Check if the gripper is currently grasping """
return self._grasping
return self._current_action == "grasping"
def is_homing(self):
""" Check if the gripper is currently homing """
return self._current_action == "homing"
def is_busy(self):
""" Check if the gripper is currently moving or grasping """
return self._moving or self._grasping or self.service_future is not None
return self._current_action is not None
def is_done(self):
""" Check if the gripper is done moving or grasping """
if not self.is_busy():
self.node.get_logger().warn("Gripper is not moving or grasping")
return False
return True
else:
if self.service_future is not None:
if self.service_future.done():
@@ -167,11 +170,11 @@ class FrankaGripperInterface:
if self.service_future.done():
response = self.service_future.result()
if isinstance(response, Move.Response):
self._moving = False
self._current_action = None
elif isinstance(response, Grasp.Response):
self._grasping = False
self._current_action = None
elif isinstance(response, Trigger.Response):
self._homing = False
self._current_action = None
else:
raise Exception("Invalid response type")
self.service_future = None
@@ -181,6 +184,7 @@ class FrankaGripperInterface:
raise Exception("No result available")
else:
raise Exception("No result available")
def get_last_message(self):
return self.last_message
@@ -189,31 +193,36 @@ class FrankaGripperInterface:
Wait until the gripper is done moving or grasping
:return: success
"""
rate = self.node.create_rate(100) # 100 Hz
if not self.is_enabled():
raise Exception("Gripper is not enabled")
if not self.is_busy():
return
while not self.is_done():
self.spin_handler()
time.sleep(0.01)
rate.sleep()
return self.get_result()
def _home(self):
self._homing = True
self._current_action = "homing"
# self.node.get_logger().info("Homing gripper")
request = Trigger.Request()
# self.node.get_logger().info("Calling home service")
self.service_future = self.home_service.call_async(request)
return self.service_future
def _move(self, width, speed):
self._moving = True
self._current_action = "moving"
# 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)
return self.service_future
def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
self._grasping = True
self._current_action = "grasping"
# self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
request = Grasp.Request()
request.width = float(width)
@@ -223,7 +232,7 @@ class FrankaGripperInterface:
request.epsilon_outer = float(epsilon_outer)
# self.node.get_logger().info("Calling grasp service")
self.service_future = self.grasp_service.call_async(request)
return self.service_future
def set_spin_handler(self, spin_handler):
self.spin_handler = spin_handler