WIP: Create autonomous sim

config/spine.yaml

@@ -0,0 +1,25 @@
+bullet:
+    follower_camera: false
+    gui: true
+    reset:
+        orientation_base_in_world: [1.0, 0.0, 0.0, 0.0]
+        position_base_in_world: [0.0, 0.0, 2.6] # [m]
+    torque_control:
+        kp: 20.0
+        kd: 1.0
+    monitor:
+        contacts:
+            left_wheel_tire: true
+            right_wheel_tire: true
+floor_contact:
+    upper_leg_torque_threshold: 10.0
+wheel_contact:
+    cutoff_period: 0.2
+    liftoff_inertia: 0.001
+    min_touchdown_acceleration: 2.0
+    min_touchdown_torque: 0.015
+    touchdown_inertia: 0.004
+wheel_odometry:
+    signed_radius:
+        left_wheel: +0.05
+        right_wheel: -0.05

pink_balancer/height_controller.py

@@ -100,8 +100,8 @@ class HeightController:
     transform_rest_to_world: dict
     max_height_difference: float
     max_lean_velocity: float
-    target_height: float = 0
-    height_difference: float = 0
+    target_height: float = 0.0
+    height_difference: float = 0.0
 
     def __init__(
         self,
@@ -173,7 +173,7 @@ def __init__(
             ),
         }
         tasks["posture"].set_target(
-            custom_configuration_vector(robot, left_knee=0.2, right_knee=-0.2)
+            custom_configuration_vector(robot, left_knee=-0.2, right_knee=0.2)
         )
 
         visualizer = None
@@ -257,7 +257,9 @@ def update_target_height(self, observation: dict, dt: float) -> None:
         """
         height = self.get_next_height_from_joystick(observation, dt)
 
-        self.target_height = clamp(height, 0, self.max_crouch_height)
+        self.target_height = clamp(
+            height, 0.0, self.max_crouch_height
+        )
 
         height_difference = self.get_next_height_difference_from_joystick(
             observation, dt

pink_balancer/wheel_controller.py

@@ -11,6 +11,7 @@
 
 import gin
 import numpy as np
+from upkie.utils.clamp import clamp_abs
 from upkie.utils.filters import abs_bounded_derivative_filter
 
 from .remote_control import RemoteControl
@@ -146,7 +147,9 @@ def update_target_ground_velocity(
         """
         try:
             axis_value = observation["joystick"]["left_axis"][1]
-            max_velocity = self.remote_control.max_linear_velocity
+            trigger_value = observation["joystick"]["right_trigger"]  # -1 to 1
+            boost_value = clamp_abs(0.5 * (trigger_value + 1.0), 1.0)  # 0 to 1
+            max_velocity = 0.5 * (1.0 + boost_value) * self.remote_control.max_linear_velocity
             unfiltered_velocity = -max_velocity * axis_value
         except KeyError:
             unfiltered_velocity = 0.0

run_agent.py

@@ -8,16 +8,42 @@
 import argparse
 import socket
 import traceback
+from math import pi
 from pathlib import Path
 
 import gin
-import upkie.config
+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 pink_balancer import WholeBodyController
+
+def get_vertical_force(
+    step: int,
+    start: int = 200,
+    lift_steps: int = 200,
+    delta: float = 0.1,
+    hold_steps: int = 400,
+) -> float:
+    lift: float = 0.0  # 0 = no force, 1 => apply -mg
+    if step < start:
+        lift = 0.0
+    elif step - start < lift_steps // 2:
+        lift = 1.0 + delta
+    elif step - start < lift_steps:
+        lift = 1.0 - delta
+    elif step - start - lift_steps < hold_steps:
+        lift = 1.0
+    else:
+        lift = 0.0
+    mass = 5.34  # approximative, in [kg]
+    return lift * mass * 9.81  # in [N]
 
 
 def parse_command_line_arguments() -> argparse.Namespace:
@@ -40,6 +66,18 @@ def parse_command_line_arguments() -> argparse.Namespace:
         default=False,
         action="store_true",
     )
+    parser.add_argument(
+        "--levitate",
+        help="Levitate the robot by applying a vertical force",
+        default=False,
+        action="store_true",
+    )
+    parser.add_argument(
+        "--forward",
+        help="Make robot go forward",
+        default=True,
+        action="store_true",
+    )
     return parser.parse_args()
 
 
@@ -48,6 +86,8 @@ def run(
     spine_config: dict,
     controller: WholeBodyController,
     frequency: float = 200.0,
+    levitate: bool = False,
+    forward: bool = False,
 ) -> None:
     """Read observations and send actions to the spine.
 
@@ -60,12 +100,53 @@ def run(
     dt = 1.0 / frequency
     rate = RateLimiter(frequency, "controller")
 
+    if levitate:
+        torso_force_in_world = [0.0, 0.0, 0.0]
+        bullet_action = {
+            "external_forces": {
+                "torso": {
+                    "force": torso_force_in_world,
+                    "local": False,
+                }
+            }
+        }
+
     spine.start(spine_config)
+
+    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
+
         action = controller.cycle(observation, dt)
+
+        if levitate:
+            torso_force_in_world[2] = get_vertical_force(step % 1000)
+            action["bullet"] = bullet_action
+
         spine.set_action(action)
+        step += 1
         rate.sleep()
 
 
@@ -86,22 +167,34 @@ def run(
     if on_raspi():
         configure_agent_process()
 
-    spine = SpineInterface(retries=10)
+    with open(config_dir / "spine.yaml", "r") as file:
+        spine_config = yaml.safe_load(file)
+
     controller = WholeBodyController(visualize=args.visualize)
-    spine_config = upkie.config.SPINE_CONFIG.copy()
     wheel_radius = controller.wheel_controller.wheel_radius
     wheel_odometry_config = spine_config["wheel_odometry"]
     wheel_odometry_config["signed_radius"]["left_wheel"] = +wheel_radius
     wheel_odometry_config["signed_radius"]["right_wheel"] = -wheel_radius
-    try:
-        run(spine, spine_config, controller)
-    except KeyboardInterrupt:
-        logging.info("Caught a keyboard interrupt")
-    except Exception:
-        logging.error("Controller raised an exception")
-        print("")
-        traceback.print_exc()
-        print("")
+    spine = SpineInterface(retries=10)
+
+    keep_trying = True
+    while keep_trying:
+        keep_trying = False
+        try:
+            run(spine, spine_config, controller, levitate=args.levitate, forward=args.forward)
+        except KeyboardInterrupt:
+            logging.info("Caught a keyboard interrupt")
+        except FallDetected:
+            logging.error("Fall detected, resetting the spine!")
+            spine.stop()
+            controller = WholeBodyController(visualize=args.visualize)
+            spine = SpineInterface(retries=10)
+            keep_trying = True
+        except Exception:
+            logging.error("Controller raised an exception!")
+            print("")
+            traceback.print_exc()
+            print("")
 
     logging.info("Stopping the spine...")
     try: