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
