Control YouBot Arm Problem Gripper
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 ...
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.
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.
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.