Debug and tune goto controller for Habitat-Lab (#54)

* Debug coordinate transform

* Tune sim config

* Modify tests

* Rename configurations to avoid confusion

* Debug coordinate transform

Author: exhaustin

src/home_robot/config/control/noplan_velocity_hw.yaml

@@ -5,8 +5,8 @@ acc_lin: 0.2  # 0.5 * base.params["motion"]["max"]["accel_m"]
 acc_ang: 0.6  # 0.5 * (accel_m_max - accel_m_default) / wheel_separation_m
 
 # Tolerances for determining whether the goal position or orientation is reached
-tol_lin: 0.1
-tol_ang: 0.2
+lin_error_tol: 0.1
+ang_error_tol: 0.2
 
 # Maximum angle error at which the controller would still exert linear motion
 max_heading_ang: 0.7854  # 45 degrees

src/home_robot/config/control/noplan_velocity_sim.yaml

@@ -1,12 +1,12 @@
 # Parameters of the trapezoidal velocity profile (acceleration & max speed)
-v_max: 0.2  # base.params["motion"]["default"]["vel_m"]
-w_max: 0.45  # (vel_m_max - vel_m_default) / wheel_separation_m
-acc_lin: 0.2  # 0.5 * base.params["motion"]["max"]["accel_m"]
-acc_ang: 0.6  # 0.5 * (accel_m_max - accel_m_default) / wheel_separation_m
+v_max: 0.3
+w_max: 0.45
+acc_lin: 0.2  # base.params["motion"]["max"]["accel_m"]
+acc_ang: 0.6  # (accel_m_max - accel_m_default) / wheel_separation_m
 
 # Tolerances for determining whether the goal position or orientation is reached
-tol_lin: 0.025
-tol_ang: 0.087  # 5 degrees
+lin_error_tol: 0.01
+ang_error_tol: 0.025
 
 # Maximum angle error at which the controller would still exert linear motion
 max_heading_ang: 0.7854  # 45 degrees

src/home_robot/home_robot/control/feedback/velocity_controllers.py

@@ -72,7 +72,7 @@ def __call__(self, xyt_err):
         heading_err_abs = abs(heading_err)
 
         # Go to goal XY position if not there yet
-        if lin_err_abs > self.cfg.tol_lin:
+        if lin_err_abs > self.cfg.lin_error_tol:
             # Compute linear velocity -- move towards goal XY
             v_raw = self._velocity_feedback_control(
                 lin_err_abs, self.cfg.acc_lin, self.cfg.v_max
@@ -90,7 +90,7 @@ def __call__(self, xyt_err):
             )
 
         # Rotate to correct yaw if XY position is at goal
-        elif abs(ang_err) > self.cfg.tol_ang:
+        elif abs(ang_err) > self.cfg.ang_error_tol:
             # Compute angular velocity -- turn to goal orientation
             w_cmd = self._velocity_feedback_control(
                 ang_err, self.cfg.acc_ang, self.cfg.w_max

src/home_robot/home_robot/control/goto_controller.py

@@ -60,12 +60,12 @@ def xyt_base_to_global(xyt_base2target, xyt_world2base):
     """
     base_cos = np.cos(xyt_world2base[2])
     base_sin = np.sin(xyt_world2base[2])
-    x_base2target = xyt_base2target[0] * base_cos - xyt_base2target[1] * base_sin
-    y_base2target = xyt_base2target[1] * base_sin + xyt_base2target[1] * base_cos
+    x_base2target_global = xyt_base2target[0] * base_cos - xyt_base2target[1] * base_sin
+    y_base2target_global = xyt_base2target[0] * base_sin + xyt_base2target[1] * base_cos
 
     xyt_world2target = np.zeros(3)
-    xyt_world2target[0] = xyt_world2base[0] + x_base2target
-    xyt_world2target[1] = xyt_world2base[1] + y_base2target
+    xyt_world2target[0] = xyt_world2base[0] + x_base2target_global
+    xyt_world2target[1] = xyt_world2base[1] + y_base2target_global
     xyt_world2target[2] = xyt_world2base[2] + xyt_base2target[2]
 
     return xyt_world2target

tests/home_robot/control/test_xyt_data.json

@@ -1 +1 @@
-[[[0.0, [1.1227949188291289, 0.3028052195530815, 0.07085925794500358], 1.0, [0.07304142216313661, -0.8427240538292873, 1.9699244478698945]], [0.0, -0.45]], [[0.0, [-0.055035123059621, -0.10731045224678325, 1.3654671795124211], 0.0, [-0.09769572171942498, 0.7610377251469934, 0.12167501649282841]], [0.0, -0.45]], [[1.0, [1.2206080970461388, -1.3394955497146936, 0.42837336964863926], 1.0, [-0.12346314911436336, -0.18158257273119596, 1.4102046311312142]], [0.0, 0.45]], [[1.0, [1.1682731374280053, 0.9471859464018728, 1.0854870347559817], 0.0, [2.3822244479133983, -2.5529898158340787, 0.6536185954403606]], [0.0, 0.45]], [[1.0, [0.5912428062842595, -0.782775504266812, -0.4442328269528734], 0.0, [-0.34518615833569954, 1.4627404474484047, 1.5350291291191829]], [0.0, 0.45]], [[0.0, [0.8131012718382519, -0.2292506291606075, 2.161717373494259], 1.0, [-0.9569314292805337, -0.3479121493261526, 0.15634896910398005]], [0.0, 0.45]], [[1.0, [-1.0261787778523443, 0.47752546922509714, 1.2926982289964546], 1.0, [-0.7314582356018166, 0.9957113486470573, -1.9332047794071159]], [0.2, -0.45]], [[0.0, [0.4053977840180792, 0.11860658523361353, 1.2544140683375582], 0.0, [1.419102040788718, -1.2527953600499262, 0.7774903558319101]], [0.0, 0.45]], [[1.0, [2.2997715236093397, -0.47113526084130336, 1.262715483788719], 0.0, [-1.1705154492631553, -0.30736481038200775, -0.36652394095310614]], [0.0, 0.45]], [[0.0, [1.5094450822654228, 1.0677751294644207, -0.6865894771933623], 1.0, [0.01487331605783535, -0.6343220936809636, -0.3627411659871381]], [0.0, -0.45]]]
+[[[0.0, [1.1227949188291289, 0.3028052195530815, 0.07085925794500358], 1.0, [0.07304142216313661, -0.8427240538292873, 1.9699244478698945]], [0.0, -0.45]], [[0.0, [-0.055035123059621, -0.10731045224678325, 1.3654671795124211], 0.0, [-0.09769572171942498, 0.7610377251469934, 0.12167501649282841]], [0.0, -0.45]], [[1.0, [1.2206080970461388, -1.3394955497146936, 0.42837336964863926], 1.0, [-0.12346314911436336, -0.18158257273119596, 1.4102046311312142]], [0.0, 0.45]], [[1.0, [1.1682731374280053, 0.9471859464018728, 1.0854870347559817], 0.0, [2.3822244479133983, -2.5529898158340787, 0.6536185954403606]], [0.0, 0.45]], [[1.0, [0.5912428062842595, -0.782775504266812, -0.4442328269528734], 0.0, [-0.34518615833569954, 1.4627404474484047, 1.5350291291191829]], [0.0, 0.45]], [[0.0, [0.8131012718382519, -0.2292506291606075, 2.161717373494259], 1.0, [-0.9569314292805337, -0.3479121493261526, 0.15634896910398005]], [0.0, 0.45]], [[1.0, [-1.0261787778523443, 0.47752546922509714, 1.2926982289964546], 1.0, [-0.7314582356018166, 0.9957113486470573, -1.9332047794071159]], [0.2859898127198834, -0.45]], [[0.0, [0.4053977840180792, 0.11860658523361353, 1.2544140683375582], 0.0, [1.419102040788718, -1.2527953600499262, 0.7774903558319101]], [0.0, 0.45]], [[1.0, [2.2997715236093397, -0.47113526084130336, 1.262715483788719], 0.0, [-1.1705154492631553, -0.30736481038200775, -0.36652394095310614]], [0.0, 0.45]], [[0.0, [1.5094450822654228, 1.0677751294644207, -0.6865894771933623], 1.0, [0.01487331605783535, -0.6343220936809636, -0.3627411659871381]], [0.0, -0.45]]]