def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "--store-timestamps", type=str, help="""Pass --store_timestamps to dump the timestamps from the observations of the three cameras into a pickle file, followed by the name of this pickle file (for eg. time_stamps.p). """, ) args = argparser.parse_args() camera_data = trifinger_cameras.tricamera.SingleProcessData() camera_driver = trifinger_cameras.tricamera.TriCameraDriver( "camera60", "camera180", "camera300" ) camera_backend = trifinger_cameras.tricamera.Backend( # noqa camera_driver, camera_data ) camera_frontend = trifinger_cameras.tricamera.Frontend(camera_data) observations_timestamps_list = [] while True: observation = camera_frontend.get_latest_observation() window_60 = "Image Stream camera60" window_180 = "Image Stream camera180" window_300 = "Image Stream camera300" cv2.imshow( window_180, utils.convert_image(observation.cameras[0].image) ) cv2.imshow( window_300, utils.convert_image(observation.cameras[1].image) ) cv2.imshow( window_60, utils.convert_image(observation.cameras[2].image) ) # stop if either "q" or ESC is pressed if cv2.waitKey(3) in [ord("q"), 27]: # 27 = ESC break observations_timestamps_list.append( [ observation.cameras[0].time_stamp, observation.cameras[1].time_stamp, observation.cameras[2].time_stamp, ] ) if args.store_timestamps: pickle.dump( observations_timestamps_list, open(args.store_timestamps, "wb") )
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "logfile", type=str, help="Path to the log file.", ) argparser.add_argument( "outfile", type=str, help="Path to the output file.", ) argparser.add_argument( "--camera", "-c", choices=["camera60", "camera180", "camera300"], required=True, help="Name of the camera", ) args = argparser.parse_args() log_reader = trifinger_cameras.tricamera.LogReader(args.logfile) if args.camera == "camera60": camera_idx = 0 elif args.camera == "camera180": camera_idx = 1 else: camera_idx = 2 first_obs = log_reader.data[0].cameras[camera_idx] last_obs = log_reader.data[-1].cameras[camera_idx] # determine rate based on time stamps start_time = first_obs.timestamp end_time = last_obs.timestamp interval = (end_time - start_time) / len(log_reader.data) fps = 1 / interval # convert to ms interval = int(interval * 1000) # Define the codec and create VideoWriter object first_img = utils.convert_image(first_obs.image) fourcc = cv2.VideoWriter_fourcc(*"XVID") writer = cv2.VideoWriter(args.outfile, fourcc, fps, first_img.shape[:2]) print( "Loaded {} frames at an average interval of {} ms ({:.1f} fps)".format( len(log_reader.data), interval, 1000 / interval)) for observation in log_reader.data: image = utils.convert_image(observation.cameras[camera_idx].image) writer.write(image)
def main(): parser = argparse.ArgumentParser() parser.add_argument("robot_log", type=str, help="Robot log file") parser.add_argument("camera_log", type=str, help="Camera log file") args = parser.parse_args() log = robot_fingers.TriFingerPlatformLog(args.robot_log, args.camera_log) # iterate over all robot time steps in the log for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1): # TriFingerPlatformLog provides the same getters as # TriFingerPlatformFrontend: robot_observation = log.get_robot_observation(t) # print position of the first finger print(robot_observation.position[:3]) # show images of camera180 try: camera_observation = log.get_camera_observation(t) cv2.imshow( "camera180", utils.convert_image(camera_observation.cameras[1].image), ) cv2.waitKey(1) except Exception as e: print(e)
def get_synced_log_data(logdir, goal, difficulty): log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"), os.path.join(logdir, "camera_data.dat")) log_camera = tricamera.LogReader(os.path.join(logdir, "camera_data.dat")) stamps = log_camera.timestamps obs = {'robot': [], 'cube': [], 'images': [], 't': [], 'desired_action': [], 'stamp': [], 'acc_reward': []} ind = 0 acc_reward = 0.0 for t in range(log.get_first_timeindex(), log.get_last_timeindex()): camera_observation = log.get_camera_observation(t) acc_reward -= move_cube.evaluate_state( move_cube.Pose(**goal), camera_observation.filtered_object_pose, difficulty ) if 1000 * log.get_timestamp_ms(t) >= stamps[ind]: robot_observation = log.get_robot_observation(t) obs['robot'].append(robot_observation) obs['cube'].append(camera_observation.filtered_object_pose) obs['images'].append([convert_image(camera.image) for camera in camera_observation.cameras]) obs['desired_action'].append(log.get_desired_action(t)) obs['acc_reward'].append(acc_reward) obs['t'].append(t) obs['stamp'].append(log.get_timestamp_ms(t)) ind += 1 return obs
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "--multi-process", action="store_true", help="""If set, use multiprocess camera data to access backend in other process. Otherwise run the backend locally. """, ) args = argparser.parse_args() camera_names = ["camera60", "camera180", "camera300"] if args.multi_process: camera_data = tricamera.MultiProcessData("tricamera", False) else: camera_data = tricamera.SingleProcessData() camera_driver = tricamera.TriCameraDriver(*camera_names) camera_backend = tricamera.Backend(camera_driver, camera_data) # noqa camera_frontend = tricamera.Frontend(camera_data) while True: observation = camera_frontend.get_latest_observation() for i, name in enumerate(camera_names): cv2.imshow(name, convert_image(observation.cameras[i].image)) print("-----") print("Object Pose Confidence:", observation.object_pose.confidence) print("Object Position:", observation.object_pose.position) print("Object Orientation:", observation.object_pose.orientation) # stop if either "q" or ESC is pressed if cv2.waitKey(3) in [ord("q"), 27]: # 27 = ESC break
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "filename", type=str, help="""Path to the log file.""", ) args = argparser.parse_args() log_reader = trifinger_cameras.camera.LogReader(args.filename) # determine rate based on time stamps start_time = log_reader.data[0].timestamp end_time = log_reader.data[-1].timestamp interval = (end_time - start_time) / len(log_reader.data) # convert to ms interval = int(interval * 1000) print( "Loaded {} frames at an average interval of {} ms ({:.1f} fps)".format( len(log_reader.data), interval, 1000 / interval)) for observation in log_reader.data: cv2.imshow("Image", utils.convert_image(observation.image)) # stop if either "q" or ESC is pressed if cv2.waitKey(interval) in [ord("q"), 27]: # 27 = ESC break
def save(self, observation): directory = os.path.join(self.out_dir, self.sample_name) os.makedirs(directory) for i, name in enumerate(self.camera_names): filename = os.path.join(directory, name + ".png") image = utils.convert_image(observation.cameras[i].image) cv2.imwrite(filename, image)
def compute_reward_rearrange_dice(task, log, t, goal, goal_masks): camera_observation = log.get_camera_observation(t) masks = tuple( segment_image(convert_image(c.image)) for c in camera_observation.cameras ) reward = -task.evaluate_state(goal_masks, masks) return reward
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "--store-timestamps", type=str, metavar="FILENAME", help="""Store the timestamps from the observations of the three cameras into the specified pickle file. """, ) argparser.add_argument( "--multi-process", action="store_true", help="""If set, use multiprocess camera data to access backend in other process. Otherwise run the backend locally. """, ) args = argparser.parse_args() camera_names = ["camera60", "camera180", "camera300"] if args.multi_process: camera_data = trifinger_cameras.tricamera.MultiProcessData( "tricamera", False ) else: camera_data = trifinger_cameras.tricamera.SingleProcessData() camera_driver = trifinger_cameras.tricamera.TriCameraDriver( *camera_names ) camera_backend = trifinger_cameras.tricamera.Backend( # noqa camera_driver, camera_data ) camera_frontend = trifinger_cameras.tricamera.Frontend(camera_data) observations_timestamps_list = [] while True: observation = camera_frontend.get_latest_observation() for i, name in enumerate(camera_names): cv2.imshow(name, utils.convert_image(observation.cameras[i].image)) # stop if either "q" or ESC is pressed if cv2.waitKey(3) in [ord("q"), 27]: # 27 = ESC break observations_timestamps_list.append( [ observation.cameras[0].timestamp, observation.cameras[1].timestamp, observation.cameras[2].timestamp, ] ) if args.store_timestamps: pickle.dump( observations_timestamps_list, open(args.store_timestamps, "wb") )
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "--pylon", action="store_true", help="""Access the camera via Pylon. If not set, OpenCV is used.""", ) argparser.add_argument( "--camera-id", "-c", default="", help="""ID of the camera that is used. If --pylon is set this refers to the DeviceUserId, otherwise it is the index of the device. """, ) argparser.add_argument( "--record", type=str, help="""Path to file in which camera data is recorded.""", ) args = argparser.parse_args() camera_data = trifinger_cameras.camera.SingleProcessData() # camera_data = trifinger_cameras.camera.MultiProcessData("cam", True, 10) if args.pylon: camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id) else: camera_id = int(args.camera_id) if args.camera_id else 0 camera_driver = trifinger_cameras.camera.OpenCVDriver(camera_id) camera_backend = trifinger_cameras.camera.Backend( # noqa camera_driver, camera_data) camera_frontend = trifinger_cameras.camera.Frontend(camera_data) if args.record: logger = trifinger_cameras.camera.Logger(camera_data, 10000) logger.start() while True: observation = camera_frontend.get_latest_observation() image = utils.convert_image(observation.image) window_name = "Image Stream" cv2.imshow(window_name, image) # stop if either "q" or ESC is pressed if cv2.waitKey(3) in [ord("q"), 27]: # 27 = ESC break if args.record: print("Save recorded data to file {}".format(args.record)) logger.stop_and_save(args.record)
def _create_observation(self, t, action): robot_observation = self.platform.get_robot_observation(t) camera_observation = self.platform.get_camera_observation(t) segmentation_masks = [ segment_image(convert_image(c.image)) for c in camera_observation.cameras ] observation = { "robot_observation": { "position": robot_observation.position, "velocity": robot_observation.velocity, "torque": robot_observation.torque, }, "action": action, "desired_goal": self.goal_masks, "achieved_goal": segmentation_masks, } return observation
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "log_dir", type=pathlib.Path, help="Path to the log file.", ) args = parser.parse_args() camera_log_file = args.log_dir / "camera_data.dat" if not args.log_dir.is_dir(): print("{} does not exist.".format(args.log_dir)) sys.exit(1) calib_files = [] for name in CAMERA_NAMES: calib_file = args.log_dir / (name + ".yml") if calib_file.exists(): calib_files.append(str(calib_file)) else: print("{} does not exist.".format(calib_file)) sys.exit(1) cube_detector = object_tracker.CubeDetector(calib_files) log_reader = tricamera.LogReader(str(camera_log_file)) for observation in log_reader.data: images = [ utils.convert_image(camera.image) for camera in observation.cameras ] pose = cube_detector.detect_cube_single_thread(images) print(pose.position) debug_img = cube_detector.create_debug_image() cv2.imshow("Debug", debug_img) # stop if either "q" or ESC is pressed if cv2.waitKey(3) in [ord("q"), 27]: # 27 = ESC break
def main(): parser = argparse.ArgumentParser() parser.add_argument("robot_log", type=str, help="Robot log file") parser.add_argument("camera_log", type=str, help="Camera log file") parser.add_argument( "--with-object", action="store_true", help="Set this if the camera observations contain an object pose.", ) args = parser.parse_args() if args.with_object: Log = robot_fingers.TriFingerPlatformWithObjectLog else: Log = robot_fingers.TriFingerPlatformLog log = Log(args.robot_log, args.camera_log) # iterate over all robot time steps in the log for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1): # TriFingerPlatformLog provides the same getters as # TriFingerPlatformFrontend: robot_observation = log.get_robot_observation(t) # print time index and position of the first finger print("%d - %s" % (t, robot_observation.position[:3])) # show images of camera180 try: camera_observation = log.get_camera_observation(t) cv2.imshow( "camera180", utils.convert_image(camera_observation.cameras[1].image), ) key = cv2.waitKey(1) if key == ord("q"): return except Exception as e: print(e)
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "filename", type=str, help="""Path to the log file.""", ) argparser.add_argument( "output_dir", type=str, help="Directory to which images are stored.", ) argparser.add_argument( "--step", "-s", type=int, metavar="n", help="Extract only every n-th frame.", ) args = argparser.parse_args() out_dir = pathlib.Path(args.output_dir) if not out_dir.is_dir(): print("{} does not exist or is not a directory".format(out_dir)) sys.exit(1) log_reader = tricamera.LogReader(args.filename) for i, observation in enumerate(log_reader.data[::args.step]): observation_dir = out_dir / ("%04d" % (i + 1)) observation_dir.mkdir() for camera_idx, name in enumerate( ["camera60", "camera180", "camera300"]): image = utils.convert_image(observation.cameras[camera_idx].image) img_path = observation_dir / "{}.png".format(name) cv2.imwrite(str(img_path), image)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "filename", type=str, help="Path to the log file.", ) parser.add_argument( "--visualize-object-pose", "-v", action="store_true", help="""Visualize detected object pose. This expects files camera{60,180,300}.yml with calibration parameters to exist in the same directory as the given camera log file. """, ) parser.add_argument( "--visualize-goal-pose", "-g", type=pathlib.Path, metavar="GOAL_FILE", help="Visualize goal from the specified JSON file.", ) parser.add_argument( "--show-confidence", action="store_true", help="Print the object pose confidence in the images.", ) parser.add_argument( "--unfiltered", action="store_true", help="Use the unfiltered object pose.", ) parser.add_argument( "--plot-cube-position", "-p", action="store_true", help="Plot cube position", ) parser.add_argument( "--compensate-cube-offset", action="store_true", help="Compensate cube position offset in old logfiles.", ) parser.add_argument( "--save-video", type=str, metavar="VIDEO_FILE", help="""Save the images of the camera selected by --camera to a AVI video file. Expects as argument the output path. """, ) parser.add_argument( "--camera", "-c", choices=CAMERA_NAMES, help="Name of the camera. Used by --save-video.", ) args = parser.parse_args() log_file_path = pathlib.Path(args.filename) if not log_file_path.exists(): print("{} does not exist.".format(log_file_path)) sys.exit(1) if args.visualize_goal_pose: if not args.visualize_goal_pose.exists(): print("{} does not exist.".format(args.visualize_goal_pose)) sys.exit(1) with open(args.visualize_goal_pose, "r") as fh: goal_dict = json.load(fh) goal_pose = trifinger_object_tracking.py_object_tracker.ObjectPose() goal_pose.position = goal_dict["goal"]["position"] goal_pose.orientation = goal_dict["goal"]["orientation"] calib_files = [] if args.visualize_object_pose or args.visualize_goal_pose: for name in CAMERA_NAMES: calib_file = log_file_path.parent / (name + ".yml") if calib_file.exists(): calib_files.append(str(calib_file)) else: print("{} does not exist.".format(calib_file)) sys.exit(1) cube_visualizer = tricamera.CubeVisualizer(calib_files) log_reader = tricamera.LogReader(args.filename) # determine rate based on time stamps start_time = log_reader.data[0].cameras[0].timestamp end_time = log_reader.data[-1].cameras[0].timestamp interval = (end_time - start_time) / len(log_reader.data) fps = 1 / interval # convert to ms interval = int(interval * 1000) if args.save_video: if not args.camera: print("--camera is required for saving video.") sys.exit(1) camera_index = CAMERA_NAMES.index(args.camera) first_img = utils.convert_image(log_reader.data[0].cameras[0].image) fourcc = cv2.VideoWriter_fourcc(*'XVID') video_writer = cv2.VideoWriter(args.save_video, fourcc, fps, first_img.shape[:2]) print( "Loaded {} frames at an average interval of {} ms ({:.1f} fps)".format( len(log_reader.data), interval, fps)) for observation in log_reader.data: images = [ utils.convert_image(camera.image) for camera in observation.cameras ] if args.unfiltered: object_pose = observation.object_pose else: object_pose = observation.filtered_object_pose if args.compensate_cube_offset: # object_pose is read-only and unfortunately copy.copy does not # work for this type object_pose_cpy = type(object_pose)() object_pose_cpy.position = np.array(object_pose.position) object_pose_cpy.orientation = np.array(object_pose.orientation) object_pose_cpy.confidence = object_pose.confidence object_pose = object_pose_cpy offset = np.array([0, 0, 0.0325]) cube_rot = Rotation.from_quat(object_pose.orientation) object_pose.position = object_pose.position + cube_rot.apply( offset) if args.visualize_goal_pose: cvmats = [trifinger_cameras.camera.cvMat(img) for img in images] images = cube_visualizer.draw_cube(cvmats, goal_pose, True) images = [np.array(img) for img in images] if args.visualize_object_pose: cvmats = [trifinger_cameras.camera.cvMat(img) for img in images] images = cube_visualizer.draw_cube(cvmats, object_pose, False) images = [np.array(img) for img in images] if args.show_confidence: images = [ cv2.putText(image, "confidence: %.2f" % object_pose.confidence, (0, image.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0)) for image in images ] if args.save_video: video_writer.write(images[camera_index]) else: for i, name in enumerate(CAMERA_NAMES): cv2.imshow(name, images[i]) # stop if either "q" or ESC is pressed if cv2.waitKey(interval) in [ord("q"), 27]: # 27 = ESC break if args.plot_cube_position: plt.scatter(observation.cameras[0].timestamp, object_pose.position[0], color="red") plt.scatter(observation.cameras[0].timestamp, object_pose.position[1], color="green") plt.scatter(observation.cameras[0].timestamp, object_pose.position[2], color="blue") plt.title("Cube Position") legend_elements = [ Line2D([0], [0], marker='o', color='w', label='x', markerfacecolor='r'), Line2D([0], [0], marker='o', color='w', label='y', markerfacecolor='g'), Line2D([0], [0], marker='o', color='w', label='z', markerfacecolor='b'), ] plt.legend(handles=legend_elements, loc="upper right") plt.pause(0.01)
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "x", type=float, help="Target x-position of the cube in world frame.", ) argparser.add_argument( "y", type=float, help="Target y-position of the cube in world frame.", ) argparser.add_argument( "z", type=float, help="Target z-position of the cube in world frame.", ) argparser.add_argument( "--multi-process", action="store_true", help="""If set, use multiprocess camera data to access backend in other process. Otherwise run the backend locally. """, ) args = argparser.parse_args() camera_names = ["camera60", "camera180", "camera300"] cube_position = np.array([args.x, args.y, args.z]) cube_pose = trifinger_object_tracking.py_object_tracker.ObjectPose() cube_pose.position = cube_position cube_pose.orientation = np.array([0, 0, 0, 1]) calib_files = [] for name in camera_names: camera_config_dir = pathlib.Path( get_package_share_directory("trifinger_cameras"), "config" ) calib_file = ( camera_config_dir / "camera_calibration_trifingerone" / ("%s_cropped.yml" % name) ) if calib_file.exists(): calib_files.append(str(calib_file)) else: print("{} does not exist.".format(calib_file)) sys.exit(1) cube_visualizer = CubeVisualizer(calib_files) if args.multi_process: camera_data = tricamera.MultiProcessData("tricamera", False) else: camera_data = tricamera.SingleProcessData() camera_driver = tricamera.TriCameraDriver( *camera_names, downsample_images=False ) camera_backend = tricamera.Backend(camera_driver, camera_data) # noqa camera_frontend = tricamera.Frontend(camera_data) while True: observation = camera_frontend.get_latest_observation() images = [ convert_image(camera.image) for camera in observation.cameras ] images = cube_visualizer.draw_cube(images, cube_pose, False) # show images of all three cameras next to each other stacked_image = np.hstack(images) cv2.imshow(" | ".join(camera_names), stacked_image) # for name, image in zip(camera_names, images): # cv2.imshow(name, image) # stop if either "q" or ESC is pressed if cv2.waitKey(100) in [ord("q"), 27]: # 27 = ESC break
def main(): argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument("--outdir", "-o", type=str, required=True, help="""Output directory.""") argparser.add_argument( "--trigger-interval", type=float, metavar="INTERVAL", help="""Automatically record frames in the specified interval (in seconds). If this is set, a live stream of the camera is shown and recording is triggered automatically every INTERVAL seconds. If not set, the script waits for the user to press Enter to record frames (in this case, no live stream is shown). """, ) argparser.add_argument( "--driver", default="tri", choices=("tri", "pylon", "opencv"), help="""Which camera driver to use. Use "tri" for the three-camera setup (using Pylon), "pylon" for a single Pylon camera and "opencv" for an arbitrary camera that is supported by OpenCV. Default is "%(default)s". """, ) argparser.add_argument( "--camera-id", "-c", help="""ID of the camera that is used. Depends on the setting of --driver: If the "pylon" driver is used this refers to the DeviceUserId, in case of "opencv" it is the index of the device. For the "tri" driver, this value is ignored. """, ) args = argparser.parse_args() if args.driver != "tri" and args.camera_id is None: print("You need to specify --camera-id") return if args.driver == "tri": camera_names = ["camera60", "camera180", "camera300"] camera_driver = trifinger_cameras.tricamera.TriCameraDriver( *camera_names) camera_module = trifinger_cameras.tricamera image_saver = TriImageSaver(args.outdir, camera_names) elif args.driver == "pylon": camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id) camera_module = trifinger_cameras.camera image_saver = SingleImageSaver(args.outdir, args.camera_id) elif args.driver == "opencv": camera_driver = trifinger_cameras.camera.OpenCVDriver( int(args.camera_id)) camera_module = trifinger_cameras.camera image_saver = SingleImageSaver(args.outdir, args.camera_id) camera_data = camera_module.SingleProcessData() camera_backend = camera_module.Backend(camera_driver, camera_data) # noqa camera_frontend = camera_module.Frontend(camera_data) while True: sample_name = image_saver.next() if image_saver.exists(): print("skip existing sample {}".format(sample_name)) continue if args.trigger_interval: interval_ms = 10 steps = int(args.trigger_interval * 1000 / interval_ms) for i in range(steps): observation = camera_frontend.get_latest_observation() if args.driver == "tri": for i, name in enumerate(camera_names): image = utils.convert_image( observation.cameras[i].image) cv2.imshow(name, image) else: image = utils.convert_image(observation.image) cv2.imshow(args.camera_id, image) cv2.waitKey(interval_ms) print("Record sample {}".format(sample_name)) else: input("Press Enter to record sample {}".format(sample_name)) observation = camera_frontend.get_latest_observation() image_saver.save(observation)
def test_convert_image(self): # NOTE: this is not testing if the result is correct but simply uses # the result of an old call of the function to verify that the # behaviour did not change. raw_image = np.array( [ [207, 206, 196, 107, 198, 204, 206], [199, 196, 175, 177, 182, 193, 201], [189, 85, 95, 175, 176, 108, 194], [40, 128, 83, 92, 92, 39, 64], [44, 64, 56, 89, 79, 60, 41], [69, 77, 64, 88, 66, 78, 73], [66, 69, 73, 78, 81, 82, 82], ], dtype=np.uint8, ) img_bgr = np.array( [ [ [196, 166, 172], [196, 166, 172], [187, 175, 146], [177, 160, 166], [185, 182, 187], [193, 174, 194], [193, 174, 194], ], [ [196, 166, 172], [196, 166, 172], [187, 175, 146], [177, 160, 166], [185, 182, 187], [193, 174, 194], [193, 174, 194], ], [ [162, 85, 142], [162, 85, 142], [148, 130, 95], [135, 175, 136], [125, 139, 176], [116, 108, 185], [116, 108, 185], ], [ [128, 68, 96], [128, 68, 96], [110, 83, 76], [92, 110, 102], [66, 92, 128], [39, 81, 123], [39, 81, 123], ], [ [103, 64, 50], [103, 64, 50], [96, 75, 56], [90, 89, 68], [74, 77, 79], [59, 60, 60], [59, 60, 60], ], [ [77, 67, 60], [77, 67, 60], [83, 64, 65], [88, 74, 72], [83, 66, 80], [78, 70, 71], [78, 70, 71], ], [ [77, 67, 60], [77, 67, 60], [83, 64, 65], [88, 74, 72], [83, 66, 80], [78, 70, 71], [78, 70, 71], ], ], dtype=np.uint8, ) img_rgb = np.array( [ [ [172, 166, 196], [172, 166, 196], [146, 175, 187], [166, 160, 177], [187, 182, 185], [194, 174, 193], [194, 174, 193], ], [ [172, 166, 196], [172, 166, 196], [146, 175, 187], [166, 160, 177], [187, 182, 185], [194, 174, 193], [194, 174, 193], ], [ [142, 85, 162], [142, 85, 162], [95, 130, 148], [136, 175, 135], [176, 139, 125], [185, 108, 116], [185, 108, 116], ], [ [96, 68, 128], [96, 68, 128], [76, 83, 110], [102, 110, 92], [128, 92, 66], [123, 81, 39], [123, 81, 39], ], [ [50, 64, 103], [50, 64, 103], [56, 75, 96], [68, 89, 90], [79, 77, 74], [60, 60, 59], [60, 60, 59], ], [ [60, 67, 77], [60, 67, 77], [65, 64, 83], [72, 74, 88], [80, 66, 83], [71, 70, 78], [71, 70, 78], ], [ [60, 67, 77], [60, 67, 77], [65, 64, 83], [72, 74, 88], [80, 66, 83], [71, 70, 78], [71, 70, 78], ], ], dtype=np.uint8, ) img_gray = np.array( [ [171, 171, 167, 164, 184, 182, 182], [171, 171, 167, 164, 184, 182, 182], [111, 111, 121, 159, 149, 132, 132], [83, 83, 84, 105, 100, 89, 89], [64, 64, 72, 83, 77, 60, 60], [66, 66, 66, 75, 72, 71, 71], [66, 66, 66, 75, 72, 71, 71], ], dtype=np.uint8, ) np.testing.assert_array_equal(utils.convert_image(raw_image), img_bgr) np.testing.assert_array_equal(utils.convert_image(raw_image, "bgr"), img_bgr) np.testing.assert_array_equal(utils.convert_image(raw_image, "rgb"), img_rgb) np.testing.assert_array_equal(utils.convert_image(raw_image, "gray"), img_gray) # verify that invalid formats result in an error with self.assertRaises(ValueError): utils.convert_image(raw_image, "foo")
def save(self, observation): filename = os.path.join(self.out_dir, self.sample_name) image = utils.convert_image(observation.image) cv2.imwrite(filename, image)
def real(): camera_names = ["camera60", "camera180", "camera300"] argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument("--camera-name", "-c", choices=camera_names) argparser.add_argument("--object-type", choices=["cube", "aruco"], default="cube") argparser.add_argument("--no-downsample", action="store_true") argparser.add_argument("--multi-process", action="store_true") args = argparser.parse_args() robot_properties_path = rospkg.RosPack().get_path("robot_properties_fingers") urdf_file = trifinger_simulation.finger_types_data.get_finger_urdf("trifingerpro") finger_urdf_path = os.path.join(robot_properties_path, "urdf", urdf_file) kinematics = trifinger_simulation.pinocchio_utils.Kinematics( finger_urdf_path, ["finger_tip_link_0", "finger_tip_link_120", "finger_tip_link_240"], ) if args.camera_name: camera_index = camera_names.index(args.camera_name) else: camera_index = None if args.multi_process: camera_data = tricamera.MultiProcessData("tricamera", False) else: camera_data = tricamera.SingleProcessData() camera_driver = tricamera.TriCameraObjectTrackerDriver( *camera_names, downsample_images=(not args.no_downsample) ) camera_backend = tricamera.Backend( # noqa camera_driver, camera_data ) camera_frontend = tricamera.Frontend(camera_data) if args.object_type == "aruco": if args.no_downsample: calib_file_fmt = "/etc/trifingerpro/camera{}_cropped.yml" else: calib_file_fmt = "/etc/trifingerpro/camera{}_cropped_and_downsampled.yml" detectors = [ ArucoDetector(calib_file_fmt.format(60)), ArucoDetector(calib_file_fmt.format(180)), ArucoDetector(calib_file_fmt.format(300)), ] if args.multi_process: robot_data = robot_interfaces.trifinger.MultiProcessData("trifinger", False) frontend = robot_interfaces.trifinger.Frontend(robot_data) else: robot = robot_fingers.Robot.create_by_name("trifingerpro") robot.initialize() frontend = robot.frontend init_pos = np.array([0, 1.5, -2.7] * 3) for i in range(500): finger_action = robot_interfaces.trifinger.Action(position=init_pos) frontend.append_desired_action(finger_action) joint_pos = init_pos i = 0 while True: i += 1 finger_action = robot_interfaces.trifinger.Action(position=joint_pos) finger_action.position_kp = [8] * 9 finger_action.position_kd = [0.01] * 9 t = frontend.append_desired_action(finger_action) obs = frontend.get_observation(t) images = camera_frontend.get_latest_observation() if args.object_type == "aruco": if camera_index is not None: img = convert_image(images.cameras[camera_index].image) world_marker_position = detectors[camera_index].detect_marker_pose(img) if world_marker_position is None: time.sleep(0.1) continue else: positions = [ detector.detect_marker_pose(convert_image(camera.image)) for detector, camera in zip(detectors, images.cameras) ] # filter out Nones positions = [p for p in positions if p is not None] if not positions: time.sleep(0.1) continue world_marker_position = np.mean(positions, axis=0) else: world_marker_position = images.object_pose.position # set goal a bit above the marker goal = np.array(world_marker_position[:3], copy=True) goal[2] += 0.08 new_joint_pos, err = kinematics.inverse_kinematics_one_finger( 0, goal, obs.position, tolerance=0.002, max_iterations=3000) # keep the other two fingers up alpha = 0.1 joint_pos[:3] = alpha * new_joint_pos[:3] + (1 - alpha) * joint_pos[:3] if i % 500 == 0: tip_pos = kinematics.forward_kinematics(obs.position) print("-----------------------------------------------------") print("Object position:", np.round(world_marker_position[:3], 3)) print("Tip goal:", np.round(goal, 3)) print("Tip position:", np.round(tip_pos[0], 3)) print("IK error:", np.round(err, 3))
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("log_dir", type=pathlib.Path, help="Path to the log files.") parser.add_argument("--rate", type=int, default=1, help="Time in ms per step.") args = parser.parse_args() robot_log_file = str(args.log_dir / "robot_data.dat") camera_log_file = str(args.log_dir / "camera_data.dat") log = robot_fingers.TriFingerPlatformLog(robot_log_file, camera_log_file) simulation = sim_finger.SimFinger( finger_type="trifingerpro", enable_visualization=True, ) cameras = camera.create_trifinger_camera_array_from_config(args.log_dir) cube_urdf_file = (pathlib.Path(trifinger_simulation.__file__).parent / "data/cube_v2/cube_v2.urdf") cube = pybullet.loadURDF(fileName=str(cube_urdf_file), ) assert cube >= 0, "Failed to load cube model." pybullet.configureDebugVisualizer(lightPosition=(0, 0, 1.0)) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SHADOWS, 1) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) # yes, it is really a segmentation maRk... pybullet.configureDebugVisualizer( pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1, 100): robot_observation = log.get_robot_observation(t) simulation.reset_finger_positions_and_velocities( robot_observation.position) # get rendered images from simulation sim_images = cameras.get_images() # images are rgb --> convert to bgr for opencv sim_images = [ cv2.cvtColor(img, cv2.COLOR_RGB2BGR) for img in sim_images ] sim_images = np.hstack(sim_images) # get real images from cameras try: camera_observation = log.get_camera_observation(t) # set cube pose pybullet.resetBasePositionAndOrientation( cube, camera_observation.object_pose.position, camera_observation.object_pose.orientation, ) real_images = [ utils.convert_image(cam.image) for cam in camera_observation.cameras ] real_images = np.hstack(real_images) except Exception as e: print(e) real_images = np.zeros_like(sim_images) # display images image = np.vstack((sim_images, real_images)) cv2.imshow("cameras", image) key = cv2.waitKey(args.rate) # exit on "q" if key == ord("q"): return