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 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 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 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
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
def _createGripperJointPositions(self, left, right):
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
return jp
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)
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
[ WARN] [1340337373.847447567]: Cannot set arm joint 3:
rospy.sleep(0.5)
try:
jp = JointPositions()
# if value == 0: gripper is closed
# if value == 0.0115: griper is open
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = 0.01
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = 0.01
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
pub.publish(jp)
rospy.sleep(1.0)
except Exception, e:
print e
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()
joint2.unit = "rad"
joint2.value = check_positions[1]
joint3.joint_uri = "arm_joint_3"
joint3.unit = "rad"
joint3.value = check_positions[2]
joint4.joint_uri = "arm_joint_4"
joint4.unit = "rad"
joint4.value = check_positions[3]
joint5.joint_uri = "arm_joint_5"
joint5.unit = "rad"
joint5.value = check_positions[4]
joint_pos.poisonStamp = poison
joint_pos.positions = [joint1, joint2, joint3, joint4, joint5]
print("Check Arm-joint")
for i in range(0, 5):
arm_joint_publisher.publish(joint_pos)
delay.sleep()
for i in range(0, 20):
joint1.value = joint_positions[0]
joint2.value = joint_positions[1]
joint3.value = joint_positions[2]
joint4.value = joint_positions[3]
joint5.value = joint_positions[4]
print("Waiting process")
# joint arm_joint_4 valid range is between 0.0221239 and 3.4292
# joint arm_joint_5 valid range is between 0.110619 and 5.64159
# joint_val_1.value = (((hand_normal_[0] /2) + 0.5) * (5.84014 - 0.0100692) ) + 0.0100692
# joint_val_2.value = (((hand_normal_[0] /2) + 0.5) * (2.61799 - 0.0100692) ) + 0.0100692
# joint_val_3.value = (((hand_normal_[0] /2) + 0.5) * (5.02655 - 0.015708) ) - 5.02655
# joint_val_4.value = (((hand_normal_[0] /2) + 0.5) * (3.4292 - 0.0221239) ) + 0.0221239
# joint_val_5.value = (((hand_normal_[0] /2) + 0.5) * (5.64159 - 0.110619) ) + 0.110619
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)
# print "x_vel: %f m/s, y_vel: %f m/s, ang_vel: %f m/s" % (twist.linear.x , twist.linear.y , twist.angular.z )
# print "pitch: %f m/s, yaw: %f m/s, roll: %f m/s" % (hand_pitch_ , hand_yaw_ , hand_roll_)
# r.sleep()
# try:
# sender()
# except rospy.ROSInterruptException:
# pass
# termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
