tonyzhaozh / aloha

MIT License
1.4k stars 242 forks source link

How to set the current limit on a gripper using python api #6

Closed DuaneNielsen closed 1 year ago

DuaneNielsen commented 1 year ago

Hey Tony,

Not an issue, feel free to close when you see it...

I read a comment in your code that you were trying to figure out how to set the current limit.

I chanced upon the solution, below...

I think you can get or set any register on the dynamixels using these ros services...

#!/usr/bin/env python

import rospy
from interbotix_xs_msgs.srv import RegisterValues

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)
        else:
            print("Failed to get register values.")

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

if __name__ == '__main__':
    rospy.init_node('gripper_register_node')
    service_command('/puppet_left/get_motor_registers', 'single', 'gripper', 'Current_Limit')
    service_command('/puppet_left/set_motor_registers', 'single', 'gripper', 'Current_Limit', 800)
    service_command('/puppet_left/get_motor_registers', 'single', 'gripper', 'Current_Limit')

there is an equivalent ros command also, the form is something like...

rosservice call /puppet_left/get_motor_registers single gripper Hardware_Error_Status 0
lukeschmitt-tr commented 1 year ago

You could also set Current_Limit (along with any other register EEPROM or RAM) on startup using a robot's motor_config file.

For example, if you wanted to set Current_Limit on the ViperX-300s gripper motor, you would modify the vx300s motor config file like so:

  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: 800
DuaneNielsen commented 1 year ago

Here's another way to set a motor register on the gripper, using the Interbotix python API

The torque must be OFF or the command will be ignored.

puppet_bot = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper", robot_name=f'puppet_{robot_side}', init_node=True)
puppet_bot.gripper.core.srv_set_reg('single', 'gripper', 'Current_Limit', 500)
MindFactoryAI commented 1 year ago

Update to this one. Here is how I'm currently setting the grippers, using the interbotix API...

puppet_bot = InterbotixManipulatorXS(robot_model="vx300s", group_name="arm", gripper_name="gripper",robotname=f'puppet{robot_side}', init_node=True)

The order of operations turns out to be important.

First we reboot the motors. Then disable torque so we can set the current limit Then set the current limit Then set current based position mode.

I learned when current based position mode is set, torque will automatically be turned on. So you cant set the operating mode then set the parameters. You have to do the operations in the below order...

The final torque enable command is probably superfluous. However I don't like relying on side effects of API calls, so I left it in.

def reboot_grippers(puppet_bot_left, puppet_bot_right, current_limit=1000):

    for bot in [puppet_bot_left, puppet_bot_right]:
        bot.dxl.robot_reboot_motors("single", "gripper", True)
        bot.dxl.robot_torque_enable("single", "gripper", False)
        bot.dxl.robot_set_motor_registers("single", "gripper", 'Current_Limit', current_limit)
        bot.dxl.robot_set_operating_modes("single", "gripper", "current_based_position")
        bot.dxl.robot_torque_enable("single", "gripper", True)
MindFactoryAI commented 1 year ago

Also, I did some testing of the gripper motors, and as you mention in your instructions, setting current_limit to higher values than 300 causes overloading, even in current_based_position mode. Whereas if it's set to 300, I observe no overloading. Anyone reading this thread can verify this by placing an object in the gripper, then pushing the position past the closing point using the dynamixel wizard with various current_limits.