Control YouBot Arm Problem Gripper

asked 2017-01-29 14:53:46 -0600

Alessio gravatar image

updated 2017-01-30 14:48:39 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

It looks like you haven't included the full error message in your post; in particular the part of the error message that actually says what the error is seems to be missing. It also looks like there are some formatting problems with your code.

ahendrix gravatar imageahendrix ( 2017-01-29 17:14:50 -0600 )edit

Since indentation is important in python, please take the time to make sure that your post appears indented in the same way that it does on your computer.

ahendrix gravatar imageahendrix ( 2017-01-29 17:15:30 -0600 )edit

The code on my pc is well formatted, I just edit my post above. I'm thinking that could be a problem related to the simulation program.

Alessio gravatar imageAlessio ( 2017-01-30 14:51:20 -0600 )edit