Grippers not respecting Current_Limit or Goal_Current

[DuaneNielsen]

I've been having trouble with the grippers frequently having overload errors and requiring reboot.

After a lot of reading and debugging, I've come up with this chart.

Here we can see, according to the rospy API, that Current_Limit is set to 300 and Goal_Current to 200. However we can see that this is not being respected by the gripper motor.

I'll attach the code I used to create this graph, but to summarize I'm pulling this data from the rospy service get_motor_registers

'/puppet_left/get_motor_registers', 'single', 'gripper', 'Present_Current', '/puppet_right/get_motor_registers', 'single', 'gripper', 'Goal_Current' '/puppet_left/get_motor_registers', 'single', 'gripper', 'Current_Limit'

I suspect this is a problem with the API and not the motor.

The reason is, if I place an object in the gripper in Dynamixel Wizard. Set "current based position mode", set the Current_Limit to 300 and set the Goal_Current to 300, the motor will not go over 850 mA (300 units) and it wont overload, no matter how far I push the goal position past the point where the gripper wont close.

Unfortunately, we don't have the source code for the Dynamixel Wizard, so I have no idea what it's doing thats special here ;)

Anyway, I'll keep looking into it.

[DuaneNielsen]

Here is the source code I used to create the plot.

to run it just do

conda activate aloha viz_joints.py gripper

import rospy
from sensor_msgs.msg import JointState
from argparse import ArgumentParser
from functools import partial
from collections import deque
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from interbotix_xs_msgs.srv import RegisterValues

class SensorQueue:
    def __init__(self):
        self.pos = deque([0] * 800, maxlen=800)
        self.effort = deque([0] * 800, maxlen=800)
        self.present_current = deque([0] * 800, maxlen=800)
        self.goal_current = deque([0] * 800, maxlen=800)
        self.current_limit = deque([0] * 800, maxlen=800)

qq = {
    'master_right': SensorQueue(),
    'puppet_right': SensorQueue(),
    'master_left': SensorQueue(),
    'puppet_left': SensorQueue(),
}

fig, [
    [left_pos_ax, right_pos_ax],
    [left_effort_ax, right_effort_ax],
    [left_current_ax, right_current_ax]
] = plt.subplots(3, 2, figsize=(24, 12), dpi=80)

###########
# Right arm
###########

# position plot
master_right_plot, = right_pos_ax.plot(qq['master_right'].pos, label='master_position')
puppet_right_plot, = right_pos_ax.plot(qq['puppet_right'].pos, label='puppet_position')
right_pos_ax.set_ylim(-1, 1.5)
right_pos_ax.legend()

# effort plot
puppet_right_effort_plot, = right_effort_ax.plot(qq['puppet_right'].effort, label='effort')
puppet_right_effort_max_plot, = right_effort_ax.plot([max(qq['puppet_right'].effort)]*len(qq['puppet_right'].effort), label='max_effort')
puppet_right_current_plot, = right_current_ax.plot(qq['puppet_right'].present_current, label='present_current')
right_effort_ax.set_ylim(0, 1200)
right_effort_ax.legend()

# current plot
puppet_right_goal_current_plot, = right_current_ax.plot(qq['puppet_right'].goal_current, label='goal_current')
puppet_right_current_limit_plot, = right_current_ax.plot(qq['puppet_right'].current_limit, label='current_limit')
right_current_ax.set_ylim(-1200, 1200)
right_current_ax.legend()

##########
# Left arm
##########

# position plot
master_left_plot, = left_pos_ax.plot(qq['master_left'].pos, label='master_position')
puppet_left_plot, = left_pos_ax.plot(qq['puppet_left'].pos, label='puppet_position')
left_pos_ax.set_ylim(-1, 1.5)
left_pos_ax.legend()

# effort plot
puppet_left_effort_plot, = left_effort_ax.plot(qq['puppet_left'].effort, label='effort')
puppet_left_effort_max_plot, = left_effort_ax.plot([max(qq['puppet_left'].effort)]*len(qq['puppet_left'].effort), label='max_effort')
puppet_left_current_plot, = left_current_ax.plot(qq['puppet_left'].present_current, label='present_current')
left_effort_ax.set_ylim(0, 1200)
left_effort_ax.legend()

# current plot
puppet_left_goal_current_plot, = left_current_ax.plot(qq['puppet_left'].goal_current, label='goal_current')
puppet_left_current_limit_plot, = left_current_ax.plot(qq['puppet_left'].current_limit, label='current_limit')
left_current_ax.set_ylim(-1200, 1200)
left_current_ax.legend()

def find_first_match_index(strings, target):
    for index, string in enumerate(strings):
        if string == target:
            return index
    return -1  # Return -1 if no match is found

global step
step = 0

def joint_states_callback(arm, args, msg):
    global step
    i = find_first_match_index(msg.name, args.joint_name)

    if step % 4 == 0:

        # Access the joint state values
        names = msg.name[i]
        velocities = msg.velocity[i]

        qq[arm].pos.append(msg.position[i])
        qq[arm].effort.append(msg.effort[i])

    step += 1
    # Print the joint state values
    # print("Topic: {}".format(arm))
    # print("Joint Name: {}".format(names))
    # print("Joint Positions: {}".format(pos))
    # # print("Joint Velocities: {}".format(velocities))
    # print("Joint Efforts: {}".format(effort))

def update_plot(frame):
    left_current = service_command('/puppet_left/get_motor_registers', 'single', 'gripper', 'Present_Current')
    right_current = service_command('/puppet_right/get_motor_registers', 'single', 'gripper', 'Present_Current')
    qq['puppet_left'].present_current.append(left_current)
    qq['puppet_right'].present_current.append(right_current)
    puppet_left_current_plot.set_ydata(qq['puppet_left'].present_current)
    puppet_right_current_plot.set_ydata(qq['puppet_right'].present_current)

    left_goal_current = service_command('/puppet_left/get_motor_registers', 'single', 'gripper', 'Goal_Current')
    right_goal_current = service_command('/puppet_right/get_motor_registers', 'single', 'gripper', 'Goal_Current')
    qq['puppet_left'].goal_current.append(left_goal_current)
    qq['puppet_right'].goal_current.append(right_goal_current)
    puppet_left_goal_current_plot.set_ydata(qq['puppet_left'].goal_current)
    puppet_right_goal_current_plot.set_ydata(qq['puppet_right'].goal_current)

    left_goal_current_limit = service_command('/puppet_left/get_motor_registers', 'single', 'gripper', 'Current_Limit')
    right_goal_current_limit = service_command('/puppet_right/get_motor_registers', 'single', 'gripper', 'Current_Limit')
    qq['puppet_left'].current_limit.append(left_goal_current_limit)
    qq['puppet_right'].current_limit.append(right_goal_current_limit)
    puppet_left_current_limit_plot.set_ydata(qq['puppet_left'].current_limit)
    puppet_right_current_limit_plot.set_ydata(qq['puppet_right'].current_limit)

    master_right_plot.set_ydata(qq['master_right'].pos)
    puppet_right_plot.set_ydata(qq['puppet_right'].pos)
    master_left_plot.set_ydata(qq['master_left'].pos)
    puppet_left_plot.set_ydata(qq['puppet_left'].pos)
    puppet_right_effort_plot.set_ydata(qq['puppet_right'].effort)
    puppet_left_effort_plot.set_ydata(qq['puppet_left'].effort)
    puppet_right_effort_max_plot.set_ydata([max(qq['puppet_right'].effort)]*len(qq['puppet_right'].effort))
    puppet_left_effort_max_plot.set_ydata([max(qq['puppet_left'].effort)]*len(qq['puppet_left'].effort))

def service_command(service_path, cmd_type, name, reg, value=0):
    rospy.wait_for_service(service_path)

    try:
        get_registers = rospy.ServiceProxy(service_path, RegisterValues)

        # Make the service call
        response = get_registers(cmd_type, name, reg, value)

        # Process the response
        if response:
            # print("Register values: ", response.values)
            return int(response.values[0])
        else:
            # print("Failed to get register values.")
            return 0

    except rospy.ServiceException as e:
        # print("Service call failed:", str(e))
        return 0

def main(args):

    # Initialize the ROS node
    rospy.init_node('joint_states_subscriber')

    master_right_joint_states_cb_partial = partial(joint_states_callback, 'master_right', args)
    puppet_right_joint_states_cb_partial = partial(joint_states_callback, 'puppet_right', args)
    master_left_joint_states_cb_partial = partial(joint_states_callback, 'master_left', args)
    puppet_left_joint_states_cb_partial = partial(joint_states_callback, 'puppet_left', args)

    # Subscribe to the joint states topic
    rospy.Subscriber('/master_right/joint_states', JointState, master_right_joint_states_cb_partial)
    rospy.Subscriber('/puppet_right/joint_states', JointState, puppet_right_joint_states_cb_partial)
    rospy.Subscriber('/master_left/joint_states', JointState, master_left_joint_states_cb_partial)
    rospy.Subscriber('/puppet_left/joint_states', JointState, puppet_left_joint_states_cb_partial)

    # ani = FuncAnimation(fig, update_plot, fargs=(master_right_plot, puppet_right_plot, master_left_plot, puppet_left_plot, ), interval=100)
    ani = FuncAnimation(fig, update_plot, interval=10)
    plt.show()
    # Spin ROS event loop
    rospy.spin()

if __name__ == '__main__':
    parser = ArgumentParser()
    parser.add_argument('joint_name')
    args = parser.parse_args()
    main(args)

[DuaneNielsen]

Ok, I think I might have found out why this is happening...

https://github.com/tonyzhaozh/aloha/blob/a4e996a7fa5ddf0a5795d6bd61367766bf1205e3/aloha_scripts/record_episodes.py#L108C1-L110C111

I think we need to restore current_based_position control after the mode switch.

    env.puppet_bot_left.dxl.robot_set_operating_modes("single", "gripper", "position")
    env.puppet_bot_right.dxl.robot_set_operating_modes("single", "gripper", "position")
    move_grippers([env.puppet_bot_left, env.puppet_bot_right], [PUPPET_GRIPPER_JOINT_OPEN] * 2, move_time=0.5)
    env.puppet_bot_left.dxl.robot_set_operating_modes("single", "gripper", "current_based_position")
    env.puppet_bot_right.dxl.robot_set_operating_modes("single", "gripper", "current_based_position")

Combine this with editing the ~/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_control/config/vx300s.yaml to add Current_Limit

  gripper:
    ID: 9
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 0
    Max_Position_Limit: 4095
    Secondary_ID: 255
    Current_Limit: 300

and I think we are golden... I'll do some more testing today.

[DuaneNielsen]

After testing, this fix appears to have resolved the problem. I did not test setting the current limit from the python API instead of the vx300s.yaml config yet.

[tonyzhaozh]

Thanks Duane! I just removed the two lines that switch the grippers to "position" mode (see here.) It is not necessary and is part of a legacy stack.

Another thing that could be helpful is to reduce friction between the fingers and the rails. Applying a small amount of lubricants can generally make things better e.g. prevent overheating.

I think editing ~/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/interbotix_xsarm_control/config/vx300s.yaml should not be necessary since the current limit is already set in Dynamixel Wizard (per instructions in README). I tried to run your monitor script with one_side_teleop.py and the current limit seems to be respected. The way I tested it is by closing the gripper around a rigid object.

I am closing this tentatively but please open it if gripper overloading keeps happening. It is definitely not an expected behavior.