def publish_to_youbot(jointState):
pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = jointState.name[0]
jv1.value = jointState.position[0]
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = jointState.name[1]
jv2.value = jointState.position[1]
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = jointState.name[2]
jv3.value = jointState.position[2]
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = jointState.name[3]
jv4.value = jointState.position[3]
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = jointState.name[4]
jv5.value = jointState.position[4]
jv5.unit = "rad"
p = Poison()
#print p
jp.poisonStamp = p
jp.positions = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
return 'arm moved successfully'
except Exception, e:
print e
return 'arm move failure'
def to_joint_positions(gripper_positions):
robot = os.getenv('ROBOT')
if robot == 'youbot-edufill':
left_gripper_val = gripper_positions[0]
right_gripper_val = gripper_positions[1]
pub = rospy.Publisher("/arm_1/gripper_controller/position_command",
JointPositions)
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper_val
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper_val
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
pub.publish(jp)
rospy.sleep(1.0)
rospy.loginfo('Gripper control command published successfully')
except Exception, e:
rospy.loginfo('%s', e)
def moveGripper(self, gPublisher, floatVal):
jp = JointPositions()
myPoison = Poison()
myPoison.originator = 'yb_grip'
myPoison.description = 'whoknows'
myPoison.qos = 0.0
jp.poisonStamp = myPoison
nowTime = rospy.Time.now()
jvl = JointValue()
jvl.timeStamp = nowTime
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = floatVal
jp.positions.append(jvl)
jvr = JointValue()
jvr.timeStamp = nowTime
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = floatVal
jp.positions.append(jvr)
gPublisher.publish(jp)
def talkerGripper(G):
pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(5) # 10hz
count = 0
while not rospy.is_shutdown():
joint_pos = JointPositions()
joint_number = 1
joint_val_1 = JointValue()
joint_val_2 = JointValue()
joint_val_1.joint_uri = "gripper_finger_joint_l"
joint_val_1.unit = "rad"
joint_val_2.joint_uri = "agripper_finger_joint_r"
joint_val_2.unit = "rad"
joint_val_1 = JointValue()
joint_val_1.joint_uri = "gripper_finger_joint_l"
joint_val_1.unit = "m"
joint_val_1.value = G[0]
joint_val_2 = JointValue()
joint_val_2.joint_uri = "gripper_finger_joint_r"
joint_val_2.unit = "m"
joint_val_2.value = G[1]
poison = Poison()
joint_pos.poisonStamp = poison
joint_pos.positions = [joint_val_1, joint_val_2]
#pub_youbotleaparm.publish(joint_pos)
# rospy.loginfo(joint_pos)
pub.publish(joint_pos)
count+=1
rate.sleep()
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'
def create_gripper_msg(left_gripper, right_gripper):
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2]
return jp
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.016
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = 0.023
jv4.unit = "rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = 0.12
jv5.unit = "rad"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jp)
pub.publish(jp)
except Exception, e:
print e
'''
JOINT LIMITS:
[ WARN] [1340337373.846618553]: Cannot set arm joint 1:
The setpoint angle is out of range. The valid range is between 0.0100692 rad and 5.84014 rad and it is: 0.01 rad
[ WARN] [1340337373.847057559]: Cannot set arm joint 2:
The setpoint angle is out of range. The valid range is between 0.0100692 rad and 2.61799 rad and it is: 0.01 rad
def talker(Q):
pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(5) # 10hz
count = 0
while not rospy.is_shutdown():
joint_pos = JointPositions()
joint_number = 1
joint_val_1 = JointValue()
joint_val_2 = JointValue()
joint_val_3 = JointValue()
joint_val_4 = JointValue()
joint_val_5 = JointValue()
joint_val_1.joint_uri = "arm_joint_1"
joint_val_1.unit = "rad"
joint_val_2.joint_uri = "arm_joint_2"
joint_val_2.unit = "rad"
joint_val_3.joint_uri = "arm_joint_3"
joint_val_3.unit = "rad"
joint_val_4.joint_uri = "arm_joint_4"
joint_val_4.unit = "rad"
joint_val_5.joint_uri = "arm_joint_5"
joint_val_5.unit = "rad"
joint_val_1 = JointValue()
joint_val_1.joint_uri = "arm_joint_1"
joint_val_1.unit = "rad"
joint_val_1.value = Q[0] + 2.95 #adding offsets to all angles, we assume 0's
#in candle possition, youbot diver has 0's
#close to folded possition
joint_val_2 = JointValue()
joint_val_2.joint_uri = "arm_joint_2"
joint_val_2.unit = "rad"
joint_val_2.value = Q[1] + 1.05
joint_val_3 = JointValue()
joint_val_3.joint_uri = "arm_joint_3"
joint_val_3.unit = "rad"
joint_val_3.value = Q[2] - 2.44
joint_val_4 = JointValue()
joint_val_4.joint_uri = "arm_joint_4"
joint_val_4.unit = "rad"
joint_val_4.value = Q[3] + 1.73
joint_val_5 = JointValue()
joint_val_5.joint_uri = "arm_joint_5"
joint_val_5.unit = "rad"
joint_val_5.value = Q[4] + 2.95
poison = Poison()
joint_pos.poisonStamp = poison
joint_pos.positions = [joint_val_1, joint_val_2, joint_val_3, joint_val_4, joint_val_5]
#pub_youbotleaparm.publish(joint_pos)
# rospy.loginfo(joint_pos)
pub.publish(joint_pos)
count+=1
rate.sleep()
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('kuka_arm_controller')
arm_joint_publisher = rospy.Publisher(
"/arm_1/arm_controller/position_command",
JointPositions,
queue_size=10)
joint1 = JointValue()
joint2 = JointValue()
joint3 = JointValue()
joint4 = JointValue()
joint5 = JointValue()
poison = Poison()
joint_pos = JointPositions()
delay = rospy.Rate(10)
try:
print(msg)
joint1.joint_uri = "arm_joint_1"
joint1.unit = "rad"
joint1.value = check_positions[0]
joint2.joint_uri = "arm_joint_2"
joint2.unit = "rad"
joint2.value = check_positions[1]
joint3.joint_uri = "arm_joint_3"
joint3.unit = "rad"
joint3.value = check_positions[2]
def talker():
pub = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(5) # 10hz
count = 0
while not rospy.is_shutdown():
joint_pos = JointPositions()
joint_number = 1
default_joint_one = 0.0100692
default_joint_two = 0.0100692
default_joint_three = -0.02655
default_joint_four = 0.0221239
default_joint_five = 0.110619
joint_val_1 = JointValue()
joint_val_2 = JointValue()
joint_val_3 = JointValue()
joint_val_4 = JointValue()
joint_val_5 = JointValue()
joint_val_1.joint_uri = "arm_joint_1"
joint_val_1.unit = "rad"
joint_val_2.joint_uri = "arm_joint_2"
joint_val_2.unit = "rad"
joint_val_3.joint_uri = "arm_joint_3"
joint_val_3.unit = "rad"
joint_val_4.joint_uri = "arm_joint_4"
joint_val_4.unit = "rad"
joint_val_5.joint_uri = "arm_joint_5"
joint_val_5.unit = "rad"
joint_val_1 = JointValue()
joint_val_1.joint_uri = "arm_joint_1"
joint_val_1.unit = "rad"
joint_val_1.value = 2.95
joint_val_2 = JointValue()
joint_val_2.joint_uri = "arm_joint_2"
joint_val_2.unit = "rad"
joint_val_2.value = 1.05
joint_val_3 = JointValue()
joint_val_3.joint_uri = "arm_joint_3"
joint_val_3.unit = "rad"
joint_val_3.value = -2.44
joint_val_4 = JointValue()
joint_val_4.joint_uri = "arm_joint_4"
joint_val_4.unit = "rad"
joint_val_4.value = 1.73
joint_val_5 = JointValue()
joint_val_5.joint_uri = "arm_joint_5"
joint_val_5.unit = "rad"
joint_val_5.value = 2.95
poison = Poison()
