def grasp(key, x, y, z, x_threshhold=0.02, y_threshhold=0.02, hoverDist=0.020, zoffset=.92, orien_const=[], or_x=0.0, or_y=-1.0, or_z=0.0, or_w=0.0): #while not rospy.is_shutdown(): try: left_arm_planner = PathPlanner("left_arm") right_arm_planner = PathPlanner("right_arm") goal = PoseStamped() goal.header.frame_id = "base" #x, y, and z position goal.pose.position.x = x + x_threshhold goal.pose.position.y = y + y_threshhold goal.pose.position.z = z - zoffset + hoverDist #Orientation as a quaternion goal.pose.orientation.x = or_x goal.pose.orientation.y = or_y goal.pose.orientation.z = or_z goal.pose.orientation.w = or_w if key == 'left_arm': plan = left_arm_planner.plan_to_pose(goal, orien_const) openLeftGripper() left_arm_planner.execute_plan(plan) closeLeftGripper() else: plan = right_arm_planner.plan_to_pose(goal, orien_const) openRightGripper() right_arm_planner.execute_plan(plan) closeRightGripper() except Exception as e: print e traceback.print_exc()
def play_chords(note_pos): planner = PathPlanner("left_arm") new_note_pos = [] num_waypoints_per_note = 4 # add waypoints above the note to ensure the note is struck properly for note in note_pos: waypoint1 = Vector3() waypoint1.x = note.x waypoint1.y = note.y waypoint1.z = note.z + waypoint_offset waypoint2 = Vector3() waypoint2.x = note.x waypoint2.y = note.y waypoint2.z = note.z + waypoint_offset / 2.0 new_note_pos += [waypoint1, waypoint2, note, waypoint1] for i, pos in enumerate(new_note_pos): print(i, pos) # Loop until that position is reached while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = pos.x goal_1.pose.position.y = pos.y goal_1.pose.position.z = pos.z # #Orientation as a quaternion, facing straight down goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_1, list()) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting ## TF CODE tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) ## TF CODE planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned limb = intera_interface.Limb("right") control = Controller(Kp, Kd, Ki, Kw, limb) ## ## Add the obstacle to the planning scene here ## X = 0.40 Y = 1.20 Z = 0.10 Xp = 0.5 Yp = 0.00 Zp = -0.15 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa #planner.add_box_obstacle([X,Y,Z], "wall", pose) #Wall 2 X = 0.40 Y = 0.10 Z = 0.30 Xp = 0.5 Yp = -0.15 Zp = 0.05 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa #planner.add_box_obstacle([X,Y,Z], "wall2", pose) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" #orien_const.orientation.y = -1.0; #orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.05 orien_const.absolute_z_axis_tolerance = 0.05 orien_const.weight = 1.0 waypoints = list() # Section to add waypoints # step_size = size between x position changes # final_length = final x position #GOAL Points # goal_1.pose.position.x = 0.585 # goal_1.pose.position.y = 0.156 # goal_1.pose.position.z = -0.138 # goal_1.pose.orientation.x = 0 # goal_1.pose.orientation.y = 0.707 # goal_1.pose.orientation.z = 0 # goal_1.pose.orientation.w = 0.707 #GOAL Points while not rospy.is_shutdown(): ## ZERO THE ROBOT ## goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = 0.585 goal_1.pose.position.y = 0.156 goal_1.pose.position.z = -0.138 goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break waypoints = list() ## ZERO THE ROBOT ## print("ROBOT ZEROED") #control._velocity_scalar = 0.5 step_size = 0.0005 #0.0025 FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_X = goal_x + offset + 0.2 x = tf_px + offset print("TF'd zero") update = 0 execute = 0 i = 0 STOP = 0 while ((x < FINAL_X or update == 0) and (i < 5)): if (update == 0): try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w x = tf_px + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 y = tf_py z = tf_pz if (y < goal_y - 0.05): y = y + step_size if (y > goal_y + 0.05): y = y - step_size if (z < goal_z - 0.05): z = z + step_size if (z > goal_z + 0.05): z = z - step_size #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) x = x + step_size if (x >= FINAL_X): execute = 1 #print("added waypoint") if (execute == 1): while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag waypoints = list() update = 0 print("EXEcute") print(i) i = i + 1 print("END OF LOOP") print("ZERO AGAIN") control._zero = 1 waypoints = list() ## ZERO THE ROBOT ## control._flag = 0 control._trigger = 0 control._velocity_scalar = 1 goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = 0.585 goal_1.pose.position.y = 0.156 goal_1.pose.position.z = -0.138 goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break waypoints = list() control._zero = 0 ## ZERO THE ROBOT ## print("ROBOT ZEROED...AGAIN") #plt.plot(control._sensor_data) s = list(map(float, control._sensor_data)) scales = list(map(float, control._scalar_data)) goal_line = list(map(float, control._goal_list)) #s = list(map(float, control._debug)) #control._sensor_data = sorted(control._sensor_data) ##print(min(s)) ##print(max(s)) plt.subplot(211) plt.plot(s) plt.plot(goal_line) plt.axis([0, len(control._sensor_data), min(s), max(s)]) plt.ylabel('Sensor Data') plt.subplot(212) plt.plot(scales) plt.show() control._sensor_data = list() control._scalar_data = list() control._goal_list = list() waypoints = list() control._velocity_scalar = 1
# #Orientation as a quaternion # table_pose.pose.orientation.w = 1.0 # planner.add_box_obstacle(table_size, "table", table_pose) # cubes = [(0.48, -0.46, "red"), (0.486, -0.46, "blue"), (0.486, -0.46, "green")] while not rospy.is_shutdown(): #Move right arm to default pose. cnt = 0 while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(default_pose, []) # print(plan) raw_input("Press <Enter> to move the right arm to default state: ") result = planner.execute_plan(plan) # print(result) if not result: raise Exception("Execution failed") except Exception as e: print(e) else: break raw_input("Press enter if camera correctly positioned") #Get positions of cubes. message = rospy.wait_for_message("/colors_and_position", ColorAndPositionPairs) cubes = message.pairs
def main(): """ Main Script """ ############# offset_p = -0.07 offset_g = 0.04 ############# # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("left_arm") ## ## Init Gripper ## rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled left = baxter_interface.Gripper('left', CHECK_VERSION) tf_listener = tf.TransformListener(rospy.Duration(1)) raw_input("Press <Enter> to start!") try_another_grasp = 0 good_grasps = rospy.get_param('good_grasps') for g in range(good_grasps): i_pregrasp = 0 ## get grasp pose tf_listener.waitForTransform('base', "sample_grasp_" + str(g), rospy.Time(0), rospy.Duration(2.0)) t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(g), rospy.Time()) matrix = quaternion_matrix(q) new_matrix = np.eye(4) new_matrix[:, 0] = matrix[:, 2] new_matrix[:, 1] = -matrix[:, 1] new_matrix[:, 2] = matrix[:, 0] t_pre = t + new_matrix[0:3, 2] * offset_p t_g = t + new_matrix[0:3, 2] * offset_g quaternion_from_matrix q = quaternion_from_matrix(new_matrix) print("try grasp {0} to move to object...........Move it!".format(g)) while not rospy.is_shutdown(): i_pregrasp += 1 try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = t_pre[0] goal_1.pose.position.y = t_pre[1] goal_1.pose.position.z = t_pre[2] #Orientation as a quaternion goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_1, list()) ## In Rviz # rospy.sleep(6) # raw_input("Press <Enter> to PRE grasp pose: ") print("PRE grasp pose {0}, try {1} times".format( g, i_pregrasp)) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e ## jump to next grasp pose (1) if i_pregrasp > 2: try_another_grasp = 1 break else: break ## jump to next grasp pose (2) if try_another_grasp == 1: print('Try another !') try_another_grasp = 0 planner.clear_goal() continue i_grasp = 0 while not rospy.is_shutdown(): i_grasp += 1 try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = t_g[0] goal_2.pose.position.y = t_g[1] goal_2.pose.position.z = t_g[2] #Orientation as a quaternion goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_2, list()) ## in Rviz # rospy.sleep(6) # raw_input("Press <Enter> to GRASP pose : ") print("GRASP pose %d" % i_grasp) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break ## ## grasp ## open_gripper(left) close_gripper(left) i_leave = 1 while not rospy.is_shutdown(): i_leave += 1 try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position # [0.551, 0.690, -0.049] goal_3.pose.position.x = 0.551 goal_3.pose.position.y = 0.690 goal_3.pose.position.z = -0.049 #Orientation as a quaternion goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_3, list()) # raw_input("Press <Enter> to LEAVE pose: ") print("LEAVE pose %d" % i_leave) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break ## ## grasp ## open_gripper(left) raw_input("Press <Enter> to continue")
# raise Exception("Execution failed") # except Exception as e: # print(e) # else: # break while not rospy.is_shutdown(): raw_input("press enter to start") move_to_default(planner) # Move left arm to vision pose. while not rospy.is_shutdown(): try: plan = planner_left.plan_to_pose(vision_pose, []) print(plan) raw_input( "Press <Enter> to move the left arm to vision state: ") result = planner_left.execute_plan(plan) print(result) if not result: raise Exception("Execution failed") except Exception as e: print(e) else: break raw_input("Press enter if camera correctly positioned") #Get positions of cubes.
def main(): """ Main Script """ ############# offset = -0.3 ############# # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("left_arm") ## ## Init Gripper ## rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled left = baxter_interface.Gripper('left', CHECK_VERSION) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; tf_listener = tf.TransformListener(rospy.Duration(1)) id_num = raw_input("Input Grasp ID: ") tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num), rospy.Time(0), rospy.Duration(2.0)) t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num), rospy.Time()) matrix = quaternion_matrix(q) new_matrix = matrix # new_matrix = np.eye(4) # new_matrix[:, 0] = matrix[:, 2] # new_matrix[:, 1] = -matrix[:, 1] # new_matrix[:, 2] = matrix[:, 0] t_pre = t + new_matrix[0:3, 2] * offset quaternion_from_matrix q = quaternion_from_matrix(new_matrix) print("try to move to object...........Move it!") ########## id_num = 0 ########## while not rospy.is_shutdown(): i_pregrasp = 0 while not rospy.is_shutdown(): ########################################### # if i_pregrasp > 5: # tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num), rospy.Time(0), rospy.Duration(2.0)) # t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num), rospy.Time()) # if id_num > rospy.get_param('good_grasps'): # print("Fail!") # return ############################################ i_pregrasp += 1 try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = t_pre[0] goal_1.pose.position.y = t_pre[1] goal_1.pose.position.z = t_pre[2] #Orientation as a quaternion goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_1, list()) print(plan) if not plan: raise Exception("Execution failed") ## in Rviz rospy.sleep(6) raw_input("Press <Enter> to PRE grasp pose: ") print("PRE grasp pose %d" % i_pregrasp) # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break i_grasp = 0 while not rospy.is_shutdown(): i_grasp += 1 try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = t[0] goal_2.pose.position.y = t[1] goal_2.pose.position.z = t[2] #Orientation as a quaternion goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_2, list()) if not plan: raise Exception("Execution failed") ## in Rviz rospy.sleep(6) raw_input("Press <Enter> to GRASP pose : ") print("GRASP pose %d" % i_grasp) # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break ## ## grasp ## # open_gripper(left) # close_gripper(left) i_leave = 1 while not rospy.is_shutdown(): i_leave += 1 try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position # [0.551, 0.690, -0.049] goal_3.pose.position.x = 0.551 goal_3.pose.position.y = 0.690 goal_3.pose.position.z = -0.049 #Orientation as a quaternion goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) plan = planner.plan_to_pose(goal_3, list()) if not plan: raise Exception("Execution failed") rospy.sleep(6) raw_input("Press <Enter> to LEAVE pose: ") print("LEAVE pose %d" % i_leave) # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break ## ## grasp ## open_gripper(left) raw_input("Press <Enter> to continue")
def main(): """ Main Script """ #=================================================== # Code to add gripper #rospy.init_node('gripper_test') # # Set up the right gripper #right_gripper = robot_gripper.Gripper('right_gripper') # #Calibrate the gripper (other commands won't work unless you do this first) #print('Calibrating...') #right_gripper.calibrate() #rospy.sleep(2.0) # #Close the right gripper to hold publisher # # MIGHT NOT NEED THIS # print('Closing...') # right_gripper.close() # rospy.sleep(1.0) #=================================================== planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned limb = intera_interface.Limb("right") control = Controller(Kp, Kd, Ki, Kw, limb) ## ## Add the obstacle to the planning scene here ## #Tower X = .075 Y = .15 Z = .0675 Xp = 0 Yp = 0 Zp = 0 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 # pose = PoseStamped() # pose.header.stamp = rospy.Time.now() # #TODO: Is this the correct frame name? # pose.header.frame_id = "ar_marker_4" # pose.pose.position.x = Xp # pose.pose.position.y = Yp # pose.pose.position.z = Zp # pose.pose.orientation.x = Xpa # pose.pose.orientation.y = Ypa # pose.pose.orientation.z = Zpa # pose.pose.orientation.w = Wpa # planner.add_box_obstacle([X,Y,Z], "tower", pose) #Table X = 1 Y = 2 Z = .0675 Xp = 1.2 Yp = 0 Zp = -0.22 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() #TODO: Is this the correct frame name? pose.header.frame_id = "base" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa planner.add_box_obstacle([X, Y, Z], "table", pose) try: #right_gripper_tip goal_1 = PoseStamped() goal_1.header.frame_id = "ar_marker_4" #x, y, and z position goal_1.pose.position.y = 0 goal_1.pose.position.z = .5 #removing the 6th layer goal_1.pose.position.x = 0 #0.0825 #Orientation as a quaternion goal_1.pose.orientation.x = 0 goal_1.pose.orientation.y = 0 goal_1.pose.orientation.z = 0.707 goal_1.pose.orientation.w = 0.707 plan = planner.plan_to_pose(goal_1, list()) if not planner.execute_plan(plan): raise Exception("Execution failed0") except Exception as e: print e
def main(cube_pose, end_pose): planner = PathPlanner("right_arm") orien_const = OrientationConstraint() orien_const.link_name = "right_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.y = -1.0; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 10000; orien_const.weight = 1.0; # allow only 1/2 pi rotation so as to not lose the cube during transit orien_path_const = OrientationConstraint() orien_path_const.link_name = "right_gripper"; orien_path_const.header.frame_id = "base"; orien_path_const.orientation.y = -1.0; orien_path_const.absolute_x_axis_tolerance = 0.1; orien_path_const.absolute_y_axis_tolerance = 0.1; orien_path_const.absolute_z_axis_tolerance = 1.57; orien_path_const.weight = 1.0; # quat = QuaternionStamped() # quat.header.frame_id = "base" # quat.quaternion.w = 1.0 start_pose = get_start(cube_pose, end_pose) end_pose.pose.orientation = start_pose.pose.orientation table_size = np.array([0.6, 1.2, 0.03]) table_pose = PoseStamped() table_pose.header.frame_id = "base" #x, y, and z position table_pose.pose.position.x = 0.5 if ROBOT == "baxter": table_pose.pose.position.z = -0.23 # for baxter else: table_pose.pose.position.z = -0.33 #Orientation as a quaternion table_pose.pose.orientation.w = 1.0 planner.add_box_obstacle(table_size, "table", table_pose) # cube_size = np.array([0.05, 0.05, 0.15]) default_state = get_pose(0.6, -0.5, -0.17, 0.0, -1.0, 0.0, 0.0) # default_state = get_pose(0.94, 0.2, 0.5, 0.0, -1.0, 0.0, 0.0) while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(default_state, []) print(type(plan)) raw_input("Press <Enter> to move the right arm to default pose: ") if not planner.execute_plan(plan): raise Exception("(ours) Execution failed") except Exception as e: print("caught!") print e else: break # planner.add_box_obstacle(cube_size, "cube", cube_pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(start_pose, [orien_const]) raw_input("Press <Enter> to move the right arm to cube: ") if not planner.execute_plan(plan): raise Exception("(ours) Execution failed") except Exception as e: print("caught!") print e else: break orien_path_const.orientation = start_pose.pose.orientation # planner.remove_obstacle("cube") while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(end_pose, [orien_path_const]) raw_input("Press <Enter> to move the right arm to end position: ") if not planner.execute_plan(plan): raise Exception("(ours) Execution failed") except Exception as e: print("caught!") print e else: break
def main(): planner = PathPlanner("right_arm") #Wait for the IK service to become available #rospy.wait_for_service('compute_ik') rospy.init_node('service_query') place_holder = {'images': [], 'camera_infos': []} rospy.Subscriber("/cameras/head_camera/image", Image, lambda x: place_holder['images'].append(x)) rospy.Subscriber("/cameras/head_camera/camera_info", CameraInfo, lambda x: place_holder['camera_infos'].append(x)) limb = Limb("right") #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) count = 0 print(limb.endpoint_pose()) calibration_points = [] while not rospy.is_shutdown() and count < 3: #calibration points 1-3, closest to robot right = origin, closest to robot left, id_card 3 3/8 inches forward from first point ''' /cameras/head_camera/camera_info /cameras/head_camera/camera_info_std /cameras/head_camera/image ''' count += 1 raw_input("calibrate point " + str(count)) pos = limb.endpoint_pose() calibration_points.append({}) calibration_points[-1]['world_coordinates'] = (pos['position'].x, pos['position'].y, pos['position'].z) calibration_points[-1]['camera_info'] = place_holder['camera_infos'][ -1] calibration_points[-1]['image'] = place_holder['images'][-1] # for i in calibration_points: # print(i['world_coordinates']) if ROBOT == "sawyer": Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3]) Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) else: Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4]) Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) ## Add the controller here! IF you uncomment below, you should get the checkoff! c = Controller(Kp, Ki, Kd, Kw, Limb('right')) for i in range(10): while not rospy.is_shutdown(): try: if ROBOT == "baxter": x, y, z = .67, -.2, .108 else: raise Exception("not configured for sawyer yet.") goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 # Might have to edit this . . . plan = planner.plan_to_pose(goal_1, []) raw_input( "Press <Enter> to move the right arm to goal pose 1: ") start = time.time() if not c.execute_path(plan): raise Exception("Execution failed") runtime = time.time() - start with open( '/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt', 'a') as f: f.write(str(runtime) + ' move above point \n') except Exception as e: print e traceback.print_exc() else: break while not rospy.is_shutdown(): try: if ROBOT == "baxter": x, y, z = .67, -.2, 0 else: raise Exception("not configured for sawyer yet.") goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = x goal_2.pose.position.y = y goal_2.pose.position.z = z #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 # Might have to edit this . . . plan = planner.plan_to_pose(goal_2, []) raw_input( "Press <Enter> to move the right arm to goal pose 2: ") start = time.time() if not c.execute_path(plan): raise Exception("Execution failed") runtime = time.time() - start with open( '/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt', 'a') as f: f.write(str(runtime) + ' move to point \n') except Exception as e: print e traceback.print_exc() else: break
def main(): """ Examples of how to run me: python scripts/main.py --help <------This prints out all the help messages and describes what each parameter is python scripts/main.py -t 1 -ar 1 -c workspace -a left --log python scripts/main.py -t 2 -ar 2 -c velocity -a left --log python scripts/main.py -t 3 -ar 3 -c torque -a right --log python scripts/main.py -t 1 -ar 4 5 --path_only --log NOTE: If running with the --moveit flag, it makes no sense to also choose your controller to be workspace, since moveit cannot accept workspace trajectories. This script simply ignores the controller selection if you specify both --moveit and --controller_name workspace, so if you want to run with moveit simply leave --controller_name as default. You can also change the rate, timeout if you want """ parser = argparse.ArgumentParser() parser.add_argument('-task', '-t', type=str, default='line', help='Options: line, circle, square. Default: line') parser.add_argument('-ar_marker', '-ar', nargs='+', help='Which AR marker to use. Default: 1') parser.add_argument( '-controller_name', '-c', type=str, default='jointspace', help='Options: workspace, jointspace, or torque. Default: jointspace') parser.add_argument('-arm', '-a', type=str, default='left', help='Options: left, right. Default: left') parser.add_argument('-rate', type=int, default=200, help=""" This specifies how many ms between loops. It is important to use a rate and not a regular while loop because you want the loop to refresh at a constant rate, otherwise you would have to tune your PD parameters if the loop runs slower / faster. Default: 200""") parser.add_argument( '-timeout', type=int, default=None, help= """after how many seconds should the controller terminate if it hasn\'t already. Default: None""") parser.add_argument( '-num_way', type=int, default=300, help= 'How many waypoints for the :obj:`moveit_msgs.msg.RobotTrajectory`. Default: 300' ) parser.add_argument( '--moveit', action='store_true', help= """If you set this flag, moveit will take the path you plan and execute it on the real robot""") parser.add_argument('--log', action='store_true', help='plots controller performance') args = parser.parse_args() rospy.init_node('moveit_node') # this is used for sending commands (velocity, torque, etc) to the robot limb = baxter_interface.Limb(args.arm) # this is used to get the dynamics (inertia matrix, manipulator jacobian, etc) from the robot # in the current position, UNLESS you specify other joint angles. see the source code # https://github.com/valmik/baxter_pykdl/blob/master/src/baxter_pykdl/baxter_pykdl.py # for info on how to use each method kin = baxter_kinematics(args.arm) # ADD COMMENT EHRE tag_pos = [lookup_tag(marker) for marker in args.ar_marker] # Get an appropriate RobotTrajectory for the task (circular, linear, or square) # If the controller is a workspace controller, this should return a trajectory where the # positions and velocities are workspace positions and velocities. If the controller # is a jointspace or torque controller, it should return a trajectory where the positions # and velocities are the positions and velocities of each joint. robot_trajectory = get_trajectory(args.task, limb, kin, tag_pos, args.num_way, args.controller_name) # This is a wrapper around MoveIt! for you to use. We use MoveIt! to go to the start position # of the trajectory planner = PathPlanner('{}_arm'.format(args.arm)) if args.controller_name == "workspace": pose = create_pose_stamped_from_pos_quat( robot_trajectory.joint_trajectory.points[0].positions, [0, 1, 0, 0], 'base') plan = planner.plan_to_pose(pose) else: plan = planner.plan_to_joint_pos( robot_trajectory.joint_trajectory.points[0].positions) planner.execute_plan(plan) if args.moveit: # LAB 1 PART A # by publishing the trajectory to the move_group/display_planned_path topic, you should # be able to view it in RViz. You will have to click the "loop animation" setting in # the planned path section of MoveIt! in the menu on the left side of the screen. pub = rospy.Publisher('move_group/display_planned_path', DisplayTrajectory, queue_size=10) disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_trajectory) # disp_traj.trajectory_start = planner._group.get_current_joint_values() disp_traj.trajectory_start = RobotState() pub.publish(disp_traj) try: raw_input('Press <Enter> to execute the trajectory using MOVEIT') except KeyboardInterrupt: sys.exit() # uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this. # the lines above will display the trajectory in RViz planner.execute_plan(robot_trajectory) else: # LAB 1 PART B controller = get_controller(args.controller_name, limb, kin) try: raw_input( 'Press <Enter> to execute the trajectory using YOUR OWN controller' ) except KeyboardInterrupt: sys.exit() # execute the path using your own controller. done = controller.execute_path(robot_trajectory, rate=args.rate, timeout=args.timeout, log=args.log) if not done: print 'Failed to move to position' sys.exit(0)
def actuate(actual, goals): planner = PathPlanner("right_arm") gripper = robot_gripper.Gripper('right') #Calibrate the gripper (other commands won't work unless you do this first) gripper.clear_calibration() gripper.calibrate() rospy.sleep(2.0) # ## # ## Add the obstacle to the planning scene here ### SHOULD WE ADD PLACED PIECES AS OBSTACLES ???? ## # pose = PoseStamped() # pose.header.frame_id = "base" # pose.pose.position.x = .5 # pose.pose.position.y = 0.0 # pose.pose.position.z = -.2 # pose.pose.orientation.x = 0.0 # pose.pose.orientation.y = 0.0 # pose.pose.orientation.z = 0.0 # pose.pose.orientation.w = 1.0 # planner.add_box_obstacle(np.ndarray(shape=(3,), buffer=np.array([.4, 1.2, .1])), "table", pose) # pose = PoseStamped() # pose.header.frame_id = "base" # pose.pose.position.x = -.5 # pose.pose.position.y = 0.0 # pose.pose.position.z = 0.0 # pose.pose.orientation.x = 0.0 # pose.pose.orientation.y = 0.0 # pose.pose.orientation.z = 0.0 # pose.pose.orientation.w = 1.0 # planner.add_box_obstacle(np.ndarray(shape=(3,), buffer=np.array([.1, 3, 3])), "wall", pose) # #Create a path constraint for the arm # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; # List of letters corresponding to the pieces in the actual configuration actual_list = actual.keys() print(actual_list) # Creation of a copy PoseStamped message to use for z translation above = PoseStamped() above.header.frame_id = "base" raw_input("Press <Enter> to move the right arm to pick up a piece: ") while not rospy.is_shutdown() and len(actual_list) > 0: # Booleans to track state in_hand, picked, placed = False, False, False piece = actual_list[0] ### PICKING UP PIECE FROM ACTUAL LOCATION ### while not rospy.is_shutdown() and not picked: print("PICKING") try: original = actual[piece] print(original) # Setting above's value to correspond to just above original position above.pose.position.x = original.pose.position.x above.pose.position.y = original.pose.position.y above.pose.position.z = original.pose.position.z + 0.2 above.pose.orientation = original.pose.orientation # Pick up the piece in the original position if not in_hand: # Translate over such that the piece is above the actual position plan = planner.plan_to_pose(above, []) if not planner.execute_plan(plan): raise Exception("Execution failed") # Actually pick up the piece plan = planner.plan_to_pose(original, []) if not planner.execute_plan(plan): raise Exception("Execution failed") ## GRIPPER CLOSE (succ) gripper.close() rospy.sleep(2.0) in_hand = True # Raise the arm a little bit so that other pieces are not affected plan = planner.plan_to_pose(above, []) if not planner.execute_plan(plan): raise Exception("Execution failed") ### PLACING PIECE AT DESIRED LOCATION ### text = raw_input("Press <Enter> to move the right arm to place the piece or 'back' to redo the previous step: ") if text == "back": picked = False else: picked = True except Exception as e: print e traceback.print_exc() else: break while not rospy.is_shutdown() and picked and not placed: print("PLACING") try: goal = goals.get(piece)[0] print(goal) # Setting above's value to correspond to just above original position above.pose.position.x = goal.pose.position.x above.pose.position.y = goal.pose.position.y above.pose.position.z = goal.pose.position.z + 0.2 above.pose.orientation = goal.pose.orientation if in_hand: # Translate over such that the piece is above the desired position plan = planner.plan_to_pose(above, []) if not planner.execute_plan(plan): raise Exception("Execution failed") # Actually place the piece on table plan = planner.plan_to_pose(goal, []) if not planner.execute_plan(plan): raise Exception("Execution failed") ## GRIPPER OPEN (release) gripper.open() rospy.sleep(2.0) in_hand = False # Raise the arm a little bit so that other pieces are not affected plan = planner.plan_to_pose(above, []) if not planner.execute_plan(plan): raise Exception("Execution failed") text = raw_input("Press <Enter> to move onto the next piece or 'back': ") if text == "back": placed = False picked = True else: placed = True picked = False goals.get(piece).pop(0) actual_list.pop(0) except Exception as e: print e traceback.print_exc() else: break
class Mover: def __init__(self, box_in): self.box = box_in self.conveyor_z = -0.038 self.mover_setup() def mover_setup(self): limb = Limb("right") # Right arm planner self.planner = PathPlanner("right_arm") # Left arm planner self.planner_left = PathPlanner("left_arm") place_holder = {'images': [], 'camera_infos': []} #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) count = 0 print(limb.endpoint_pose()) calibration_points = [] if ROBOT == "sawyer": Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3]) Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) else: Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4]) Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Controller for right arm self.c = Controller(Kp, Ki, Kd, Kw, Limb('right')) self.orien_const = OrientationConstraint() self.orien_const.link_name = "right_gripper"; self.orien_const.header.frame_id = "base"; self.orien_const.orientation.y = -1.0; self.orien_const.absolute_x_axis_tolerance = 0.1; self.orien_const.absolute_y_axis_tolerance = 0.1; self.orien_const.absolute_z_axis_tolerance = 0.1; self.orien_const.weight = 1.0; box = PoseStamped() box.header.frame_id = "base" box.pose.position.x = self.box.pose.position.x box.pose.position.y = self.box.pose.position.y # box.pose.position.z = self.box.pose.position.z box.pose.position.z = self.conveyor_z # box.pose.position.x = 0.5 # box.pose.position.y = 0.0 # box.pose.position.z = 0.0 box.pose.orientation.x = 0.0 box.pose.orientation.y = 0.0 box.pose.orientation.z = 0.0 box.pose.orientation.w = 1.0 self.planner.add_box_obstacle((100, 100, 0.00001), "box", box) # Controller for left arm self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left')) def move(self, x, y, z): try: print("Moving\n\n\n") print(x, y, z) goal = PoseStamped() goal.header.frame_id = "base" #x, y, and z position goal.pose.position.x = x goal.pose.position.y = y goal.pose.position.z = z #Orientation as a quaternion goal.pose.orientation.x = 0.0 goal.pose.orientation.y = -1.0 goal.pose.orientation.z = 0.0 goal.pose.orientation.w = 0.0 plan = self.planner.plan_to_pose(goal, [self.orien_const]) # TODO: Remove the raw_input for the demo. # x = raw_input("Move?") if not self.c.execute_path(plan): raise Exception("Execution failed") except Exception as e: print(e) traceback.print_exc() def delayed_move(self, x, y, z, delay, time1): try: print("Delayed Move\n\n") print(x, y, z) goal = PoseStamped() goal.header.frame_id = "base" #x, y, and z position goal.pose.position.x = x goal.pose.position.y = y goal.pose.position.z = z #Orientation as a quaternion goal.pose.orientation.x = 0.0 goal.pose.orientation.y = -1.0 goal.pose.orientation.z = 0.0 goal.pose.orientation.w = 0.0 plan = self.planner.plan_to_pose(goal, [self.orien_const]) # Wait till delay has elapsed. while rospy.Time.now().to_sec() < time1 + delay: pass if not self.c.execute_path(plan): raise Exception("Execution failed") except Exception as e: print(e) traceback.print_exc() # Move camera to set position (optional if cannot find a good intermediary pose) def move_camera(self): try: goal = PoseStamped() goal.header.frame_id = "base" # TODO: Update position and orientation values #x, y, and z position goal.pose.position.x = .655 goal.pose.position.y = .21 goal.pose.position.z = .25 #Orientation as a quaternion goal.pose.orientation.x = -.7 goal.pose.orientation.y = .71 goal.pose.orientation.z = -.03 goal.pose.orientation.w = -.02 plan = self.planner.plan_to_pose(goal) # TODO: Remove the raw_input for the demo. x = raw_input("Move?") if not self.c.execute_path(plan): raise Exception("Execution failed") except Exception as e: print(e) traceback.print_exc()
class Actuator(object): def __init__(self, sub_topic, pub_topic): self.planner = None self.limb = None self.executed = False self.planner = PathPlanner("right_arm") self.limb = Limb("right") # self.orien_const = OrientationConstraint() # self.orien_const.link_name = "right_gripper" # self.orien_const.header.frame_id = "base" # self.orien_const.orientation.z = 0 # self.orien_const.orientation.y = 1 # self.orien_const.orientation.w = 0 # self.orien_const.absolute_x_axis_tolerance = 0.1 # self.orien_const.absolute_y_axis_tolerance = 0.1 # self.orien_const.absolute_z_axis_tolerance = 0.1 # self.orien_const.weight = 1.0 size = [1, 2, 2] obstacle_pose = PoseStamped() obstacle_pose.header.frame_id = "base" obstacle_pose.pose.position.x = -1 obstacle_pose.pose.position.y = 0 obstacle_pose.pose.position.z = 0 obstacle_pose.pose.orientation.x = 0 obstacle_pose.pose.orientation.y = 0 obstacle_pose.pose.orientation.z = 0 obstacle_pose.pose.orientation.w = 1 self.planner.add_box_obstacle(size, "wall", obstacle_pose) size = [.75, 1, 1] obstacle_pose = PoseStamped() obstacle_pose.header.frame_id = "base" obstacle_pose.pose.position.x = 1 obstacle_pose.pose.position.y = 0 obstacle_pose.pose.position.z = -.4 obstacle_pose.pose.orientation.x = 0 obstacle_pose.pose.orientation.y = 0 obstacle_pose.pose.orientation.z = 0 obstacle_pose.pose.orientation.w = 1 self.planner.add_box_obstacle(size, "table", obstacle_pose) self.pub = rospy.Publisher(pub_topic, Bool) self.sub = rospy.Subscriber(sub_topic, PoseStamped, self.callback) def callback(self, goal): while not self.executed: # if goal.pose.position.y <= 0: # self.planner = PathPlanner("right_arm") # self.limb = Limb("right") # else: # self.planner = PathPlanner("left_arm") # self.limb = Limb("left") try: plan = self.planner.plan_to_pose(goal, []) raw_input("Press <Enter> to execute plan") if not self.planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: self.executed = True print("Execution succeeded! Release the ball when ready!") break self.pub.publish(True)
# If the controller is a workspace controller, this should return a trajectory where the # positions and velocities are workspace positions and velocities. If the controller # is a jointspace or torque controller, it should return a trajectory where the positions # and velocities are the positions and velocities of each joint. robot_trajectory = get_trajectory(args.task, current_pos, tag_pos, args.num_way, args.controller_name, limb, kin, args.rate) # This is a wrapper around MoveIt! for you to use. We use MoveIt! to go to the start position # of the trajectory planner = PathPlanner('{}_arm'.format(args.arm)) if args.controller_name == "workspace": pose = create_pose_stamped_from_pos_quat( robot_trajectory.joint_trajectory.points[0].positions, [0, 1, 0, 0], 'base') plan = planner.plan_to_pose(pose) else: plan = planner.plan_to_joint_pos( robot_trajectory.joint_trajectory.points[0].positions) planner.execute_plan(plan) if args.moveit: # LAB 1 PART A # by publishing the trajectory to the move_group/display_planned_path topic, you should # be able to view it in RViz. You will have to click the "loop animation" setting in # the planned path section of MoveIt! in the menu on the left side of the screen. pub = rospy.Publisher('move_group/display_planned_path', DisplayTrajectory, queue_size=10) disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_trajectory)
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned ## ## Add the obstacle to the planning scene here ## # Create a path constraint for the arm orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 # obs1 = PoseStamped() # obs1.pose.position.x = 0.5 # obs1.pose.position.y = 0.0 # obs1.pose.position.z = -0.3 # obs1.pose.orientation.x = 0.0 # obs1.pose.orientation.y = 0.0 # obs1.pose.orientation.z = 0.0 # obs1.pose.orientation.w = 1.0 # obs1.header.frame_id = "base" # planner.add_box_obstacle([0.4, 1.2, 0.1], 'box', obs1) # obs2 = PoseStamped() # obs2.pose.position.x = 0.5 # obs2.pose.position.y = 0.0 # obs2.pose.position.z = -0.25 # obs2.pose.orientation.x = 1.0 # obs2.pose.orientation.y = 1.0 # obs2.pose.orientation.z = 1.0 # obs2.pose.orientation.w = 1.0 # obs2.header.frame_id = "base" # planner.add_box_obstacle([0.4, 1.2, 0.1], 'box', obs2) raw_input("Press <Enter> to move the right arm to goal pose 1: ") while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.68 goal_1.pose.position.y = 0.73 goal_1.pose.position.z = -0.03 #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 cont = controller.Controller(Kp, Kd, Ki, Kw, Limb('right')) plan = planner.plan_to_pose(goal_1, list()) if not cont.execute_path(plan): # if not planner.execute_plan(plan): raise ZeroDivisionError("Execution failed") except ZeroDivisionError as e: raise e else: break
def draw(top, bot, board): Q = homogenous(top.T, board) top = np.dot(Q, top.T).T bot = np.dot(Q, bot.T).T planner = PathPlanner('right_arm') Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned controller = Controller(Kp, Kd, Ki, Kw, Limb('right')) orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 ori = PoseStamped() ori.header.frame_id = 'base' ori.pose.orientation.x = 0.0 ori.pose.orientation.y = -1.0 ori.pose.orientation.z = 0.0 ori.pose.orientation.w = 0.0 goals = np.empty(len(top) + len(bot), dtype=object) while not rospy.is_shutdown(): try: counter = 0 count_top = 0 count_bot = len(bot) - 1 while count_top < len(top) or count_bot >= 0: if count_top < len(top): goal = PoseStamped() goal.header.frame_id = "base" goal.pose.position.x = top[count_top][0] / top[count_top][ 2] - 0.014 + .012 goal.pose.position.y = top[count_top][1] / top[count_top][ 2] + 0.005 - 0.04 goal.pose.position.z = -0.1849 goals[counter] = goal counter += 1 count_top += 1 if count_bot >= 0: goal = PoseStamped() goal.header.frame_id = "base" goal.pose.position.x = bot[count_bot][0] / bot[count_bot][ 2] + 0.003 goal.pose.position.y = bot[count_bot][1] / bot[count_bot][ 2] - 0.01 goal.pose.position.z = -0.1861 goals[counter] = goal counter += 1 count_bot -= 1 for i in goals: i.pose.orientation = ori.pose.orientation plan = planner.plan_to_pose(i, list()) if not controller.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def main(): planner = PathPlanner("left_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned # #Create a path constraint for the arm orien_const = OrientationConstraint() orien_const.link_name = "left_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 filename = "test_cup.jpg" table_height = 0 # get positions of cups cups = find_cups(filename) start_cup = cups[0] goal_cup = cups[1] cup_pos = [0.756, 0.300, -0.162] # position of cup goal_pos = [0.756, 0.600, -0.162] # position of goal cup init_pos = [cup_pos[0] - .2, cup_pos[1], cup_pos[2] + .05] # initial position start_pos = [init_pos[0] + .2, init_pos[1], init_pos[2]] # start position, pick up cup up_pos = [goal_pos[0], goal_pos[1] - .05, goal_pos[2] + .1] # up position, ready to tilt end_pos = start_pos # end position, put down cup final_pos = init_pos # final position, away from cup # add table obstacle table_size = np.array([.4, .8, .1]) table_pose = PoseStamped() table_pose.header.frame_id = "base" #x, y, and z position table_pose.pose.position.x = 0.5 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = 0.0 planner.add_box_obstacle(table_size, 'table', table_pose) # add goal cup obstacle goal_cup_size = np.array([.1, .1, .2]) goal_cup_pose = PoseStamped() goal_cup_pose.header.frame_id = "base" #x, y, and z position goal_cup_pose.pose.position.x = goal_pos[0] goal_cup_pose.pose.position.y = goal_pos[1] goal_cup_pose.pose.position.z = goal_pos[2] planner.add_box_obstacle(goal_cup_size, 'goal cup', goal_cup_pose) # add first cup obstacle cup_size = np.array([.1, .1, .2]) cup_pose = PoseStamped() cup_pose.header.frame_id = "base" #x, y, and z position cup_pose.pose.position.x = cup_pos[0] cup_pose.pose.position.y = cup_pos[1] cup_pose.pose.position.z = cup_pos[2] planner.add_box_obstacle(cup_size, 'cup', cup_pose) while not rospy.is_shutdown(): left_gripper = robot_gripper.Gripper('left') print('Calibrating...') left_gripper.calibrate() pose = PoseStamped() pose.header.frame_id = "base" #Orientation as a quaternion pose.pose.orientation.x = 0.0 pose.pose.orientation.y = -1.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 0.0 while not rospy.is_shutdown(): try: #x, y, and z position pose.pose.position.x = init_pos[0] pose.pose.position.y = init_pos[1] pose.pose.position.z = init_pos[2] plan = planner.plan_to_pose(pose, [orien_const]) raw_input( "Press <Enter> to move the left arm to initial pose: ") if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break # grab cup # need to make sure it doesn't knock it over planner.remove_obstacle('cup') while not rospy.is_shutdown(): try: #x, y, and z position pose.pose.position.x = start_pos[0] pose.pose.position.y = start_pos[1] pose.pose.position.z = start_pos[2] plan = planner.plan_to_pose(pose, [orien_const]) raw_input( "Press <Enter> to move the left arm to grab the cup: ") if not planner.execute_plan(plan): raise Exception("Execution failed") print('Closing...') left_gripper.close() rospy.sleep(2) except Exception as e: print e else: break # position to pour while not rospy.is_shutdown(): try: #x, y, and z position pose.pose.position.x = up_pos[0] pose.pose.position.y = up_pos[1] pose.pose.position.z = up_pos[2] plan = planner.plan_to_pose(pose, [orien_const]) raw_input( "Press <Enter> to move the left arm to above the goal cup: " ) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break # pouring raw_input("Press <Enter> to move the left arm to begin pouring: ") for degree in range(180): while not rospy.is_shutdown(): try: #Orientation as a quaternion pose.pose.orientation.x = 0.0 pose.pose.orientation.y = -1.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 0.0 plan = planner.plan_to_pose(pose, []) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break # move cup away from goal on table while not rospy.is_shutdown(): try: #x, y, and z position pose.pose.position.x = end_pos[0] pose.pose.position.y = end_pos[1] pose.pose.position.z = end_pos[2] plan = planner.plan_to_pose(pose, [orien_const]) raw_input( "Press <Enter> to move the left arm to away from the goal cup: " ) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break # let go of cup on table # need to make sure to not to hit cup while not rospy.is_shutdown(): try: #x, y, and z position pose.pose.position.x = final_pos[0] pose.pose.position.y = final_pos[1] pose.pose.position.z = final_pos[2] plan = planner.plan_to_pose(pose, [orien_const]) raw_input( "Press <Enter> to move the left arm to let go of the cup: " ) print('Opening...') left_gripper.open() rospy.sleep(1.0) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break # get new cup position new_cup_pos = [] # readd the cup obstacle cup_pose.pose.position.x = new_cup_pos[0] cup_pose.pose.position.y = new_cup_pos[1] cup_pose.pose.position.z = new_cup_pos[2] planner.add_box_obstacle(cup_size, 'cup', cup_pose)
def move(): planner = PathPlanner('right_arm') Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned controller = Controller(Kp, Kd, Ki, Kw, Limb('right')) ori = PoseStamped() ori.header.frame_id = 'base' ori.pose.orientation.x = 0.0 ori.pose.orientation.y = -1.0 ori.pose.orientation.z = 0.0 ori.pose.orientation.w = 0.0 diff = 0 observe(150000) ready(20000) #draw(planner, controller, ori) while not rospy.is_shutdown(): for _ in range(10): while not rospy.is_shutdown(): try: #rosrun tf tf_echo base right_gripper_tip goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.65 goal_1.pose.position.y = 0.02 + diff goal_1.pose.position.z = -0.227 #Orientation as a quaternion goal_1.pose.orientation = ori.pose.orientation plan1 = planner.plan_to_pose(goal_1, list()) if not controller.execute_path(plan1): raise Exception("Execution failed") goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.80 goal_2.pose.position.y = -0.01 + diff goal_2.pose.position.z = -0.227 #should be a little bit lower than initial point #Orientation as a quaternion goal_2.pose.orientation = ori.pose.orientation plan2 = planner.plan_to_pose(goal_2, list()) diff += .004 #raw_input("Press <Enter>") if not controller.execute_path(plan2): raise Exception("Execution failed") except Exception as e: print e else: break observe(50000) ready(20000)
def main(): """ Main Script """ use_orien_const = False input = raw_input("use orientation constraint? y/n\n") if input == 'y' or input == 'yes': use_orien_const = True print("using orientation constraint") elif input == 'n' or input == 'no': use_orien_const = False print("not using orientation constraint") else: print("input error, not using orientation constraint as default") add_box = False input = raw_input("add_box? y/n\n") if input == 'y' or input == 'yes': add_box = True print("add_box") elif input == 'n' or input == 'no': add_box = False print("not add_box") else: print("input error, not add_box as default") open_loop_contro = False input = raw_input("using open loop controller? y/n\n") if input == 'y' or input == 'yes': open_loop_contro = True print("using open loop controller") elif input == 'n' or input == 'no': open_loop_contro = False print("not using open loop controller") else: print("input error, not using open loop controller as default") # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned # joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'] my_controller = Controller(Kp, Kd, Ki, Kw, Limb("right")) ## ## Add the obstacle to the planning scene here ## if add_box: name = "obstacle_1" size = np.array([0.4, 1.2, 0.1]) pose = PoseStamped() pose.header.frame_id = "base" #x, y, and z position pose.pose.position.x = 0.5 pose.pose.position.y = 0.0 pose.pose.position.z = 0.0 #Orientation as a quaternion pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 1.0 planner.add_box_obstacle(size, name, pose) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.4 goal_1.pose.position.y = -0.3 goal_1.pose.position.z = 0.2 #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 if not use_orien_const: plan = planner.plan_to_pose(goal_1, list()) else: plan = planner.plan_to_pose(goal_1, [orien_const]) raw_input( "Press <Enter> to move the right arm to goal pose 1: ") if open_loop_contro: if not my_controller.execute_path(plan): raise Exception("Execution failed") else: if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.6 goal_2.pose.position.y = -0.3 goal_2.pose.position.z = 0.0 #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 # plan = planner.plan_to_pose(goal_2, list()) if not use_orien_const: plan = planner.plan_to_pose(goal_2, list()) else: plan = planner.plan_to_pose(goal_2, [orien_const]) raw_input( "Press <Enter> to move the right arm to goal pose 2: ") if open_loop_contro: if not my_controller.execute_path(plan): raise Exception("Execution failed") else: if not planner.execute_plan(plan): raise Exception("Execution failed") # if not my_controller.execute_path(plan): # raise Exception("Execution failed") # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.6 goal_3.pose.position.y = -0.1 goal_3.pose.position.z = 0.1 #Orientation as a quaternion goal_3.pose.orientation.x = 0.0 goal_3.pose.orientation.y = -1.0 goal_3.pose.orientation.z = 0.0 goal_3.pose.orientation.w = 0.0 # plan = planner.plan_to_pose(goal_3, list()) if not use_orien_const: plan = planner.plan_to_pose(goal_3, list()) else: plan = planner.plan_to_pose(goal_3, [orien_const]) raw_input( "Press <Enter> to move the right arm to goal pose 3: ") if open_loop_contro: if not my_controller.execute_path(plan): raise Exception("Execution failed") else: if not planner.execute_plan(plan): raise Exception("Execution failed") # if not my_controller.execute_path(plan): # raise Exception("Execution failed") # if not planner.execute_plan(plan): # raise Exception("Execution failed") except Exception as e: print e else: break
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned ## ## Add the obstacle to the planning scene here ## # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; #hardcodeDESE COORDINATE VALUES #IN THE VIEW OF THE CAMERA #CORNER1--------->ORNER2 # | | # | | # | | #CORNER3 ------------| CORNER1 = CORNER2 = CORNER3 = #CREATE THE GRID dir1 = CORNER2 - CORNER1 dir2 = CONRER3 - CORNER1 grid_vals = [] for i in range(0, 4): for j in range(0, 4): grid = CORNER1 + i * dir1 / 3 + j * dir2 / 3 grid_vals.append(grid) while not rospy.is_shutdown(): for g in grid_vals: while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = g[0] goal_1.pose.position.y = g[1] goal_1.pose.position.z = g[2] #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = 0.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 1 plan = planner.plan_to_pose(goal_1, list()) raw_input("Press <Enter> to move the right arm to goal: " + "x = " + str(g[0]) + "y = " + str(g[1]) + " z = " + str(g[2])) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def main(list_of_class_objs, basket_coordinates, planner_left): """ Main Script input:first argument: a list of classX_objs. Ex: [class1_objs, class2_objs] where class1_objs contains the same kind of fruits second argument: a list of baskets coordinates """ # Make sure that you've looked at and understand path_planner.py before starting planner_left = PathPlanner("left_arm") home_coordinates = [0.544, 0.306, 0.3] home = PoseStamped() home.header.frame_id = "base" #home x,y,z position home_x = home_coordinates[0] home_y = home_coordinates[1] home_z = home_coordinates[2] home.pose.position.x = home_x home.pose.position.y = home_y home.pose.position.z = home_z #Orientation as a quaternion home.pose.orientation.x = 1.0 home.pose.orientation.y = 0.0 home.pose.orientation.z = 0.0 home.pose.orientation.w = 0.0 intermediate_obstacle = PoseStamped() intermediate_obstacle.header.frame_id = "base" intermediate_obstacle.pose.position.x = 0 intermediate_obstacle.pose.position.y = 0 intermediate_obstacle.pose.position.z = 0.764 intermediate_obstacle.pose.orientation.x = 1 intermediate_obstacle.pose.orientation.y = 0 intermediate_obstacle.pose.orientation.z = 0 intermediate_obstacle.pose.orientation.w = 0 intermediate_size = [1, 1, 0.2] left_gripper = robot_gripper.Gripper('left') print('Calibrating...') left_gripper.calibrate() while not rospy.is_shutdown(): try: #GO HOME plan = planner_left.plan_to_pose(home, list()) raw_input("Press <Enter> to move the left arm to home position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break for i, classi_objs in enumerate(list_of_class_objs): #x, y,z, orientation of class(basket) print("processing class: " + str(i)) classi_position = basket_coordinates[i] classi = PoseStamped() classi.header.frame_id = "base" classi_x = classi_position[0] classi_y = classi_position[1] classi_z = classi_position[2] classi.pose.position.x = classi_x classi.pose.position.y = classi_y classi.pose.position.z = classi_z +.1 classi.pose.orientation.x = 1.0 classi.pose.orientation.y = 0.0 classi.pose.orientation.z = 0.0 classi.pose.orientation.w = 0.0 for fruit in classi_objs: gripper_yaw = fruit[3] fruit_x = fruit[0] fruit_y = fruit[1] fruit_z = fruit[2] gripper_orientation = euler_to_quaternion(gripper_yaw) orien_const = OrientationConstraint() orien_const.link_name = "left_gripper"; orien_const.header.frame_id = "base"; gripper_orientation_x = gripper_orientation[0] gripper_orientation_y = gripper_orientation[1] gripper_orientation_z = gripper_orientation[2] gripper_orientation_w = gripper_orientation[3] orien_const.orientation.x = gripper_orientation_x orien_const.orientation.y = gripper_orientation_y orien_const.orientation.z = gripper_orientation_z orien_const.orientation.w = gripper_orientation_w orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 print('Opening...') left_gripper.open() rospy.sleep(1.0) while not rospy.is_shutdown(): try: planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose) #intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper intermidiate_to_fruit = PoseStamped() intermidiate_to_fruit.header.frame_id = "base" #x,y,z position intermidiate_to_fruit.pose.position.x = fruit_x intermidiate_to_fruit.pose.position.y = fruit_y intermidiate_to_fruit.pose.position.z = home_z - .1 print("Trying to reach intermeidiate_to_fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z)) intermidiate_to_fruit.pose.orientation.x = gripper_orientation_x intermidiate_to_fruit.pose.orientation.y = gripper_orientation_y intermidiate_to_fruit.pose.orientation.z = gripper_orientation_z intermidiate_to_fruit.pose.orientation.w = gripper_orientation_w plan = planner_left.plan_to_pose(intermidiate_to_fruit, list()) raw_input("Press <Enter> to move the left arm to intermidiate_to_fruit position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") planner_left.remove_obstacle("intermediate") except Exception as e: print e else: break while not rospy.is_shutdown(): try: #go down to the actual height of the fruit and close gripper fruitobs = generate_obstacle(fruit_x, fruit_y) fruit = PoseStamped() fruit.header.frame_id = "base" #x,y,z position fruit.pose.position.x = fruit_x fruit.pose.position.y = fruit_y fruit.pose.position.z = fruit_z print("Trying to reach fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z)) #Orientation as a quaternion fruit.pose.orientation.x = gripper_orientation_x fruit.pose.orientation.y = gripper_orientation_y fruit.pose.orientation.z = gripper_orientation_z fruit.pose.orientation.w = gripper_orientation_w plan = planner_left.plan_to_pose(fruit, [orien_const]) raw_input("Press <Enter> to move the left arm to fruit position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") fruitobs() except Exception as e: print e else: break #close the gripper print('Closing...') left_gripper.close() rospy.sleep(1.0) while not rospy.is_shutdown(): try: fruitobs = generate_obstacle(fruit_x, fruit_y) #intermidiate_to_basket stage1: Lift up to a height higher than the height of the basket firt_intermidiate_to_class_i = PoseStamped() firt_intermidiate_to_class_i.header.frame_id = "base" #x, y, and z position firt_intermidiate_to_class_i.pose.position.x = fruit_x firt_intermidiate_to_class_i.pose.position.y = fruit_y firt_intermidiate_to_class_i.pose.position.z = classi_z + 0.25 print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2)) #Orientation as a quaternion firt_intermidiate_to_class_i.pose.orientation.x = 1.0 firt_intermidiate_to_class_i.pose.orientation.y = 0.0 firt_intermidiate_to_class_i.pose.orientation.z = 0.0 firt_intermidiate_to_class_i.pose.orientation.w = 0.0 plan = planner_left.plan_to_pose(firt_intermidiate_to_class_i, list()) raw_input("Press <Enter> to move the left arm to first_intermidiate_to_class_" + str(i) + "position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") fruitobs() except Exception as e: print e else: break while not rospy.is_shutdown(): try: planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose) #intermidiate_to_basket stage2: Move to the top of the basket intermidiate_to_class_i = PoseStamped() intermidiate_to_class_i.header.frame_id = "base" #x, y, and z position intermidiate_to_class_i.pose.position.x = classi_x intermidiate_to_class_i.pose.position.y = classi_y intermidiate_to_class_i.pose.position.z = classi_z + 0.2 print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2)) #Orientation as a quaternion intermidiate_to_class_i.pose.orientation.x = 1.0 intermidiate_to_class_i.pose.orientation.y = 0.0 intermidiate_to_class_i.pose.orientation.z = 0.0 intermidiate_to_class_i.pose.orientation.w = 0.0 plan = planner_left.plan_to_pose(intermidiate_to_class_i, list()) raw_input("Press <Enter> to move the left arm to second_intermidiate_to_class_" + str(i) + "position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") planner_left.remove_obstacle("intermediate") except Exception as e: print e else: break while not rospy.is_shutdown(): try: #basket stage: put the fruit in the basket classi_obs = generate_obstacle(classi_x, class_y) plan = planner_left.plan_to_pose(classi, list()) raw_input("Press <Enter> to move the left arm to sclass_" + str(i) + "position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") classi_obs() except Exception as e: print e else: break #Open the gripper print('Opening...') left_gripper.open() rospy.sleep(1.0) while not rospy.is_shutdown(): try: #intermidiate to home stage: lifting up to the home position height intermidiate_to_home_1 = PoseStamped() intermidiate_to_home_1.header.frame_id = "base" #x, y, and z position intermidiate_to_home_1.pose.position.x = classi_x intermidiate_to_home_1.pose.position.y = classi_y intermidiate_to_home_1.pose.position.z = home_z #Orientation as a quaternion intermidiate_to_home_1.pose.orientation.x = 1.0 intermidiate_to_home_1.pose.orientation.y = 0.0 intermidiate_to_home_1.pose.orientation.z = 0.0 intermidiate_to_home_1.pose.orientation.w = 0.0 plan = planner_left.plan_to_pose(intermidiate_to_home_1, list()) raw_input("Press <Enter> to move the right arm to intermidiate_to_home_1 position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break planner_left.shutdown()
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned controller = Controller(Kp, Kd, Ki, Kw, Limb('right')) ## ## Add the obstacle to the planning scene here bagel = PoseStamped() bagel.header.frame_id = 'base' #x, y, and z position bagel.pose.position.x = 0.5 bagel.pose.position.y = 0.0 bagel.pose.position.z = 0.0 #Orientation as a quaternion bagel.pose.orientation.x = 0.0 bagel.pose.orientation.y = 0.0 bagel.pose.orientation.z = 0.0 bagel.pose.orientation.w = 1.0 planner.add_box_obstacle(np.array([0.4, 1.2, 0.1]), 'bagel', bagel) #Create a path constraint for the arm #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART orien_const = OrientationConstraint() orien_const.link_name = "right_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.y = -1.0; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.4 goal_1.pose.position.y = -0.3 goal_1.pose.position.z = 0.2 #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_1, list()) raw_input("Press <Enter> to move the right arm to goal pose 1: ") if not controller.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.6 goal_2.pose.position.y = -0.3 goal_2.pose.position.z = 0.0 #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_2, list()) raw_input("Press <Enter> to move the right arm to goal pose 2: ") if not controller.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.6 goal_3.pose.position.y = -0.1 goal_3.pose.position.z = 0.1 #Orientation as a quaternion goal_3.pose.orientation.x = 0.0 goal_3.pose.orientation.y = -1.0 goal_3.pose.orientation.z = 0.0 goal_3.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_3, list()) raw_input("Press <Enter> to move the right arm to goal pose 3: ") if not controller.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned limb = intera_interface.Limb("right") control = Controller(Kp, Kd, Ki, Kw, limb) ## ## Add the obstacle to the planning scene here ## X = 0.40 Y = 1.20 Z = 0.10 Xp = 0.5 Yp = 0.00 Zp = -0.15 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa #planner.add_box_obstacle([X,Y,Z], "wall", pose) #Wall 2 X = 0.40 Y = 0.10 Z = 0.30 Xp = 0.5 Yp = -0.15 Zp = 0.05 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa #planner.add_box_obstacle([X,Y,Z], "wall2", pose) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART orien_const = OrientationConstraint() orien_const.link_name = "right_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.y = -1.0; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.4 goal_1.pose.position.y = -0.3 goal_1.pose.position.z = 0.2 #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_1, list()) raw_input("Press <Enter> to move the right arm to goal pose 1: ") if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.6 goal_2.pose.position.y = -0.3 goal_2.pose.position.z = 0.0 #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_2, [orien_const]) raw_input("Press <Enter> to move the right arm to goal pose 2: ") if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.6 goal_3.pose.position.y = -0.1 goal_3.pose.position.z = 0.1 #Orientation as a quaternion goal_3.pose.orientation.x = 0.0 goal_3.pose.orientation.y = -1.0 goal_3.pose.orientation.z = 0.0 goal_3.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_3, [orien_const]) raw_input("Press <Enter> to move the right arm to goal pose 3: ") if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break
calib1 = PoseStamped() calib1.header.frame_id = "base" #x,y,z position calib1.pose.position.x = 0.35 calib1.pose.position.y = 0.75 calib1.pose.position.z = -.23 calib1.pose.orientation.x = 1 calib1.pose.orientation.y = 0 calib1.pose.orientation.z = 0 calib1.pose.orientation.w = 0 plan = planner_left.plan_to_pose(calib1, obstacles) raw_input("Press <Enter> to move the left arm to calib1 position: ") if not planner_left.execute_plan(plan): raise Exception("Execution failed") else: break except Exception as e: print e while not rospy.is_shutdown(): try: #intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper calib1 = PoseStamped() calib1.header.frame_id = "base"
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") if ROBOT == "sawyer": Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3]) Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) else: Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4]) Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) ## ## Add the obstacle to the planning scene here ## # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: if ROBOT == "baxter": x, y, z = 0.47, -0.85, 0.07 else: x, y, z = 0.8, 0.05, 0.07 goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 # Might have to edit this . . . plan = planner.plan_to_pose(goal_1, []) raw_input( "Press <Enter> to move the right arm to goal pose 1: ") if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e traceback.print_exc() else: break while not rospy.is_shutdown(): try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.6 goal_2.pose.position.y = -0.3 goal_2.pose.position.z = 0.0 #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_2, []) raw_input( "Press <Enter> to move the right arm to goal pose 2: ") if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.6 goal_3.pose.position.y = -0.1 goal_3.pose.position.z = 0.1 #Orientation as a quaternion goal_3.pose.orientation.x = 0.0 goal_3.pose.orientation.y = -1.0 goal_3.pose.orientation.z = 0.0 goal_3.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_3, []) raw_input( "Press <Enter> to move the right arm to goal pose 3: ") if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def main(): global prev_msg global sec_pre plandraw = PathPlanner('right_arm') # plandraw.grip?per_op plandraw.start_position() ## BOX box_size = np.array([2.4, 2.4, 0.1]) box_pose = PoseStamped() box_pose.header.stamp = rospy.Time.now() box_pose.header.frame_id = "base" box_pose.pose.position.x = 0 box_pose.pose.position.y = 0 box_pose.pose.position.z = -0.4 box_pose.pose.orientation.x = 0.00 box_pose.pose.orientation.y = 0.00 box_pose.pose.orientation.z = 0.00 box_pose.pose.orientation.w = 1.00 plandraw.add_box_obstacle(box_size, "box1", box_pose) box_size2 = np.array([2.4, 2.4, 0.1]) box_pose2 = PoseStamped() box_pose2.header.stamp = rospy.Time.now() box_pose2.header.frame_id = "base" box_pose2.pose.position.x = 0 box_pose2.pose.position.y = 0 box_pose2.pose.position.z = 1 box_pose2.pose.orientation.x = 0.00 box_pose2.pose.orientation.y = 0.00 box_pose2.pose.orientation.z = 0.00 box_pose2.pose.orientation.w = 1.00 plandraw.add_box_obstacle(box_size2, "box2", box_pose2) orien_const = OrientationConstraint() orien_const.link_name = "right_gripper_tip" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 def set_use_pen(pen_id, goal_1): if pen_id == 0: goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -0.9848078 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = -0.1736482 if pen_id == 1: goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 if pen_id == 2: goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -0.9848078 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.1736482 waypoints = [] while not rospy.is_shutdown(): #raw_input("~~~~~~~~~~~~!!!!!!!!!!!!") while not rospy.is_shutdown(): try: while len(queue): print(len(queue)) cur = queue.popleft() x, y, z = cur.position_x, cur.position_y, cur.position_z x -= 0.085 # ada different coordinate z += 0.03 if cur.status_type != "edge_grad": # ti bi !!!!! luo bi !!!! if cur.status_type == "starting": print("start") # waypoints = [] goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z + 0.12 #Orientation as a quaternion # [0.766, -0.623, 0.139, -0.082] # [-0.077408, 0.99027, -0.024714, ] set_use_pen(cur.pen_type, goal_1) waypoints.append(copy.deepcopy(goal_1.pose)) goal_1.pose.position.z -= 0.12 waypoints.append(copy.deepcopy(goal_1.pose)) # plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints) # if not plandraw.execute_plan(plan): # raise Exception("Starting execution failed") # else: # queue.pop(0) elif cur.status_type == "next_point": print("next") goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion # goal_1.pose.orientation.x = 0.459962 # goal_1.pose.orientation.y = -0.7666033 # goal_1.pose.orientation.z = 0.0 # goal_1.pose.orientation.w = -0.4480562 set_use_pen(cur.pen_type, goal_1) # waypoints = [] waypoints.append(copy.deepcopy(goal_1.pose)) # plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints) # if not plandraw.execute_plan(plan): # raise Exception("Execution failed, point is ", cur) # else: # queue.pop(0) elif cur.status_type == "ending": print("ppppppp ", cur) # mmm = plandraw.get_cur_pos().pose goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z + 0.12 #Orientation as a quaternion set_use_pen(1, goal_1) # waypoints = [] waypoints.append(copy.deepcopy(goal_1.pose)) plan = plandraw.plan_to_pose(goal_1, [], waypoints) waypoints = [] # queue.pop(0) if not plandraw.execute_plan(plan): raise Exception("Execution failed") print("ti bi") # rospy.sleep(5) #raw_input("Press <Enter> to move next!!!") except Exception as e: print e else: #print("lllllllllllllllllllll") break
def play_song(note_pos): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned z_offset = 0.05 ## ## Add the obstacle to the planning scene here ## # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; # # Note 1 for testing # note1 = Vector3() # note1.x = 0.6 # note1.y = -0.3 # note1.z = 0.1 # # Note 2 for testing # note2 = Vector3() # note2.x = 0.6 # note2.y = -0.25 # note2.z = 0.1 # while not rospy.is_shutdown(): # Iterate through all note positions for the song for i, pos in enumerate(note_pos): # Loop until that position is reached while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = pos.x goal_1.pose.position.y = pos.y goal_1.pose.position.z = pos.z #Orientation as a quaternion, facing straight down goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal_1, list()) # raw_input("Press <Enter> to move the right arm to goal pose {}: ".format(i)) if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting plannerR = PathPlanner("right_arm") plannerL = PathPlanner("left_arm") right_gripper = robot_gripper.Gripper('right') left_gripper = robot_gripper.Gripper('left') # Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students # Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students # Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned # Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned # right_arm = Limb("right") # controller = Controller(Kp, Kd, Ki, Kw, right_arm) ## ## Add the obstacle to the planning scene here ## obstacle1 = PoseStamped() obstacle1.header.frame_id = "base" #x, y, and z position obstacle1.pose.position.x = 0.4 obstacle1.pose.position.y = 0 obstacle1.pose.position.z = -0.29 #Orientation as a quaternion obstacle1.pose.orientation.x = 0.0 obstacle1.pose.orientation.y = 0.0 obstacle1.pose.orientation.z = 0.0 obstacle1.pose.orientation.w = 1 plannerR.add_box_obstacle(np.array([1.2, 1.2, .005]), "tableBox", obstacle1) # obstacle2 = PoseStamped() # obstacle2.header.frame_id = "wall" # #x, y, and z position # obstacle2.pose.position.x = 0.466 # obstacle2.pose.position.y = -0.670 # obstacle2.pose.position.z = -0.005 # #Orientation as a quaternion # obstacle2.pose.orientation.x = 0.694 # obstacle2.pose.orientation.y = -0.669 # obstacle2.pose.orientation.z = 0.251 # obstacle2.pose.orientation.w = -0.092 # planner.add_box_obstacle(np.array([0.01, 2, 2]), "wall", obstacle2) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART # orien_const = OrientationConstraint() # orien_const.link_name = "right_gripper"; # orien_const.header.frame_id = "base"; # orien_const.orientation.y = -1.0; # orien_const.absolute_x_axis_tolerance = 0.1; # orien_const.absolute_y_axis_tolerance = 0.1; # orien_const.absolute_z_axis_tolerance = 0.1; # orien_const.weight = 1.0; while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.440 goal_3.pose.position.y = -0.012 goal_3.pose.position.z = 0.549 #Orientation as a quaternion goal_3.pose.orientation.x = -0.314 goal_3.pose.orientation.y = -0.389 goal_3.pose.orientation.z = 0.432 goal_3.pose.orientation.w = 0.749 #plan = plannerR.plan_to_pose(goal_3, list()) raw_input( "Press <Enter> to move the right arm to goal pose 3: ") if not plannerR.execute_plan( plannerR.plan_to_pose(goal_3, list())): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.448 goal_2.pose.position.y = -0.047 goal_2.pose.position.z = -0.245 #Orientation as a quaternion goal_2.pose.orientation.x = 0 goal_2.pose.orientation.y = 1 goal_2.pose.orientation.z = 0 goal_2.pose.orientation.w = 0 #plan = plannerR.plan_to_pose(goal_2, list()) raw_input( "Press <Enter> to move the right arm to goal pose 2: ") if not plannerR.execute_plan( plannerR.plan_to_pose(goal_2, list())): raise Exception("Execution failed") except Exception as e: print e else: break while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.448 goal_1.pose.position.y = -0.047 goal_1.pose.position.z = -0.245 #Orientation as a quaternion goal_1.pose.orientation.x = 1 goal_1.pose.orientation.y = 0 goal_1.pose.orientation.z = 0 goal_1.pose.orientation.w = 0 # Might have to edit this . . . #plan = plannerR.plan_to_pose(goal_1, list()) # put orien_const here raw_input( "Press <Enter> to move the right arm to goal pose 1: ") if not plannerR.execute_plan( plannerR.plan_to_pose(goal_1, list())): raise Exception("Execution failed") except Exception as e: print e else: break
def move(x, y, z, box_in): planner = PathPlanner("right_arm") #TODO: Add constraint restricting z > 0. #Wait for the IK service to become available #rospy.wait_for_service('compute_ik') #rospy.init_node('service_query') place_holder = {'images': [], 'camera_infos': []} #rospy.Subscriber("/cameras/head_camera/image", Image, lambda x: place_holder['images'].append(x)) #rospy.Subscriber("/cameras/head_camera/camera_info", CameraInfo, lambda x: place_holder['camera_infos'].append(x)) limb = Limb("right") #print(limb) #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) count = 0 print(limb.endpoint_pose()) calibration_points = [] # while not rospy.is_shutdown() and count < 3: # #calibration points 1-3, closest to robot right = origin, closest to robot left, id_card 3 3/8 inches forward from first point # ''' # /cameras/head_camera/camera_info # /cameras/head_camera/camera_info_std # /cameras/head_camera/image # ''' # count += 1 # raw_input("calibrate point " + str(count)) # pos = limb.endpoint_pose() # calibration_points.append({}) # calibration_points[-1]['world_coordinates'] = (pos['position'].x, pos['position'].y, pos['position'].z) # #calibration_points[-1]['camera_info'] = place_holder['camera_infos'][-1] # #calibration_points[-1]['image'] = place_holder['images'][-1] # for i in calibration_points: # print(i['world_coordinates']) if ROBOT == "sawyer": Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3]) Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) else: Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4]) Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) ## Add the controller here! IF you uncomment below, you should get the checkoff! c = Controller(Kp, Ki, Kd, Kw, Limb('right')) orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 box = PoseStamped() box.header.frame_id = "base" box.pose.position.x = box_in.pose.position.x box.pose.position.y = box_in.pose.position.y box.pose.position.z = box_in.pose.position.z # box.pose.position.x = 0.5 # box.pose.position.y = 0.0 # box.pose.position.z = 0.0 box.pose.orientation.x = 0.0 box.pose.orientation.y = 0.0 box.pose.orientation.z = 0.0 box.pose.orientation.w = 1.0 planner.add_box_obstacle((3, 4, 0.10), "box", box) # planner.add_box_obstacle((box_in.pose.position.x, box_in.pose.position.y, box_in.pose.position.z), "box", box) # ree = input("some text") while not rospy.is_shutdown(): try: if ROBOT == "baxter": _x, _y, _z = .67, -.2, .3 #x, y, z = .95, .11, .02 #x, y, z = .7, -.4, .2 print(0) else: raise Exception("not configured for sawyer yet.") goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = _x goal_1.pose.position.y = _y goal_1.pose.position.z = _z #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 # Might have to edit this . . . plan = planner.plan_to_pose(goal_1, [orien_const]) raw_input("Press <Enter> to move the right arm to goal pose 1: ") start = time.time() if not c.execute_path(plan): raise Exception("Execution failed") runtime = time.time() - start with open( '/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt', 'a') as f: f.write(str(runtime) + ' move above point \n') except Exception as e: print(e) traceback.print_exc() else: break while not rospy.is_shutdown(): try: if ROBOT == "baxter": #x, y, z = .67, -.2, 0 # x, y, z = .79, .04, -.04 pass else: raise Exception("not configured for sawyer yet.") goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = x goal_2.pose.position.y = y goal_2.pose.position.z = z #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 # Might have to edit this . . . plan = planner.plan_to_pose(goal_2, [orien_const]) raw_input("Press <Enter> to move the right arm to goal pose 2: ") start = time.time() if not c.execute_path(plan): raise Exception("Execution failed") runtime = time.time() - start with open( '/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt', 'a') as f: f.write(str(runtime) + ' move to point \n') except Exception as e: print(e) traceback.print_exc() else: break