add 'forward' command line option

run_agent.py

@@ -8,18 +8,20 @@
 import argparse
 import socket
 import traceback
+from math import pi
 from pathlib import Path
 
 import gin
 import yaml
 
 from loop_rate_limiters import RateLimiter
+from scipy.spatial.transform import Rotation
+
+from pink_balancer import WholeBodyController
+from upkie.exceptions import FallDetected
 from upkie.spine import SpineInterface
 from upkie.utils.raspi import configure_agent_process, on_raspi
 from upkie.utils.spdlog import logging
-from upkie.exceptions import FallDetected
-
-from pink_balancer import WholeBodyController
 
 
 def get_vertical_force(
@@ -70,6 +72,12 @@ def parse_command_line_arguments() -> argparse.Namespace:
         default=False,
         action="store_true",
     )
+    parser.add_argument(
+        "--forward",
+        help="Make robot go forward",
+        default=True,
+        action="store_true",
+    )
     return parser.parse_args()
 
 
@@ -79,6 +87,7 @@ def run(
     controller: WholeBodyController,
     frequency: float = 200.0,
     levitate: bool = False,
+    forward: bool = False,
 ) -> None:
     """Read observations and send actions to the spine.
 
@@ -106,10 +115,27 @@ def run(
 
     step: int = 0
     observation = spine.get_observation()  # pre-reset observation
+
+    observation["joystick"] = {""}
+
 
     while True:
         observation = spine.get_observation()
 
+        if forward: 
+            controller.height_controller.target_height = 0.05 # max_crouch_height: 0.08
+            controller.wheel_controller.target_ground_velocity = 0.3
+
+            # Simple P-controller to make sure we are always
+            # facing the stairs (which corresponds to a 0-yaw value)
+            current_orientation = observation["imu"]["orientation"]
+            current_yaw = Rotation.from_quat(current_orientation).as_euler("xyz")[0] * 180 / pi
+            print(f"current_yaw: {current_yaw.round(1)}")
+            target_yaw = 0.0
+            controller.wheel_controller.target_yaw_velocity = - 0.1 * (
+                target_yaw - current_yaw
+            )
+
         if step % 1000 and levitate:
             observation["joystick"] = {"cross_button": True} # trigger reset
 
@@ -155,7 +181,7 @@ def run(
     while keep_trying:
         keep_trying = False
         try:
-            run(spine, spine_config, controller, levitate=args.levitate)
+            run(spine, spine_config, controller, levitate=args.levitate, forward=args.forward)
         except KeyboardInterrupt:
             logging.info("Caught a keyboard interrupt")
         except FallDetected: