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 compute_reward(logdir, simulation):
with open(os.path.join(logdir, "goal.json"), 'r') as f:
goal = json.load(f)
difficulty = goal['difficulty']
goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']),
orientation=np.array(
goal['goal']['orientation']))
if (simulation == 0):
log = robot_fingers.TriFingerPlatformLog(
os.path.join(logdir, "robot_data.dat"),
os.path.join(logdir, "camera_data.dat"))
reward = 0.0
for t in range(log.get_first_timeindex(),
log.get_last_timeindex() + 1):
camera_observation = log.get_camera_observation(t)
reward -= evaluate_state(goal_pose,
camera_observation.filtered_object_pose,
difficulty)
return reward, True
else:
path = os.path.join(logdir, 'observations.pkl')
with open(path, 'rb') as handle:
observations = pkl.load(handle)
reward = 0.0
ex_state = move_cube.sample_goal(difficulty=-1)
for i in range(len(observations)):
ex_state.position = observations[i]["achieved_goal"]["position"]
ex_state.orientation = observations[i]["achieved_goal"][
"orientation"]
reward -= evaluate_state(goal_pose, ex_state, difficulty)
return reward, True
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():
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)
robot_timestamps_ms = []
camera_timestamps_ms = []
# iterate over all robot time steps in the log
for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1):
robot_timestamps_ms.append(log.get_timestamp_ms(t))
# show images of camera180
try:
camera_observation = log.get_camera_observation(t)
camera_timestamps_ms.append(
camera_observation.cameras[0].timestamp * 1000.0)
except Exception as e:
print(e)
camera_timestamps_ms.append(0)
plt.plot(robot_timestamps_ms, label="robot")
plt.plot(camera_timestamps_ms, label="camera")
plt.legend()
plt.show()
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--robot-log",
required=True,
type=str,
help="Path to the robot log file.",
)
parser.add_argument(
"--camera-log",
required=True,
type=str,
help="Path to the camera log file.",
)
parser.add_argument(
"--goal-file",
required=True,
type=str,
help="Path to the goal JSON file.",
)
args = parser.parse_args()
# load goal from json file
move_cube.validate_goal_file(args.goal_file)
with open(args.goal_file, "r") as fh:
goal = json.load(fh)
difficulty = int(goal["difficulty"])
goal_pose = move_cube.Pose.from_dict(goal["goal"])
try:
log = robot_fingers.TriFingerPlatformLog(args.robot_log,
args.camera_log)
except Exception as e:
print("Failed to load logs:", e)
sys.exit(1)
# verify that the log is complete and matches the expected length
t_first = log.get_first_timeindex()
t_last = log.get_last_timeindex()
if t_first != 0:
print("Invalid log: First time index in robot log is {}. Expected 0.".
format(t_first))
sys.exit(1)
if t_last != move_cube.episode_length - 1:
print("Invalid log: Last time index in robot log is {}. Expected {}.".
format(t_last, move_cube.episode_length - 1))
sys.exit(1)
cumulative_reward = 0
for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1):
camera_observation = log.get_camera_observation(t)
cube_pose = camera_observation.object_pose
reward = -move_cube.evaluate_state(goal_pose, cube_pose, difficulty)
cumulative_reward += reward
print("Cumulative Reward:", cumulative_reward)
def compute_reward_adaptive_behind(logdir, simulation, TOTALTIMESTEPS):
with open(os.path.join(logdir, "goal.json"), 'r') as f:
goal = json.load(f)
difficulty = goal['difficulty']
goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']),
orientation=np.array(
goal['goal']['orientation']))
indice = goal['reachstart']
if (indice == -1):
return -10000, False
if (simulation == 0):
min_length = TOTALTIMESTEPS
log = robot_fingers.TriFingerPlatformLog(
os.path.join(logdir, "robot_data.dat"),
os.path.join(logdir, "camera_data.dat"))
reward = 0.0
count = 0
for t in range(log.get_last_timeindex() - TOTALTIMESTEPS,
log.get_last_timeindex() + 1):
count += 1
camera_observation = log.get_camera_observation(t)
reward -= evaluate_state(goal_pose,
camera_observation.filtered_object_pose,
difficulty)
if (count == min_length):
break
if (count == min_length):
return reward, True
else:
return -10000, False
else:
min_length = TOTALTIMESTEPS
path = os.path.join(logdir, 'observations.pkl')
with open(path, 'rb') as handle:
observations = pkl.load(handle)
reward = 0.0
count = 0
ex_state = move_cube.sample_goal(difficulty=-1)
for i in range(
len(observations) - TOTALTIMESTEPS - 1, len(observations)):
count += 1
ex_state.position = observations[i]["achieved_goal"]["position"]
ex_state.orientation = observations[i]["achieved_goal"][
"orientation"]
reward -= evaluate_state(goal_pose, ex_state, difficulty)
if (count == min_length):
break
if (count == min_length):
return reward, True
else:
return -10000, False
def compute_reward(logdir):
log = robot_fingers.TriFingerPlatformLog(os.path.join(logdir, "robot_data.dat"),
os.path.join(logdir, "camera_data.dat"))
with open(os.path.join(logdir, "goal.json"), 'r') as f:
goal = json.load(f)
difficulty = goal['difficulty']
goal_pose = move_cube.Pose(position=np.array(goal['goal']['position']),
orientation=np.array(goal['goal']['orientation']))
reward = 0.0
for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1):
camera_observation = log.get_camera_observation(t)
reward -= move_cube.evaluate_state(
goal_pose, camera_observation.filtered_object_pose, difficulty
)
return reward
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
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--task",
required=True,
choices=["move_cube", "move_cube_on_trajectory", "rearrange_dice"],
help="Name of the task.",
)
parser.add_argument(
"--log-dir",
required=True,
type=pathlib.Path,
help="Directory containing the log files.",
)
args = parser.parse_args()
# some setup depending on the selected task
if args.task == "move_cube":
import trifinger_simulation.tasks.move_cube as task
compute_reward = compute_reward_move_cube
elif args.task == "move_cube_on_trajectory":
import trifinger_simulation.tasks.move_cube_on_trajectory as task
compute_reward = compute_reward_move_cube_on_trajectory
elif args.task == "rearrange_dice":
import trifinger_simulation.tasks.rearrange_dice as task
compute_reward = compute_reward_rearrange_dice
else:
raise ValueError(f"Invalid task {args.task}")
# dictionary with additional, task-specific data that is passed to the
# compute_reward function
additional_data = {}
# load goal from json file
try:
with open(args.log_dir / "goal.json", "r") as fh:
goal = json.load(fh)
task.validate_goal(goal["goal"])
except Exception as e:
print("Invalid goal file:", e)
sys.exit(1)
if args.task == "rearrange_dice":
# for the rearrange_dice task, we need to generate the goal mask
camera_params = load_camera_parameters(args.log_dir, "camera{id}.yml")
additional_data["goal_masks"] = task.generate_goal_mask(
camera_params, goal["goal"]
)
try:
robot_log = str(args.log_dir / "robot_data.dat")
camera_log = str(args.log_dir / "camera_data.dat")
if args.task in ("move_cube", "move_cube_on_trajectory"):
log = robot_fingers.TriFingerPlatformWithObjectLog(
robot_log, camera_log
)
else:
log = robot_fingers.TriFingerPlatformLog(robot_log, camera_log)
except Exception as e:
print("Failed to load logs:", e)
sys.exit(1)
# verify that the log is complete and matches the expected length
t_first = log.get_first_timeindex()
t_last = log.get_last_timeindex()
if t_first != 0:
print(
"Invalid log: First time index in robot log is {}. Expected 0.".format(
t_first
)
)
sys.exit(1)
if t_last != task.EPISODE_LENGTH - 1:
print(
"Invalid log: Last time index in robot log is {}. Expected {}.".format(
t_last, task.EPISODE_LENGTH - 1
)
)
sys.exit(1)
cumulative_reward = 0
for t in range(log.get_first_timeindex(), log.get_last_timeindex() + 1):
reward = compute_reward(task, log, t, goal, **additional_data)
cumulative_reward += reward
print("Cumulative Reward:", cumulative_reward)