def left2right_motion(robot, left_point, center, right_point, multiplier): # Move left and then move back with multiplier 2 robot.add_goal(cb2_robot.Goal(left_point, True, 'linear', radius=0.0)) while not robot.goals.empty(): robot.move_on_stop() print("moved left!") robot.add_goal(cb2_robot.Goal(center, True, 'linear', radius=0.0)) spliner = Spline(order=2) path = spliner.get_path(left_point[:3], center[:3], right_point[:3], n=100, bezier=False) #print(path) new = center new[2] = new[2] + 0.1 robot.add_goal(cb2_robot.Goal(new, True, 'linear', radius=0.0)) while not robot.goals.empty(): robot.move_on_error(multiplier=multiplier) print("moved to the right with spline")
def center_robot(robot, pose): # angle_start = [90, -95, 90, 0, 90, 90] # angle_start = map(cb2_robot.cb2_send.deg_2_rad, angle_start) angle_start = [ -01.5503467, -02.0665188, -01.3330899, +00.1346348, +01.6603317, -01.5165228 ] robot.add_goal(cb2_robot.Goal(angle_start, False, 'joint')) robot.add_goal(cb2_robot.Goal(pose, cartesian=True, move_type='linear')) while not robot.goals.empty(): robot.move_on_stop()
def curve_motion(robot, current, goal, new): spliner = Spline(order=2) path = spliner.get_path(current[:3], goal[:3], new[:3], n=30, bezier=True) print(path) for p in path: robot.add_goal( cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2], True, 'process')) while not robot.goals.empty(): robot.move_on_error(multiplier=2.5)
def get_images_poses(robot_samples, cam_name, robot_address, robot_port, folder_out, file_out): """ Gets images from a cam_name and and poses of a UR Robot. Relies on pre-trained points to direct robot motion. Generates a json file with the pose of the robot and filename of the corresponding image. Args: robot_samples (str): The filename for the file containing the list of points which the robot should move through. This file can be generated using the `cb2-record` command from the ur_cb2 package. cam_name (str): The name of the cam_name to be used. Valid options are: - `flycap` robot_address (str): The address of the robot in form: `###.###.###` robot_port (int): The port of the robot folder_out (str): The folder in which to save the data. file_out (str): The file in which to save all of the generated data. """ with open(robot_samples, 'r') as f: data = json.load(f) write_time = data['time'] points = data['points'] print('read in {} points, written at: {}'.format( len(points.keys()), write_time)) tcp2robot = [] im_num = 0 if not os.path.isdir(folder_out): os.mkdir(folder_out) with cb2_robot.URRobot(robot_address, robot_port) as robot: with camera.Camera(cam_name) as cam: for number in sorted([int(x) for x in points.keys()]): robot.add_goal( cb2_robot.Goal(points[str(number)]['joint'], False, 'joint')) # TODO: this appears to skip the first point! robot.move_on_stop() print('Beginning move: {}'.format(number)) while not (robot.at_goal() and robot.is_stopped()): time.sleep(.25) time.sleep(.25) # let everything settle with robot.receiver.lock: tcp2robot.append(robot.receiver.position) cv2.imwrite(os.path.join(folder_out, str(im_num) + '.png'), cam.capture_raw()) im_num += 1 print(np.asarray(tcp2robot)) # `[x,y,z,<rotation vector>]` where `<rotation vector>` is a three element # vector representing the an axis about which to rotate (`<x,y,z>`) in # radians equal to the magnitude of the vector. json_dict = {"time": str(datetime.datetime.now()), "tcp2robot": tcp2robot} with open( os.path.join(folder_out, os.path.splitext(file_out)[0] + '.json'), 'w') as \ result_json_file: json.dump(json_dict, result_json_file, indent=4)
def get_correspondences(robot_samples, calibration, rows, cols, spacing, camera, robot_address, robot_port, file_out): """ Gets correspondences between a camera and UR Robot with a grid attached. Relies on pre-trained points to direct robot motion. Will try to find the grid 5 times per position. Generates a json file with all of the data needed o then calculate the tool offset and the camera to robot transformation. Args: robot_samples (str): The filename for the file containing the list of points which the robot should move through. This file can be generated using the `cb2-record` command from the ur_cb2 package. calibration (str): The filename of the camera calibration information. This file can be generated using the `calibrate-camera` command from the camera-calibration toolbox. rows (int): The number of rows on the grid which is attached to the robot cols (int): The number of columns on the grid which is attached to the robot spacing (float): The spacing in mm between grid corners on the grid which is attached to the robot camera (str): The name of the camera to be used. Valid options are: - `flycap` robot_address (str): The address of the robot in form: `###.###.###` robot_port (int): The port of the robot file_out (str): The file in which to save all of the generated data. """ with open(robot_samples, 'r') as f: data = json.load(f) write_time = data['time'] points = data['points'] print('read in {} points, written at: {}'.format( len(points.keys()), write_time)) camera2grid = [] tcp2robot = [] with ci.GridLocation(calibration, rows, cols, spacing, camera) as calib: with cb2_robot.URRobot(robot_address, robot_port) as robot: for number in sorted([int(x) for x in points.keys()]): robot.add_goal( cb2_robot.Goal(points[str(number)]['joint'], False, 'joint')) # TODO: this appears to skip the first point! robot.move_on_stop() print('Beginning move: {}'.format(number)) while not (robot.at_goal() and robot.is_stopped()): time.sleep(.25) time.sleep(.25) # let everything settle print("reached goal") go_on = 0 while go_on <= 5: try: camera2grid.append(calib.get_cam2grid()) calib.show_images() with robot.receiver.lock: tcp2robot.append(robot.receiver.position) print("got the grid") go_on = 6 except RuntimeError as e: print("something went wrong: {}".format(e)) go_on += 1 tcp2robot = np.array(tcp2robot) tcp2robot[:, 0:3] = tcp2robot[:, 0:3] * 1000 tcp2robot = tcp2robot.tolist() print(np.asarray(tcp2robot)) # Axis-Angle [x,y,z,ax,ay,az] print(np.asarray(camera2grid)) json_dict = { "grid": { "rows": rows, "cols": cols, "spacing": spacing }, "time": str(datetime.datetime.now()), "calibration": calibration, "tcp2robot": tcp2robot, "camera2grid": camera2grid } with open(os.path.splitext(file_out)[0] + '.json', 'w') as \ result_json_file: json.dump(json_dict, result_json_file, indent=4)
def semi_track(robot, current): """ Start at the center and do a complete circuit motion Args: robot: current: Returns: """ spliner = Spline(order=2) # A point little to the right of the center point1 = list(current) point1[0] = current[0] + 0.2 # a point above the previous point. # Should form curve path to this using previous point as a control point point2 = list(point1) point2[2] = point1[2] + 0.1 path = spliner.get_path(current[:3], point1[:3], point2[:3], n=30, bezier=True) #print(path) for p in path: robot.add_goal( cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) # Continue moving slightly up point3 = list(point2) point3[2] += 0.01 robot.add_goal( cb2_robot.Goal([point3[0], point3[1], point3[2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) # A little more up from point2 point4 = list(point2) point4[2] += 0.05 # A point to the left of point 4. # Should have a curve path from 3 to 5 via control point 4 point5 = list(point4) point5[0] -= 0.2 path = spliner.get_path(point3[:3], point4[:3], point5[:3], n=30, bezier=True) #print(path) for p in path: robot.add_goal( cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) while not robot.goals.empty(): robot.move_on_error(multiplier=2.5)
def circuit(robot, start): spliner = Spline(order=2) # A point little to the right of the start point1 = list(start) point1[0] += 0.2 # a point above the previous point. # Should form curve path to this using previous point as a control point point2 = list(point1) point2[2] = point1[2] + 0.1 path = spliner.get_path(start[:3], point1[:3], point2[:3], n=50, bezier=True) for p in path: robot.add_goal( cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) # Continue moving slightly up point3 = list(point2) point3[2] += 0.01 robot.add_goal( cb2_robot.Goal([point3[0], point3[1], point3[2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) # A little more up from point3 point4 = list(point2) point4[2] += 0.05 # A point to the left of point 4. # Should have a curve path from 3 to 5 via control point 4 point5 = list(point4) point5[0] -= 0.2 path = spliner.get_path(point3[:3], point4[:3], point5[:3], n=50, bezier=True) for p in path: robot.add_goal( cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) # Point more to the left of point 5 and slightly below point6 = list(point5) point6[0] -= 0.3 point6[2] -= 0.1 # point down to point 6 #point7 = list(point6) #point7[2] -= 0.1 # move back to the start point in a curve path = spliner.get_path(point5[:3], point6[:3], start[:3], n=50, bezier=True) for p in path[:-5]: robot.add_goal( cb2_robot.Goal([p[0], p[1], p[2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) while not robot.goals.empty(): robot.move_on_error(multiplier=2.0) robot.add_goal( cb2_robot.Goal([path[-1][0], path[-1][1], path[-1][2], 1.2, -1.2, 1.2], True, 'process', radius=0.001)) while not robot.goals.empty(): robot.move_on_error(multiplier=1.0) time.sleep(2)