Control YouBot Arm Problem Gripper

edit

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/kuka_ws/src/teleop_kuka/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 ...