Moblie-Aloha家务机器人-15.机械臂API分析1.夹持器核心文件gripper.py

import rospy
from interbotix_xs_msgs.msg import JointSingleCommand
from interbotix_xs_modules.core import InterbotixRobotXSCore

### @brief Standalone Module to control an Interbotix Gripper using PWM or Current control
###          使用 PWM 或电流控制来控制 Interbotix 夹具的独立模块
### @param robot_model - Interbotix Arm model (ex. 'wx200' or 'vx300s')
### @param gripper_name - name of the gripper joint as defined in the 'motor_config' yaml file; typically, this is 'gripper'
### @param robot_name - defaults to value given to 'robot_model'; this can be customized to best suit the user's needs
### @param gripper_pressure - fraction from 0 - 1 where '0' means the gripper operates at 'gripper_pressure_lower_limit' and '1' means the gripper operates at 'gripper_pressure_upper_limit'
###            0 - 1 之间的分数，其中“0”表示夹具在“gripper_Pressure_lower_limit”下运行，“1”表示夹具在“gripper_Pressure_upper_limit”下运行
### @param gripper_pressure_lower_limit - lowest 'effort' that should be applied to the gripper if gripper_pressure is set to 0; it should be high enough to open/close the gripper (~150 PWM or ~400 mA current)
### @param gripper_pressure_upper_limit - largest 'effort' that should be applied to the gripper if gripper_pressure is set to 1; it should be low enough that the motor doesn't 'overload' when gripping an object for a few seconds (~350 PWM or ~900 mA)
###          抓手压力下限-如果抓手压力设置为0，则应施加到抓手的最低“努力”； 它应该足够高以打开/关闭夹具（~150 PWM 或~400 mA 电流）
###          抓手压力上限-如果抓手压力设置为1，则应施加到抓手的最大“作用力”； 它应该足够低，以便电机在抓住物体几秒钟时不会“过载”（~350 PWM 或~900 mA）
### @param init_node - set to True if the InterbotixRobotXSCore class should initialize the ROS node - this is the most Pythonic approach; to incorporate a robot into an existing ROS node though, set to False
### @details - note that this module doesn't really have any use case except in controlling just the gripper joint on an Interbotix Arm.
###               请注意，除了仅控制 Interbotix 手臂上的夹具关节外，该模块实际上没有任何用例。
class InterbotixGripperXS(object):
    def __init__(self, robot_model, gripper_name, robot_name=None, gripper_pressure=0.5, gripper_pressure_lower_limit=150, gripper_pressure_upper_limit=350, init_node=True):
        self.dxl = InterbotixRobotXSCore(robot_model, robot_name, init_node)
        self.gripper = InterbotixGripperXSInterface(self.dxl, gripper_name, gripper_pressure, gripper_pressure_lower_limit, gripper_pressure_upper_limit)

### @brief Definition of the Interbotix Gripper Module
### @param core - reference to the InterbotixRobotXSCore class containing the internal ROS plumbing that drives the Python API
### @param gripper_name - name of the gripper joint as defined in the 'motor_config' yaml file; typically, this is 'gripper'
### @param gripper_pressure - fraction from 0 - 1 where '0' means the gripper operates at 'gripper_pressure_lower_limit' and '1' means the gripper operates at 'gripper_pressure_upper_limit'
### @param gripper_pressure_lower_limit - lowest 'effort' that should be applied to the gripper if gripper_pressure is set to 0; it should be high enough to open/close the gripper (~150 PWM or ~400 mA current)
### @param gripper_pressure_upper_limit - largest 'effort' that should be applied to the gripper if gripper_pressure is set to 1; it should be low enough that the motor doesn't 'overload' when gripping an object for a few seconds (~350 PWM or ~900 mA)
class InterbotixGripperXSInterface(object):

    def __init__(self, core, gripper_name, gripper_pressure=0.5, gripper_pressure_lower_limit=150, gripper_pressure_upper_limit=350):
        self.core = core
        gripper_info = self.core.srv_get_info("single", gripper_name)
        if (gripper_info.mode != "current" and gripper_info.mode != "pwm"):
            rospy.logerr("Please set the gripper's 'operating mode' to 'pwm' or 'current'.")
        self.gripper_moving = False
        self.gripper_command = JointSingleCommand(name="gripper")
        self.gripper_pressure_lower_limit = gripper_pressure_lower_limit
        self.gripper_pressure_upper_limit = gripper_pressure_upper_limit
        self.gripper_value = gripper_pressure_lower_limit + (gripper_pressure * (gripper_pressure_upper_limit - gripper_pressure_lower_limit))
        self.left_finger_index = self.core.js_index_map[gripper_info.joint_names[0]]
        self.left_finger_lower_limit = gripper_info.joint_lower_limits[0]
        self.left_finger_upper_limit = gripper_info.joint_upper_limits[0]
        tmr_gripper_state = rospy.Timer(rospy.Duration(0.02), self.gripper_state)
        print("Gripper Name: %s\nGripper Pressure: %d%%" % (gripper_name, gripper_pressure * 100))
        print("Initialized InterbotixGripperXSInterface!\n")

    ### @brief ROS Timer Callback function to stop the gripper moving past its limits when in PWM mode
    ###          ROS 计时器回调功能可在 PWM 模式下阻止夹具移动超过其极限
    ### @param event [unused] - Timer event message
    def gripper_state(self, event):
        if (self.gripper_moving):
            with self.core.js_mutex:
                gripper_pos = self.core.joint_states.position[self.left_finger_index]
            if ((self.gripper_command.cmd > 0 and gripper_pos >= self.left_finger_upper_limit) or
                (self.gripper_command.cmd < 0 and gripper_pos <= self.left_finger_lower_limit)):
                self.gripper_command.cmd = 0
                self.core.pub_single.publish(self.gripper_command)
                self.gripper_moving = False

    ### @brief Helper function used to publish effort commands to the gripper (when in 'pwm' or 'current' mode)
    ### @param effort - effort command to send to the gripper motor
    ### @param delay - number of seconds to wait before returning control to the user
    def gripper_controller(self, effort, delay):
        self.gripper_command.cmd = effort
        with self.core.js_mutex:
            gripper_pos = self.core.joint_states.position[self.left_finger_index]
        if ((self.gripper_command.cmd > 0 and gripper_pos < self.left_finger_upper_limit) or
            (self.gripper_command.cmd < 0 and gripper_pos > self.left_finger_lower_limit)):
            self.core.pub_single.publish(self.gripper_command)
            self.gripper_moving = True
            rospy.sleep(delay)

    ### @brief Set the amount of pressure that the gripper should use when grasping an object (when in 'effort' control mode)
    ### @param pressure - a scaling factor from 0 to 1 where the pressure increases as the factor increases
    def set_pressure(self, pressure):
        self.gripper_value = self.gripper_pressure_lower_limit + pressure * \
        (self.gripper_pressure_upper_limit - self.gripper_pressure_lower_limit)

    ### @brief Opens the gripper (when in 'pwm' control mode)
    ### @param delay - number of seconds to delay before returning control to the user
    def open(self, delay=1.0):
        self.gripper_controller(self.gripper_value, delay)

    ### @brief Closes the gripper (when in 'pwm' control mode)
    ### @param delay - number of seconds to delay before returning control to the user
    def close(self, delay=1.0):
        self.gripper_controller(-self.gripper_value, delay)