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)