def joint_velocities(joint_velocities):
robot = os.getenv('ROBOT')
if robot == 'youbot-edufill':
joint_velocity_1 = joint_velocities[0]
joint_velocity_2 = joint_velocities[1]
joint_velocity_3 = joint_velocities[2]
joint_velocity_4 = joint_velocities[3]
joint_velocity_5 = joint_velocities[4]
pub = rospy.Publisher('arm_1/arm_controller/velocity_command', JointVelocities)
rospy.sleep(0.5)
try:
jv = JointVelocities()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = joint_velocity_1
jv1.unit = "s^-1 rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = joint_velocity_2
jv2.unit = "s^-1 rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = joint_velocity_3
jv3.unit = "s^-1 rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = joint_velocity_4
jv4.unit = "s^-1 rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = joint_velocity_5
jv5.unit = "s^-1 rad"
p = Poison()
#print p
jv.poisonStamp = p
jv.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jv)
return 'arm moved successfully'
except Exception, e:
print e
return 'arm move failure'
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.5
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.5
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = 3.0
jv5.unit = "rad"
p = Poison()
jp.poisonStamp = p
jp.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
pub.publish(jp)
except Exception, e:
print e
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.5
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = 3.0
jv5.unit = "rad"
p = Poison()
jp.poisonStamp = p
jp.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
pub.publish(jp)
except Exception, e:
print e
