From 009da095967167d7c8cd78ac088a2aaf4b2d0475 Mon Sep 17 00:00:00 2001
From: Mikael Dallaire Cote <110583667+0mdc@users.noreply.github.com>
Date: Sat, 31 Aug 2024 18:34:41 -0400
Subject: [PATCH] Add switch for allowing to control humanoid velocity from
 config.

---
 .../humanoid_rearrange_controller.py          | 24 +++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/habitat-lab/habitat/articulated_agent_controllers/humanoid_rearrange_controller.py b/habitat-lab/habitat/articulated_agent_controllers/humanoid_rearrange_controller.py
index 3e716895ca..dd1ccd852a 100644
--- a/habitat-lab/habitat/articulated_agent_controllers/humanoid_rearrange_controller.py
+++ b/habitat-lab/habitat/articulated_agent_controllers/humanoid_rearrange_controller.py
@@ -28,6 +28,7 @@
 DIST_TO_STOP: float = (
     1e-9  # If the amount to move is this distance, just stop the character
 )
+DEFINE_VELOCITY_FROM_CONFIG: bool = True
 
 from typing import List, Tuple
 
@@ -83,6 +84,7 @@ def __init__(
 
         self.prev_orientation = None
         self.walk_mocap_frame = 0
+        self.meters_per_step = 0.0
 
         self.hand_processed_data = {}
         self._hand_names = ["left_hand", "right_hand"]
@@ -125,8 +127,8 @@ def set_framerate_for_linspeed(
         """
 
         seconds_per_step = 1.0 / ctrl_freq
-        meters_per_step = lin_speed * seconds_per_step
-        frames_per_step = meters_per_step / self.dist_per_step_size
+        self.meters_per_step = lin_speed * seconds_per_step
+        frames_per_step = self.meters_per_step / self.dist_per_step_size
         self.motion_fps = self.walk_motion.fps / frames_per_step
         rotate_amount = ang_speed * seconds_per_step
         rotate_amount = rotate_amount * 180.0 / np.pi
@@ -270,7 +272,14 @@ def calculate_walk_pose(
         # The base_transform here is independent of transforms caused by the current
         # motion pose.
         obj_transform_base = look_at_path_T
-        forward_V_dist = forward_V * dist_diff * distance_multiplier
+
+        # TODO: Switch between animation-driven speed and config-driven speed.
+        if DEFINE_VELOCITY_FROM_CONFIG:
+            forward_V_dist = (
+                forward_V * self.meters_per_step * distance_multiplier
+            )
+        else:
+            forward_V_dist = forward_V * dist_diff * distance_multiplier
         obj_transform_base.translation += forward_V_dist
 
         rot_offset = mn.Matrix4.rotation(
@@ -445,7 +454,14 @@ def calculate_walk_pose_directional(
         # The base_transform here is independent of transforms caused by the current
         # motion pose.
         obj_transform_base = look_at_path_T
-        forward_V_dist = forward_V * dist_diff * distance_multiplier
+
+        # TODO: Switch between animation-driven speed and config-driven speed.
+        if DEFINE_VELOCITY_FROM_CONFIG:
+            forward_V_dist = (
+                forward_V * self.meters_per_step * distance_multiplier
+            )
+        else:
+            forward_V_dist = forward_V * dist_diff * distance_multiplier
         obj_transform_base.translation += forward_V_dist
 
         rot_offset = mn.Matrix4.rotation(