Gripper

[anhtuduong]

3 fingers gripper is currently controlled by a ROS service:

ros.Service('move_gripper', generic_float, self.move_gripper_callback)

But it seems to move parallelly with robot joint, which is a bug.

[anhtuduong]

Current gripper function:

def move_gripper(self, diameter):
        rate = ros.Rate(1/self.dt)
        self.controller_manager.gm.move_gripper(diameter)
        target = self.controller_manager.gm.q_des_gripper

        # Loop until the gripper is closed
        while True:
            self.controller_manager.sendReference(self.q_des)
            current = self.controller_manager.gm.getDesGripperJoints()
            if (abs(current - target) < 0.01):
                break
            rate.sleep()

Which trying to loop sending the current joint states with moving 3 finger of the gripper. This causes an error when calling the gripper service in Motion node.

[anhtuduong]

Gripper fixed:

def move_gripper(self, diameter):
        """
        """
        rate = ros.Rate(1/self.dt)
        self.controller_manager.gm.move_gripper(diameter)
        target = self.controller_manager.gm.q_des_gripper
        count = 0

        while True:
            current = self.controller_manager.gm.getDesGripperJoints()
            self.controller_manager.sendReference(self.q_des)
            count += 1
            rate.sleep()
            if np.any(np.abs(target - current) < 0.001):
                break
        
        current = self.controller_manager.gm.getDesGripperJoints()
        log.info(f'Gripper moved: {count} steps')
        log.debug(f'Gripper target\t{target}')
        log.debug(f'Gripper current\t{current}')