Position control of robotiq_2f_85 gripper

[autonomousTurtle]

Summary

I am using ROS1 Noetic with a kinova gen3 arm and a robotiq 2f_85 gripper.

My objective is to publish angles in near real time to the robotiq gripper and have it move smoothly to those positions. The purpose is to slowly close the gripper until a peak current is reported and then stop closing, thus simulating a force feedback of the gripper which I understand is not implemented in the kortex API.

An example script of what I am working with is below. The result of this script is a slightly jerky motion from point to point, and if I increase the update rate above 15hz or wait more than 1 minute, my gripper light flashes blue and purple and locks up, requiring a full reboot of the robot to reset it.

#!/usr/bin/env python3

import rospy
import time
from kortex_driver.srv import SendGripperCommand
from kortex_driver.msg import GripperCommand, GripperMode, GripperRequest, GripperMode, Gripper, Finger
from sensor_msgs.msg import Joy, JointState

class RobotiqGripperController:

    def __init__(self):
        rospy.init_node('robotiq_gripper_controller')
        rospy.loginfo("Starting Robotiq Gripper Controller node")

        self.gripper_command_service = rospy.ServiceProxy('/my_gen3/base/send_gripper_command', SendGripperCommand)
        self.gripper_state_subscriber = rospy.Subscriber('/my_gen3/joint_states', JointState, self.gripper_state_callback)

        self.gripper_state = None
        self.gripper_open = 2.3
        self.gripper_closed = 3.7
        self.prev_gripper_pos = 0
        self.update_rate = 20

    def gripper_state_callback(self, msg):
        self.gripper_state = msg

    def control_loop(self):
        rate = rospy.Rate(self.update_rate)  # 10 Hz control loop

        while not rospy.is_shutdown():
            if self.gripper_state is not None:
                # Control gripper based on the received gripper state
                target_position = self.gripper_state.position[6]
                speed = self.gripper_state.velocity[6]
                effort = self.gripper_state.effort[6]

                max_velocity = 0.03
                gain = 1.0

                rospy.loginfo("Gripper State: position: {}, speed: {}, effort: {}".format(target_position, speed, effort))

                for i in range(100):

                    # proprotional control
                    position_error = i - self.prev_gripper_pos
                    velocity_command = gain * position_error

                    # Clamp the vel command
                    velocity_command = max(min(velocity_command, max_velocity), -max_velocity)

                    target_position = self.prev_gripper_pos + velocity_command

                    gripper_command = GripperCommand()
                    finger = Finger()
                    finger.finger_identifier = 0
                    finger.value = i/100
                    gripper_command.gripper.finger.append(finger)
                    gripper_command.mode = GripperMode.GRIPPER_POSITION
                    self.prev_gripper_pos = target_position

                try:
                    self.gripper_command_service(gripper_command)
                except rospy.ServiceException as e:
                    rospy.logerr("Service call failed: {}".format(e))

            rate.sleep()

if __name__ == '__main__':
    try:
        gripper_controller = RobotiqGripperController()
        gripper_controller.control_loop()
    except rospy.ROSInterruptException:
        pass

Any suggestions on how to achieve this control without introducing a blocking sleep function that is in the kortex_examples folder?

Thank you!

[felixmaisonneuve]

Hi @autonomousTurtle,

The kortexAPI and the ROS driver are running at 40Hz and I wouldn't recommand changing them. It is possible to use Low Level servoing mode on an arm to get a 1000Hz communication frequency, but it is not possible to use in ROS. In Low Level, it is possible to do something like you mention and look for current spike in the gripper.

Unfortunately, I do not think it is possible for you to use ROS at a rate different than 40Hz.

Best, Felix

[autonomousTurtle]

Thanks, Felix - I agree, and I can make 40Hz work, but I seem to continue to brick the gripper using commands even sent at 1Hz. Do you have a different recommendation for how to use the API to move the gripper or is this the correct approach?

For example, once the gripper stops working, I am no longer able to even move it from the Kinova Web App or through the MoveIt Planning interface inside of the demo unless I power the robot and gripper completely off and back on.

Thanks, Aaron

[felixmaisonneuve]

If you are using the API directly (without ROS), you could switch to Low Level servoing. This would allow you to communicate with the gripper at a 1000Hz communication rate.

It is weird that the arm must be rebooted to use the gripper again, but it is possible something breaks entirely in the arm if you change the communication rate.