Do not convert objects to dict and log more data

- With pickle it is not necessary to convert the action/observation
  objects to dictionaries.  In fact, this conversion led to some
  inaccuracy.
- Log robot and object observation of each step for easier debugging in
  case something goes wrong in the replay.

Author: luator

python/rrc_simulation/trifinger_platform.py

@@ -1,4 +1,3 @@
-import json
 import pickle
 import numpy as np
 
@@ -205,11 +204,8 @@ def __init__(
         # Initialize log
         # ==============
         self._action_log = {
-            "initial_robot_position": initial_robot_position.tolist(),
-            "initial_object_pose": {
-                "position": initial_object_pose.position.tolist(),
-                "orientation": initial_object_pose.orientation.tolist(),
-            },
+            "initial_robot_position": initial_robot_position,
+            "initial_object_pose": initial_object_pose,
             "actions": [],
         }
 
@@ -262,13 +258,14 @@ def append_desired_action(self, action):
                     ].timestamp = camera_timestamp_s
 
         # write the desired action to the log
+        object_pose = self.get_object_pose(t)
+        robot_obs = self.get_robot_observation(t)
         self._action_log["actions"].append(
             {
                 "t": t,
-                "torque": action.torque.tolist(),
-                "position": action.position.tolist(),
-                "position_kp": action.position_kp.tolist(),
-                "position_kd": action.position_kd.tolist(),
+                "action": action,
+                "object_pose": object_pose,
+                "robot_observation": robot_obs,
             }
         )
 
@@ -387,10 +384,9 @@ def store_action_log(self, filename):
         t = self.simfinger.get_current_timeindex()
         object_pose = self.get_object_pose(t)
         self._action_log["final_object_pose"] = {
-            "position": object_pose.position.tolist(),
-            "orientation": object_pose.orientation.tolist(),
+            "t": t,
+            "pose": object_pose,
         }
 
         with open(filename, "wb") as fh:
-            # json.dump(self._action_log, fh)
             pickle.dump(self._action_log, fh)

scripts/replay_action_log.py

@@ -19,7 +19,6 @@
 
 """
 import argparse
-import json
 import pickle
 import sys
 import numpy as np
@@ -66,16 +65,43 @@ def main():
     args = parser.parse_args()
 
     with open(args.logfile, "rb") as fh:
-        # log = json.load(fh)
         log = pickle.load(fh)
 
     initial_object_pose = move_cube.Pose.from_json(args.initial_pose)
     goal_pose = move_cube.Pose.from_json(args.goal_pose)
 
+    # verify that the initial object pose matches with the one in the log file
+    try:
+        np.testing.assert_array_almost_equal(
+            initial_object_pose.position, log["initial_object_pose"].position,
+            err_msg=("Given initial object position does not match with log file.")
+        )
+        np.testing.assert_array_almost_equal(
+            initial_object_pose.orientation, log["initial_object_pose"].orientation,
+            err_msg=("Given initial object orientation does not match with log file.")
+        )
+    except AssertionError as e:
+        print("Failed.", file=sys.stderr)
+        print(e, file=sys.stderr)
+        sys.exit(1)
+
     platform = trifinger_platform.TriFingerPlatform(
         visualization=False, initial_object_pose=initial_object_pose
     )
 
+    # verify that the robot is initialized to the same position as in the log
+    # file
+    initial_robot_position = platform.get_robot_observation(0).position
+    try:
+        np.testing.assert_array_almost_equal(
+            initial_robot_position, log["initial_robot_position"],
+            err_msg=("Initial robot position does not match with log file.")
+        )
+    except AssertionError as e:
+        print("Failed.", file=sys.stderr)
+        print(e, file=sys.stderr)
+        sys.exit(1)
+
     # verify that the number of logged actions matches with the episode length
     n_actions = len(log["actions"])
     assert (
@@ -84,14 +110,11 @@ def main():
 
     accumulated_reward = 0
     for logged_action in log["actions"]:
-        action = platform.Action()
-        action.torque = np.array(logged_action["torque"])
-        action.position = np.array(logged_action["position"])
-        action.position_kp = np.array(logged_action["position_kp"])
-        action.position_kd = np.array(logged_action["position_kd"])
+        action = logged_action["action"]
 
         t = platform.append_desired_action(action)
 
+        robot_obs = platform.get_robot_observation(t)
         cube_pose = platform.get_object_pose(t)
         reward = -move_cube.evaluate_state(
             goal_pose, cube_pose, args.difficulty
@@ -100,20 +123,47 @@ def main():
 
         assert logged_action["t"] == t
 
+        np.testing.assert_array_almost_equal(
+            robot_obs.position, logged_action["robot_observation"].position,
+            err_msg=("Step %d: Recorded robot position does not match with"
+                     " the one achieved by the replay" % t)
+        )
+        np.testing.assert_array_almost_equal(
+            robot_obs.torque, logged_action["robot_observation"].torque,
+            err_msg=("Step %d: Recorded robot torque does not match with"
+                     " the one achieved by the replay" % t)
+        )
+        np.testing.assert_array_almost_equal(
+            robot_obs.velocity, logged_action["robot_observation"].velocity,
+            err_msg=("Step %d: Recorded robot velocity does not match with"
+                     " the one achieved by the replay" % t)
+        )
+
+        np.testing.assert_array_almost_equal(
+            cube_pose.position, logged_action["object_pose"].position,
+            err_msg=("Step %d: Recorded object position does not match with"
+                     " the one achieved by the replay" % t)
+        )
+        np.testing.assert_array_almost_equal(
+            cube_pose.orientation, logged_action["object_pose"].orientation,
+            err_msg=("Step %d: Recorded object orientation does not match with"
+                     " the one achieved by the replay" % t)
+        )
+
     cube_pose = platform.get_object_pose(t)
-    final_pose = log["final_object_pose"]
+    final_pose = log["final_object_pose"]["pose"]
 
     print("Accumulated Reward:", accumulated_reward)
 
     # verify that actual and logged final object pose match
     try:
         np.testing.assert_array_almost_equal(
-            cube_pose.position, final_pose["position"], decimal=3,
+            cube_pose.position, final_pose.position, decimal=3,
             err_msg=("Recorded object position does not match with the one"
                      " achieved by the replay")
         )
         np.testing.assert_array_almost_equal(
-            cube_pose.orientation, final_pose["orientation"], decimal=3,
+            cube_pose.orientation, final_pose.orientation, decimal=3,
             err_msg=("Recorded object orientation does not match with the one"
                      " achieved by the replay")
         )