From df89e2a45b4430553c2bee1983efb92de0375747 Mon Sep 17 00:00:00 2001 From: ubgk Date: Wed, 28 Aug 2024 22:14:43 +0200 Subject: [PATCH 1/5] fix merge --- config/spine.yaml | 25 +++++++++++++ run_agent.py | 91 ++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 104 insertions(+), 12 deletions(-) create mode 100644 config/spine.yaml diff --git a/config/spine.yaml b/config/spine.yaml new file mode 100644 index 0000000..fa288f9 --- /dev/null +++ b/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 diff --git a/run_agent.py b/run_agent.py index f068316..10ee1cd 100644 --- a/run_agent.py +++ b/run_agent.py @@ -11,15 +11,39 @@ from pathlib import Path import gin -import upkie.config +import yaml + from loop_rate_limiters import RateLimiter 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( + 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: """Parse command line arguments. @@ -40,6 +64,12 @@ 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", + ) return parser.parse_args() @@ -48,6 +78,7 @@ def run( spine_config: dict, controller: WholeBodyController, frequency: float = 200.0, + levitate: bool = False, ) -> None: """Read observations and send actions to the spine. @@ -60,12 +91,36 @@ 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 + while True: observation = spine.get_observation() + + 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 +141,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) + 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: From 6b3333b8f525458177872ce1f8d4ebfc21be1fe5 Mon Sep 17 00:00:00 2001 From: ubgk Date: Wed, 28 Aug 2024 22:17:14 +0200 Subject: [PATCH 2/5] invert knees --- pink_balancer/height_controller.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/pink_balancer/height_controller.py b/pink_balancer/height_controller.py index 2cbda62..59086b2 100644 --- a/pink_balancer/height_controller.py +++ b/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 From 23764f513c06544fc099db62f8dbf9e43ede42c5 Mon Sep 17 00:00:00 2001 From: ubgk Date: Wed, 28 Aug 2024 23:02:08 +0200 Subject: [PATCH 3/5] Revert "Disable auxiliary boost from right trigger" This reverts commit 348afabd1a3c472a6b0ee86d3aa2be50266f469d. --- pink_balancer/wheel_controller.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/pink_balancer/wheel_controller.py b/pink_balancer/wheel_controller.py index b1eeb9f..d70be61 100644 --- a/pink_balancer/wheel_controller.py +++ b/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.max_target_velocity unfiltered_velocity = -max_velocity * axis_value except KeyError: unfiltered_velocity = 0.0 From c334d774db6a4299ce42fc2778d3c8b48d0d0a8c Mon Sep 17 00:00:00 2001 From: ubgk Date: Tue, 3 Sep 2024 14:02:22 +0200 Subject: [PATCH 4/5] wheel balancer fix --- pink_balancer/wheel_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pink_balancer/wheel_controller.py b/pink_balancer/wheel_controller.py index d70be61..276b28a 100644 --- a/pink_balancer/wheel_controller.py +++ b/pink_balancer/wheel_controller.py @@ -149,7 +149,7 @@ def update_target_ground_velocity( axis_value = observation["joystick"]["left_axis"][1] 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.max_target_velocity + 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 From 480bca04f223d5b6c827e49ecaa0e30d56a0a166 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Wed, 4 Sep 2024 17:41:22 +0200 Subject: [PATCH 5/5] add 'forward' command line option --- run_agent.py | 34 ++++++++++++++++++++++++++++++---- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/run_agent.py b/run_agent.py index 10ee1cd..776153b 100644 --- a/run_agent.py +++ b/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: