def talker(origin_frame): #Run this program as a new node in the ROS computation graph #called /talker. rospy.init_node('main_controller', anonymous=True) #Create an instance of the rospy.Publisher object which we can #use to publish messages to a topic. pub = rospy.Publisher('gripper', String, queue_size=10) #Create a tf buffer, which is primed with a tf listener tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) planner = PathPlanner("right_arm") right = Limb('right') # rj = right.joint_names() r = rospy.Rate(10) # Loop until the node is killed with Ctrl-C while not rospy.is_shutdown(): try: pub_string = raw_input('Type [open] or [close]: ') # Publish our string to the 'chatter_talk' topic pub.publish(pub_string) while not rospy.is_shutdown(): try: raw_input("Press <Enter> to move the right arm to goal pose 1: ") plan = planner.plan_to_joint_default() # print(plan) if not planner.execute_plan(plan): raise Exception("Execution failed") time.sleep(2) except Exception as e: print e traceback.print_exc() else: break trans_to_wrist = tfBuffer.lookup_transform(origin_frame, "right_hand", rospy.Time()) # trans_to_tip = tfBuffer.lookup_transform(origin_frame, "right_gripper", rospy.Time()) trans_to_base = tfBuffer.lookup_transform(origin_frame, "right_lower_shoulder", rospy.Time()) trans_to_cup = tfBuffer.lookup_transform(origin_frame, "cup_center", rospy.Time()) gun = [0,0,0] base = [0,0,0] goal = [0,0,0] # gun[0] = (trans_to_wrist.transform.translation.x+trans_to_tip.transform.translation.x)/2 # gun[1] = (trans_to_wrist.transform.translation.y+trans_to_tip.transform.translation.y)/2 # gun[2] = (trans_to_wrist.transform.translation.z+trans_to_tip.transform.translation.z)/2 gun[0] = trans_to_wrist.transform.translation.x gun[1] = trans_to_wrist.transform.translation.y gun[2] = trans_to_wrist.transform.translation.z base[0] = trans_to_base.transform.translation.x base[1] = trans_to_base.transform.translation.y base[2] = trans_to_base.transform.translation.z goal[0] = trans_to_cup.transform.translation.x goal[1] = trans_to_cup.transform.translation.y # height of cup goal[2] = 0.12065/2 theta1 = Projectile.calc_xy(goal, base, gun) print("theta1 = "+str(theta1)) while not rospy.is_shutdown(): try: raw_input("Press <Enter> to move the right arm to theta1: ") goal_joints = right.joint_angles() goal_joints["right_s0"] += np.radians(theta1) plan = planner.plan_to_joint(goal_joints) if not planner.execute_plan(plan): raise Exception("Execution failed") time.sleep(2) except Exception as e: print e traceback.print_exc() else: break trans_to_wrist = tfBuffer.lookup_transform(origin_frame, "right_hand", rospy.Time()) # trans_to_tip = tfBuffer.lookup_transform(origin_frame, "right_gripper", rospy.Time()) # gun[0] = (trans_to_wrist.transform.translation.x+trans_to_tip.transform.translation.x)/2 # gun[1] = (trans_to_wrist.transform.translation.y+trans_to_tip.transform.translation.y)/2 # gun[2] = (trans_to_wrist.transform.translation.z+trans_to_tip.transform.translation.z)/2 gun[0] = trans_to_wrist.transform.translation.x gun[1] = trans_to_wrist.transform.translation.y gun[2] = trans_to_wrist.transform.translation.z theta2 = gripper_sub.aim_z(gun,goal) print("theta2 = "+str(np.degrees(theta2))) while not rospy.is_shutdown(): try: raw_input("Press <Enter> to move the right arm to theta2: ") goal_joints = right.joint_angles() goal_joints["right_w2"] = pi/2-theta2 + np.radians(OFFSET) plan = planner.plan_to_joint(goal_joints) if not planner.execute_plan(plan): raise Exception("Execution failed") time.sleep(2) except Exception as e: print e traceback.print_exc() else: break # Construct a string that we want to publish # (In Python, the "%" operator functions similarly # to sprintf in C or MATLAB) pub_string = raw_input('Type [open] or [close]: ') # Publish our string to the 'chatter_talk' topic pub.publish(pub_string) # print(np.degrees(joint_angles)) # print(delta) # if np.amax(error) >= .1: # print("go") # gripper_sub.default_state(JOINT_DEFAULT) # aim_xy(end_coord, base_coord, cup_coord, joint_state) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print('loading') pass r.sleep()