Hi, I'm trying to control the arm of a Kuka YouBot. I have a problem with the gripper, i'm stuck with an error when i publish desiredGripPos.

The error is the following: File "/home/alessio/kukaws/src/teleopkuka/src/arm_teleop.py", line 139, in processJoints self.gripPub.publish(desiredGripPos)

Instead i have no problem with the others joints. The python code works well because i can see the changing in the values but it does not publish this values to the gripper.

Ubuntu 14 + ROS Indigo

Here the code:

import rospy
from sensor_msgs.msg import JointState, Joy
from brics_actuator.msg import JointPositions, JointValue, JointVelocities
import sys
import copy
import numpy

class arm_teleop:

 def __init__(self):
    # rospy.init_node('arm_teleop')
    rospy.Subscriber("joint_states", JointState, self.processJoints)
    rospy.Subscriber('/joy', Joy, self.processJoystick)
    self.armPub = rospy.Publisher('/arm_controller/position_command', JointPositions, queue_size=10)
    self.velPub = rospy.Publisher('/arm_controller/velocity_command', JointVelocities, queue_size=10)
    self.gripPub = rospy.Publisher('/gripper_controller/position_command', JointPositions, queue_size=10)

    self.joyMsg = None

    self.crntJointStates = JointState()
    self.crntJointPositions = JointPositions()
    self.gripperPositions = JointPositions()
    self.angularStep = 0.07
    self.maxVel = 1.0
    self.gripStep = 0.001
    self.previous_vel_change = False

    self.jointBounds = [
        {'upper': 5.80, 'lower': 0.011},
        {'upper': 2.60, 'lower': 0.020},
        {'upper': -0.015708, 'lower': -5.02654},
        {'upper': 3.40, 'lower': 0.03},
        {'upper': 5.60, 'lower': 0.12},
        {'upper': 0.0115, 'lower': 0.0},
        {'upper': 0.0115, 'lower': 0.0},
    ]


def processJoints(self, joint_states):
    armStatesReceived = joint_states.name[10] == 'arm_joint_1'


    if armStatesReceived and self.joyMsg is not None:

        self.crntJointStates = copy.deepcopy(joint_states)
        for index in range(0, len(joint_states.position)-2):
            pos = JointValue()
            pos.joint_uri = "arm_joint_" + str(index + 1)
            pos.unit = "rad"
            pos.value = joint_states.position[index]
            self.crntJointPositions.positions.append(pos)
        pos = JointValue()
        pos.joint_uri = "gripper_finger_joint_l"
        pos.unit = "m"
        pos.value = joint_states.position[5]
        self.gripperPositions.positions.append(pos)

        pos = JointValue()
        pos.joint_uri = "gripper_finger_joint_r"
        pos.unit = "m"
        pos.value = joint_states.position[6]
        self.gripperPositions.positions.append(pos)

        # desiredPos = copy.deepcopy(self.crntJointPositions)
        desiredGripPos = copy.deepcopy(self.gripperPositions)

        self.joyMsg.axes = numpy.array(self.joyMsg.axes)        
        left_axis = self.joyMsg.axes[[0, 1]]
        right_axis = self.joyMsg.axes[[2, 3]]
        l2_axis = self.joyMsg.axes[12] * -1
        r2_axis = self.joyMsg.axes[13] * -1
    square = self.joyMsg.buttons[15]
    x = self.joyMsg.buttons[14]
    circle = self.joyMsg.buttons[13]
        triangle = self.joyMsg.buttons[12]
    r1 = self.joyMsg.buttons[11]
    l1 = self.joyMsg.buttons[10]
    r2 = self.joyMsg.buttons[9]
    l2 = self.joyMsg.buttons[8]
        left_dpad = self.joyMsg.axes[6] == 1
        right_dpad = self.joyMsg.axes[6] == -1
        up_dpad = self.joyMsg.axes[7] == 1
        down_dpad = self.joyMsg.axes[7] == -1

        if not self.previous_vel_change:
            if square:
                self.maxVel += 0.25
            if circle:
                self.maxVel -= 0.25
            if self.maxVel < 0.25:
                self.maxVel = 0.25

        self.previous_vel_change = square or circle

        # object that will carry all desired velocities to each joint
        desiredVel = JointVelocities()
    # desiredGripPos = JointPositions()

        for index in range(0, len(joint_states.position)-2):
            vel = JointValue()
            vel.joint_uri = "arm_joint_" + str(index + 1)
            vel.unit = "s^-1 rad"  # radians per second
            desiredVel.velocities.append(vel)

        desiredVel.velocities[0].value -= left_axis[0] * self.maxVel
        desiredVel.velocities[1].value -= left_axis[1] * self.maxVel
        if not r1:
            desiredVel.velocities[2].value -= right_axis[1] * self.maxVel
        else:
            desiredVel.velocities[3].value -= right_axis[1] * self.maxVel

        desiredVel.velocities[4].value -= right_axis[0] * self.maxVel * 2


        desiredGripPos.positions[0].value -= r2_axis * 0.0115
        desiredGripPos.positions[1].value = desiredGripPos.positions[0].value


        joint_message = ''
        vel_message = ''
        grip_message = ''
        for jointNum in range(5):
            joint_message += ('joint ' + str(jointNum) + ': ' + str(self.crntJointPositions.positions[jointNum].value) + '    ')
            vel_message += ('veloc ' + str(jointNum) + ': ' + str(desiredVel.velocities[jointNum].value) + '    ')      

        for gripNum in [0, 1]:
            desiredGripPos.positions[gripNum].value = self.checkValues(desiredGripPos.positions[gripNum].value, gripNum+5)
            grip_message += ('grip ' + str(gripNum) + ': ' + str(desiredGripPos.positions[gripNum].value) + '    ')

    self.gripperPositions = desiredGripPos

        print(joint_message)
        print(vel_message)
        print(grip_message)

        self.gripPub.publish(desiredGripPos)
        self.velPub.publish(desiredVel)

def checkValues(self, val, jointNum):
    # print("Received checkValues:", val, jointNum)
    jointNum = int(jointNum)
    # Limit angles for the joints, list is not entirely correct

    if val > self.jointBounds[jointNum]['upper']:
        val = self.jointBounds[jointNum]['upper']
    elif val < self.jointBounds[jointNum]['lower']:
        val = self.jointBounds[jointNum]['lower']
    return val

def processJoystick(self, joyMsg):
    self.joyMsg = joyMsg


def main(args):
arm_teleop()
rospy.init_node('arm_teleop', anonymous=True)
try:
    rospy.spin()
except KeyboardInterrupt:
    print("Shutting down")

 if __name__ == '__main__':
main(sys.argv)

Full error message:

[ERROR] [WallTime: 1485808919.140427] [197.350006] bad callback: > Traceback (most recent call last): File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 720, in invokecallback cb(msg) File "/home/alessio/kukaws/src/teleopkuka/src/arm_teleop.py", line 155, in processJoints self.gripPub.publish(desiredGripPos) File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 852, in publish self.impl.publish(data) File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 1011, in publish raise ROSException("publish() to a closed topic") ROSException: publish() to a closed topic

Also the example provided by Kuka works correctly until it needs to close the gripper. At this point nothing happens. here the example

I'm working on the simulator V-REP with this youbot plugin.