Problem with gripper movement

[OXOSMDF]

Hello, I try to use franky to manipulate franka panda and collect data from it. I wrote this ROS node. I separate gripper control, robot movement control and data collection into 3 threads. I found problem when I use gripper.open_async and gripper.grasp_async as I would block other 2 threads and I could only use other 2 threads after open gripper and grasp gripper is done.
`#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from franky import *
import franky
import threading
from std_msgs.msg import Int16, Float32
from teng_arduino.msg import TwistGripper
import pyrealsense2 as rs
import numpy as np
import pickle
import copy
import traceback
import os
from scipy.spatial.transform import Rotation
import open3d as o3d

def visualize_point_cloud(points):
"""
Visualize the point cloud using Open3D.
:param points: The Nx3 numpy array of point cloud coordinates.
"""
# Convert points to Open3D point cloud
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

# Visualize the point cloud
o3d.visualization.draw_geometries([pcd], window_name="Point Cloud Viewer",
                                  width=640, height=480)

def quaternion_to_euler(q):

q_x, q_y, q_z, q_w = q[0], q[1], q[2], q[3]
roll = np.arctan2(2.0 * (q_w * q_x + q_y * q_z), 1.0 - 2.0 * (q_x**2 + q_y**2))
pitch = np.arcsin(2.0 * (q_w * q_y - q_z * q_x))
yaw = np.arctan2(2.0 * (q_w * q_z + q_x * q_y), 1.0 - 2.0 * (q_y**2 + q_z**2))

return roll, pitch, yaw

class FrankaRobotController:
def init(self):
rospy.init_node('franka_robot_controller')
robot_ip = rospy.get_param('~robot_ip')
self.robot = Robot(robot_ip)
self.robot.recover_from_errors()
self.robot.relative_dynamics_factor = 0.05
self.robot.velocity_rel = 0.4
self.robot.acceleration_rel = 0.2
self.robot.jerk_rel = 0.02
self.gripper = franky.Gripper(robot_ip)
self.gripper_speed = 0.02
self.gripper_force = 5
print('Connecting to robot at {}'.format(robot_ip))

    target_positions = [0.2, 0, -0.28, -2.63, -0.03, 2.61, 0.75]
    joint_state_target = JointState(position=target_positions)
    waypoint = JointWaypoint(target=joint_state_target, reference_type=ReferenceType.Absolute)
    init_motion = JointWaypointMotion([waypoint])
    self.robot.move(init_motion)
    
    # self.twist_sub = rospy.Subscriber('/franka_twist', Twist, self.twist_callback)
    self.gripper_sub = rospy.Subscriber('/franka_gripper_command', Int16, self.gripper_open_callback, queue_size=1)
    self.twist_gripper_sub = rospy.Subscriber('/franka_twist_gripper', TwistGripper, self.twist_gripper_callback, queue_size=1)        
    self.translation = [0, 0, 0]
    self.angular_y = 0.0
    self.movement = [0, 0, 0, 0]
    self.new_command_received = False
    self.new_grippr_command_received = False
    self.gripper_cnt = 0
    self.gripper_command = 0
    self.max_gripper_width = 0.08
    
    self.pipeline = rs.pipeline()
    config = rs.config()
    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    self.pipeline.start(config)
    self.workspace = [(-0.14, 0.17),
                      (-0.37, 0.28),
                      (0.61, 1)]
    
    self.data_cache = {
        'point_cloud': [],
        'agent_pose': [],
        'action': []
    }
    
    self.gripper_move = -0.1
    
    self.lock = threading.Lock()
    
    self.control_thread = threading.Thread(target=self.robot_control_loop)
    self.control_thread.start()
    
    self.record_thread = threading.Thread(target=self.robot_record_loop)
    self.record_thread.start()
    
    self.gripper_thread = threading.Thread(target=self.gripper_control_loop)
    self.gripper_thread.start()
    
def twist_gripper_callback(self, msg):
    with self.lock:
        self.movement = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z, msg.twist.angular.y]
        self.gripper_command = msg.gripper
        self.new_command_received = True

def gripper_open_callback(self, msg):
    with self.lock:
        if msg.data == 1:
            self.gripper_move = 0.1
            self.new_grippr_command_received = True
        elif msg.data == 2:
            self.gripper_move = 0.03
            self.new_grippr_command_received = True
        elif msg.data == 3:
            self.gripper_move = -0.1
            self.new_grippr_command_received = False

def save_data(self, directory = '/home/tumi6/yxt/pkl_data/panda_pick_place'):
    path = os.path.join(directory, 'traj20.pkl')
    with open(path, 'wb') as f:
        pickle.dump(self.data_cache, f)

def robot_control_loop(self):
    rate = rospy.Rate(2) 
    while not rospy.is_shutdown():
        with self.lock:
            if self.new_command_received:
                movement = self.movement
                self.new_command_received = False
            else:
                movement = [0, 0, 0, 0]
                        
        if any(movement):
            translation = np.array([self.movement[0], self.movement[1], self.movement[2]])
            quat = Rotation.from_euler("xyz", [0, self.movement[3], 0]).as_quat()
            way = Affine(translation, quat)
            motion_forward = CartesianMotion(way, ReferenceType.Relative)
            self.robot.move(motion_forward, asynchronous=True)
            
        rate.sleep()
        
def robot_record_loop(self):
    rate = rospy.Rate(2)
    while not rospy.is_shutdown():
        frames = self.pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
    
        cartesian_state = self.robot.current_cartesian_state
        pose = cartesian_state.pose
        #self.gripper_width = self.gripper.width

        x, y, z = pose.end_effector_pose.translation.flatten()
        q = pose.end_effector_pose.quaternion.flatten()
        roll, pitch, yaw = quaternion_to_euler(q)
        pose_arr = np.array([x, y, z, roll, pitch, yaw], dtype=np.float32)
        #print(pose_arr)
        pc = rs.pointcloud()
        points = pc.calculate(depth_frame)
        points = np.asanyarray(points.get_vertices()).view(np.float32).reshape(-1, 3)
        points = points[np.where((points[..., 0] > self.workspace[0][0]) & (points[..., 0] < self.workspace[0][1]) &
                             (points[..., 1] > self.workspace[1][0]) & (points[..., 1] < self.workspace[1][1]) &
                             (points[..., 2] > self.workspace[2][0]) & (points[..., 2] < self.workspace[2][1]))]
            
        action = np.array([self.movement[0], self.movement[1], self.movement[2], 0, self.movement[3], 0, self.gripper_move], dtype=Float32)
        print(action)
        with self.lock:    
            self.data_cache['point_cloud'].append(copy.deepcopy(points))
            self.data_cache['action'].append(copy.deepcopy(action))
            self.data_cache['agent_pose'].append(copy.deepcopy(pose_arr)) 
        
        rate.sleep()
        
def gripper_control_loop (self):
    rate = rospy.Rate(2)
    while not rospy.is_shutdown():    
        with self.lock:
            if self.new_grippr_command_received:
                current_gripper_move = self.gripper_move
                self.new_grippr_command_received = False 
            else:
                current_gripper_move = -0.1
                
        if current_gripper_move == 0.1:
            self.gripper.open_async(self.gripper_speed)
        elif current_gripper_move == 0.03:
            self.gripper.grasp_async(0.03, self.gripper_speed, self.gripper_force, epsilon_outer=0.2
            
    rate.sleep()

def log_current_thread(self):
    current_thread = threading.current_thread().name
    rospy.loginfo(f'Running thread: {current_thread}')

def run(self):
    try:
        #translation = np.array([[0.0], [0.1], [0.0]])
        #quaternion = np.array([[0.0], [0.0], [0.0], [1.0]])
        #way = Affine(translation=translation, quaternion=quaternion)
        #motion_forward = CartesianMotion(way, ReferenceType.Relative)
        #self.robot.move(motion_forward, asynchronous=True)
        #motion_backward = CartesianMotion(way.inverse, ReferenceType.Relative)
        #self.robot.move(motion_backward, asynchronous=True)
        self.gripper.move_async(self.gripper.max_width/2, self.gripper_speed)
        print('Gripper opened: {}'.format(self.gripper.max_width))
        rospy.spin()
    finally:
        self.save_data()

if name == 'main':
controller = FrankaRobotController()
controller.run()`

[TimSchneider42]

Hi,
I have observed this behavior, too. Unfortunately, this seems to be a limitation of libfranka, that a gripper movement cannot be stopped once commanded, as all other gripper commands seem to wait for the current gripper command to terminate. I have no solution to this problem except to use a different gripper.
Best,
Tim

[Xiaoyao-Li]

Hi,
Thanks for the awesome franky, which allows me to easily control my Franka🚀
I also found that asynchronous control of the gripper and the arm within the same process can cause conflicts, even when using multithreading.
However, I discovered that this issue can be resolved when the gripper and the arm are controled in separate processes. When I run two Python scripts simultaneously—one for controlling the arm and the other for the gripper—the low-frequency Franka gripper movements no longer block the high-frequency Franka arm control.

Best,
Puhao

[yaoxt3]

Hi,

frankly thanks for your project. In the end, I figure out some way to control the robot arm and gripper at the same time in multithreading.
`def gripper_control_loop(self):
rate = rospy.Rate(2)
while not rospy.is_shutdown():

        if self.new_gripper_command_received:
            if current_gripper_move:
                current_future = self.gripper.open_async(self.gripper_speed)
            elif not current_gripper_move:
                current_future = self.gripper.grasp_async(0.03, self.gripper_speed, self.gripper_force, epsilon_outer=0.2)
                
        rate.sleep()`

I don't no why but it actually works. Thanks again for your contribution.

Best regards,
Yirui

[TimSchneider42]

I just realized that I forgot to release the GIL when the gripper is controlled. This explains why the functions are blocking in threads but not in processes. 0ef80ab fixes this issue. I will push a new version (v0.11.1) with the fix included.

Can you test if this solves the issue?

[Xiaoyao-Li]

Hello @TimSchneider42 , Thank you for your quick and helpful reply, as well as your support!

It seems that the threading issue still persists in the latest version (v0.11.1). Following is the text code example:

import time
import numpy as np
import threading
from franky import Robot, Gripper, CartesianMotion, Affine

# Initialize robot and gripper
robot = Robot("172.16.3.1")
robot.relative_dynamics_factor = 0.05
gripper = Gripper("172.16.3.1")
GRIPPER_SPEED = 0.1
GRIPPER_FORCE = 20

# Initial pose
init_tcp_pos = np.array([0.36131644, -0.02966702, 0.26901625])
init_tcp_quat = np.array([1.0, 0., 0., 0.])  # Initial quaternion

# Reciprocal motion range
pose_range_min = np.array([0.36131644 - 0.1, -0.02966702 - 0.1, 0.26901625 - 0.1])
pose_range_max = np.array([0.36131644, -0.02966702, 0.26901625])

# Gripper reciprocal motion control
def gripper_control():
    """Control gripper open/close"""
    while True:
        current_time = time.time()
        # Gripper reciprocal motion: open and close every 2 seconds
        if int(current_time) % 4 < 2:  # 2s cycle for reciprocation
            gripper_target_width = 0.02
        else:
            gripper_target_width = 0.08
        gripper.move_async(gripper_target_width, GRIPPER_SPEED)

        # Print gripper state
        gripper_width = gripper.width
        print(f"[{time.time()}] Gripper Width: {gripper_width}")
        
        time.sleep(0.1)

# Robot reciprocal motion control
def robot_control():
    """Control robot reciprocal motion"""
    while True:
        current_time = time.time()

        # Control robot motion based on time
        t = (current_time % 10) / 5  # 5 seconds for a reciprocal cycle
        if t < 1:
            target_pos = pose_range_min + t * (pose_range_max - pose_range_min)
        else:
            target_pos = pose_range_max - (t - 1) * (pose_range_max - pose_range_min)

        target_tcp_motion = CartesianMotion(Affine(target_pos, init_tcp_quat))
        robot.move(target_tcp_motion, asynchronous=True)

        # Print robot state
        robot_state = robot.current_cartesian_state.pose.end_effector_pose.__getstate__()
        print(f"[{time.time()}] Arm State: Position: {robot_state[0]}, Quaternion: {robot_state[1]}")

        time.sleep(0.05) 

if __name__ == "__main__":
    init_motion = CartesianMotion(Affine(init_tcp_pos, init_tcp_quat))
    robot.move(init_motion, asynchronous=False)
    # Start gripper control thread
    gripper_thread = threading.Thread(target=gripper_control, daemon=True)
    gripper_thread.start()

    # Start robot motion control thread
    robot_thread = threading.Thread(target=robot_control, daemon=True)
    robot_thread.start()

    # Keep the main thread running
    while True:
        time.sleep(1)

The log shows that the arm control thread and the print current state thread are getting blocked when the gripper is moving.

Everything works fine when I comment out the gripper control command gripper.move_async(gripper_target_width, GRIPPER_SPEED). Additionally, I also tried the solution provided by @yaoxt3 , but the blocking issue still occurs when continuously controlling the Franka Panda gripper.

[TimSchneider42]

Just to confirm, with multiprocessing you do not have this issue?

[TimSchneider42]

@Xiaoyao-Li, I just realized something. It is possible that the future object returned by move_async gets destroyed and that is why everything is hanging. As a workaround, could you try to retain all of them? E.g.

# Gripper reciprocal motion control
def gripper_control():
    """Control gripper open/close"""
    futures = []
    while True:
        current_time = time.time()
        # Gripper reciprocal motion: open and close every 2 seconds
        if int(current_time) % 4 < 2:  # 2s cycle for reciprocation
            gripper_target_width = 0.02
        else:
            gripper_target_width = 0.08
        futures.append(gripper.move_async(gripper_target_width, GRIPPER_SPEED))

        # Print gripper state
        gripper_width = gripper.width
        print(f"[{time.time()}] Gripper Width: {gripper_width}")
        
        time.sleep(0.1)

It is not a permanent solution but I would be curious if it solves the problem. I will try to think of a better solution. The problem is that we do not really use the gripper ourselves because we use different grippers, so I never really tested it.

Best,
Tim

[TimSchneider42]

@Xiaoyao-Li, I implemented a solution for the future issue in version 0.11.2. Could you give it a go and let me know if it works?

[Xiaoyao-Li]

Sorry for the late test and reply. I think your assumption is right. I tested the provided solution under v0.11.1 without destroying the returned future object, and the issue was resolved. I also successfully tested it on v0.11.2, where the thread-blocking issue was reliably fixed, and calling gripper.move_async no longer requires keeping the returned future.

Really appreciate your work! If you need any further testing with Franka Panda gripper, feel free to assign it to me. 🤖🚀

Best,
Puhao

[TimSchneider42]

Thanks for testing and letting me know @Xiaoyao-Li! Also, I appreciate the offer. Might come back to it later on :)