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