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