Separate examples for sim and real

Have separate "move_up_and_down" demos, one for the real robot (the one
that already existed) and one for the simulation.  To make them more
self-contained, do not define the move function in a separate file.

Further add separate "sim_trajectory_example_with_gym" for simulation
and prepend the existing one with "real_".

Author: luator

README.md

@@ -2,7 +2,7 @@ Example Package for the Real Robot Challenge
 ============================================
 
 This is a basic example for a package that can be submitted to the robots of
-the [Real Robot Challenge](https://real-robot-challenge.com).
+the [Real Robot Challenge 2021](https://real-robot-challenge.com).
 
 It is a normal ROS2 Python package that can be build with colcon.  However,
 there are a few special files in the root directory that are needed for
@@ -18,10 +18,27 @@ documentation](https://docs.ros.org/en/foxy/Tutorials/Creating-Your-First-ROS2-P
 Challenge Simulation Phase
 --------------------------
 
-For evaluation of the simulation phase (phase 1) of the challenge, the critical
-file is the `evaluate_policy.py` at the root directory of the package.  This is
-what is going to be executed by `rrc_evaluate_prestage.py` (found in
-`scripts/`).
+There are two example scripts using the simulation:
+
+- `sim_move_up_and_down`:  Directly uses the `TriFingerPlatform` class to simply
+  move the robot between two fixed positions.  This is implemented in
+  `rrc_example_package/scripts/sim_move_up_and_down.py`.
+
+- `sim_trajectory_example_with_gym`:  Wraps the robot class in a Gym environment
+  and uses that to run a dummy policy which simply points with one finger on the
+  goal positions of the trajectory.  This is implemented in 
+  `rrc_example_package/scripts/sim_trajectory_example_with_gym.py`.
+
+To execute the examples, [build the
+package](https://people.tuebingen.mpg.de/felixwidmaier/rrc2021/singularity.html#singularity-build-ws)
+and execute
+
+    ros2 run rrc_example_package <example_name>
+
+
+For evaluation of the pre-stage of the challenge, the critical file is the
+`evaluate_policy.py` at the root directory of the package.  This is what is
+going to be executed by `rrc_evaluate_prestage.py` (found in `scripts/`).
 
 For more information, see https://people.tuebingen.mpg.de/felixwidmaier/rrc2021/
 
@@ -36,15 +53,15 @@ For the challenge phases on the real robots, you need to provide the following
 files at the root directory of the package such that your jobs can executed on
 the robots:
 
-- `goal.json`:  May contain some configuration for sampling random goals or a
-  fixed goal (might be useful for testing/training).  See the documentation of
-  the challenge tasks for more details.
 - `run`:  Script that is executed when submitting the package to the robot.
   This can, for example, be a Python script or a symlink to a script somewhere
   else inside the repository.  In the given example, it is a shell script
   running a Python script via `ros2 run`.  This approach would also work for C++
   executables.  When executed, a JSON string encoding the goal is passed as
   argument (the exact structure of the goal depends on the current task).
+- `goal.json`:  Optional.  May contain a fixed goal (might be useful for
+  testing/training).  See the documentation of the challenge tasks for more
+  details.
 
 It is important that the `run` script is executable.  For this, you need to do
 two things:

rrc_example_package/example.py

@@ -1,13 +1,11 @@
-"""Simple example on how to move the robot."""
+"""Example policies, demonstrating how to control the robot."""
 import os
 
 import numpy as np
 from ament_index_python.packages import get_package_share_directory
 
 import trifinger_simulation.finger_types_data
 import trifinger_simulation.pinocchio_utils
-import robot_interfaces
-import robot_fingers
 
 
 class PointAtTrajectoryPolicy:
@@ -145,41 +143,3 @@ def predict(self, observation, t):
 
         # make sure to return a copy, not a reference to self.joint_positions
         return np.array(self.joint_positions)
-
-
-def move_up_and_down(
-    frontend: robot_fingers.TriFingerPlatformWithObjectFrontend, episode_length: int
-):
-    """Move up and down multiple times using fixed goal positions.
-
-    Args:
-        frontend:  Frontend of the TriFingerPro platform.  Used to control the
-            robot.
-        episode_length:  Number of time steps in the episode.  Used to ensure
-            that the limit is not exceeded.
-    """
-    position_down = [-0.08, 0.84, -1.2] * 3
-    position_up = [0.5, 1.2, -2.4] * 3
-    target_positions = [position_down, position_up]
-
-    i = 0
-    while True:
-        print("Iteration {}".format(i))
-        action = robot_interfaces.trifinger.Action(
-            position=target_positions[i % 2]
-        )
-        i += 1
-
-        for _ in range(500):
-            t = frontend.append_desired_action(action)
-            frontend.wait_until_timeindex(t)
-
-            # make sure to not exceed the number of allowed actions
-            if t >= episode_length - 1:
-                return
-
-        robot_observation = frontend.get_robot_observation(t)
-        print("Finger positions:", robot_observation.position)
-
-        camera_observation = frontend.get_camera_observation(t)
-        print("Object position:", camera_observation.object_pose.position)

rrc_example_package/scripts/move_up_and_down.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+"""Simple example on how to move the robot."""
+import json
+import sys
+import robot_interfaces
+import robot_fingers
+from trifinger_simulation.tasks import move_cube_on_trajectory
+
+
+def main():
+    # the goal is passed as JSON string
+    goal_json = sys.argv[1]
+    goal = json.loads(goal_json)
+    print("Goal: %s" % goal)
+
+    # It is possible to create custom files in "/output"
+    with open("/output/hello.txt", "w") as fh:
+        fh.write("Hello there!\n")
+
+    # create the robot frontend
+    frontend = robot_fingers.TriFingerPlatformWithObjectFrontend()
+
+    # move the robot between two fixed positions ("up" and "down")
+    position_down = [-0.08, 0.84, -1.2] * 3
+    position_up = [0.5, 1.2, -2.4] * 3
+    target_positions = [position_down, position_up]
+
+    i = 0
+    while True:
+        print("Iteration {}".format(i))
+        action = robot_interfaces.trifinger.Action(
+            position=target_positions[i % 2]
+        )
+        i += 1
+
+        for _ in range(500):
+            t = frontend.append_desired_action(action)
+            frontend.wait_until_timeindex(t)
+
+            # make sure to not exceed the number of allowed actions
+            if t >= move_cube_on_trajectory.EPISODE_LENGTH - 1:
+                return
+
+        robot_observation = frontend.get_robot_observation(t)
+        print("Finger positions:", robot_observation.position)
+
+        camera_observation = frontend.get_camera_observation(t)
+        print("Object position:", camera_observation.object_pose.position)

rrc_example_package/scripts/real_move_up_and_down.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+"""Simple example on how to move the robot."""
+import trifinger_simulation
+from trifinger_simulation.tasks import move_cube_on_trajectory
+from trifinger_simulation.trifinger_platform import ObjectType
+
+
+def main():
+    """Example on how to move the robot in simulation."""
+    # create the robot platform simulation
+    robot = trifinger_simulation.TriFingerPlatform(
+        visualization=True,
+        object_type=ObjectType.COLORED_CUBE,
+    )
+
+    # move the robot
+    position_down = [-0.08, 0.84, -1.2] * 3
+    position_up = [0.5, 1.2, -2.4] * 3
+    target_positions = [position_down, position_up]
+
+    i = 0
+    while True:
+        action = robot.Action(
+            position=target_positions[i % 2]
+        )
+        i += 1
+
+        for _ in range(500):
+            t = robot.append_desired_action(action)
+
+            # make sure to not exceed the number of allowed actions
+            if t >= move_cube_on_trajectory.EPISODE_LENGTH - 1:
+                return
+
+        robot_observation = robot.get_robot_observation(t)
+        print("Finger positions:", robot_observation.position)
+
+        camera_observation = robot.get_camera_observation(t)
+        print("Object position:", camera_observation.object_pose.position)

rrc_example_package/scripts/trajectory_example_with_gym.py renamed to rrc_example_package/scripts/real_trajectory_example_with_gym.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+"""Demo on how to run the simulation using the Gym environment
+
+This demo creates a SimCubeTrajectoryEnv environment and runs one episode using
+a dummy policy.
+"""
+from rrc_example_package import cube_trajectory_env
+from rrc_example_package.example import PointAtTrajectoryPolicy
+
+
+def main():
+    env = cube_trajectory_env.SimCubeTrajectoryEnv(
+        goal_trajectory=None,  # passing None to sample a random trajectory
+        action_type=cube_trajectory_env.ActionType.POSITION,
+        visualization=True,
+    )
+
+    is_done = False
+    observation = env.reset()
+    t = 0
+
+    policy = PointAtTrajectoryPolicy(env.action_space, env.info["trajectory"])
+    while not is_done:
+        action = policy.predict(observation, t)
+        observation, reward, is_done, info = env.step(action)
+        t = info["time_index"]
+
+
+if __name__ == "__main__":
+    main()

rrc_example_package/scripts/sim_move_up_and_down.py

@@ -3,6 +3,6 @@
 # run the script via `ros2 run` (for python scripts a simple symlink would also
 # work but using `ros2 run` works for C++ executables as well)
 
-# ros2 run rrc_example_package trajectory_example_with_gym "$@"
+# ros2 run rrc_example_package real_trajectory_example_with_gym "$@"
 ros2 run rrc_example_package dice_example_with_gym "$@"
-# ros2 run rrc_example_package move_up_and_down "$@"
+# ros2 run rrc_example_package real_move_up_and_down "$@"

rrc_example_package/scripts/sim_trajectory_example_with_gym.py

@@ -30,8 +30,10 @@
     # scripts here.
     entry_points={
         "console_scripts": [
-            "move_up_and_down = rrc_example_package.scripts.move_up_and_down:main",
-            "trajectory_example_with_gym = rrc_example_package.scripts.trajectory_example_with_gym:main",
+            "real_move_up_and_down = rrc_example_package.scripts.real_move_up_and_down:main",
+            "sim_move_up_and_down = rrc_example_package.scripts.sim_move_up_and_down:main",
+            "real_trajectory_example_with_gym = rrc_example_package.scripts.real_trajectory_example_with_gym:main",
+            "sim_trajectory_example_with_gym = rrc_example_package.scripts.sim_trajectory_example_with_gym:main",
             "dice_example_with_gym = rrc_example_package.scripts.dice_example_with_gym:main",
         ],
     },