Revamp velocity controller with tested diff drive vel controller

facebookresearch · Nov 2, 2022 · 304a10e · 304a10e
1 parent 616f045
commit 304a10e
Showing 1 changed file with 92 additions and 82 deletions.
diff --git a/droidlet/lowlevel/hello_robot/remote/goto_controller.py b/droidlet/lowlevel/hello_robot/remote/goto_controller.py
@@ -9,27 +9,105 @@
 
 V_MAX_DEFAULT = 0.2  # base.params["motion"]["default"]["vel_m"]
 W_MAX_DEFAULT = 0.45  # (vel_m_max - vel_m_default) / wheel_separation_m
-ACC_LIN = 0.1  # 0.25 * base.params["motion"]["max"]["accel_m"]
-ACC_ANG = 0.3  # 0.25 * (accel_m_max - accel_m_default) / wheel_separation_m
+ACC_LIN = 0.4  # base.params["motion"]["max"]["accel_m"]
+ACC_ANG = 1.2  # (accel_m_max - accel_m_default) / wheel_separation_m
+MAX_HEADING_ANG = np.pi / 4
+
+
+class DiffDriveVelocityControl:
+    """
+    Control logic for differential drive robot velocity control
+    """
+
+    def __init__(self, hz):
+        self.v_max = V_MAX_DEFAULT
+        self.w_max = W_MAX_DEFAULT
+        self.lin_error_tol = self.v_max / hz
+        self.ang_error_tol = self.w_max / hz
+
+    @staticmethod
+    def _velocity_feedback_control(x_err, a, v_max):
+        """
+        Computes velocity based on distance from target (trapezoidal velocity profile).
+        Used for both linear and angular motion.
+        """
+        t = np.sqrt(2.0 * abs(x_err) / a)  # x_err = (1/2) * a * t^2
+        v = min(a * t, v_max)
+        return v * np.sign(x_err)
+
+    @staticmethod
+    def _turn_rate_limit(lin_err, heading_diff, w_max, tol=0.0):
+        """
+        Compute velocity limit that prevents path from overshooting goal
+
+        heading error decrease rate > linear error decrease rate
+        (w - v * np.sin(phi) / D) / phi > v * np.cos(phi) / D
+        v < (w / phi) / (np.sin(phi) / D / phi + np.cos(phi) / D)
+        v < w * D / (np.sin(phi) + phi * np.cos(phi))
+
+        (D = linear error, phi = angular error)
+        """
+        assert lin_err >= 0.0
+        assert heading_diff >= 0.0
+
+        if heading_diff > MAX_HEADING_ANG:
+            return 0.0
+        else:
+            return (
+                w_max
+                * lin_err
+                / (np.sin(heading_diff) + heading_diff * np.cos(heading_diff) + 1e-5)
+            )
+
+    def __call__(self, xyt_err):
+        v_cmd = w_cmd = 0
+
+        lin_err_abs = np.linalg.norm(xyt_err[0:2])
+        ang_err = xyt_err[2]
+
+        # Go to goal XY position if not there yet
+        if lin_err_abs > self.lin_error_tol:
+            heading_err = np.arctan2(xyt_err[1], xyt_err[0])
+            heading_err_abs = abs(heading_err)
+
+            # Compute linear velocity
+            v_raw = self._velocity_feedback_control(lin_err_abs, ACC_LIN, self.v_max)
+            v_limit = self._turn_rate_limit(
+                lin_err_abs,
+                heading_err_abs,
+                self.w_max / 2.0,
+                tol=self.lin_error_tol,
+            )
+            v_cmd = np.clip(v_raw, 0.0, v_limit)
+
+            # Compute angular velocity
+            w_cmd = self._velocity_feedback_control(heading_err, ACC_ANG, self.w_max)
+
+        # Rotate to correct yaw if XY position is at goal
+        elif abs(ang_err) > self.ang_error_tol:
+            # Compute angular velocity
+            w_cmd = self._velocity_feedback_control(ang_err, ACC_ANG, self.w_max)
+
+        return v_cmd, w_cmd
 
 
 class GotoVelocityController:
+    """
+    Self-contained controller module for moving a diff drive robot to a target goal.
+    Target goal is update-able at any given instant.
+    """
+
     def __init__(
         self,
         robot,
         hz: float,
-        v_max: Optional[float] = None,
-        w_max: Optional[float] = None,
     ):
         self.robot = robot
         self.hz = hz
         self.dt = 1.0 / self.hz
 
-        # Params
-        self.v_max = v_max or V_MAX_DEFAULT
-        self.w_max = w_max or W_MAX_DEFAULT
-        self.lin_error_tol = 2 * self.v_max / hz
-        self.ang_error_tol = 2 * self.w_max / hz
+        # Control module
+        self.control = DiffDriveVelocityControl(hz)
 
         # Initialize
         self.loop_thr = None
@@ -40,43 +118,6 @@ def __init__(
         self.xyt_goal = self.xyt_loc
         self.track_yaw = True
 
-    @staticmethod
-    def _velocity_feedback_control(x_err, a, v_max, tol=0.0, use_acc=True):
-        """
-        Computes velocity based on distance from target.
-        Used for both linear and angular motion.
-
-        Current implementation: Trapezoidal velocity profile
-        """
-        if use_acc:
-            t = np.sqrt(2.0 * max(abs(x_err) - tol, 0.0) / a)  # x_err = (1/2) * a * t^2
-            v = min(a * t, v_max)
-            return v * np.sign(x_err)
-        else:
-            return np.sign(x_err) * (abs(x_err) > tol) * v_max
-
-    @staticmethod
-    def _projection_velocity_multiplier(theta_diff, tol=0.0):
-        """
-        Compute velocity muliplier based on yaw (faster if facing towards target).
-        Used to control linear motion.
-
-        Current implementation:
-        Output = 1 when facing target, gradually decreases to 0 when angle to target is pi/3.
-        """
-        assert theta_diff >= 0.0
-        return 1.0 - np.sin(max(theta_diff - tol, 0.0))
-
-    @staticmethod
-    def _turn_rate_limit(lin_err, heading_diff, w_max, dead_zone=0.0):
-        """
-        Computed velocity limit based on the turning radius required to reach goal.
-        """
-        assert lin_err >= 0.0
-        assert heading_diff >= 0.0
-        dist_projected = lin_err * np.sin(heading_diff)
-        return w_max * max(dist_projected - dead_zone, 0.0)
-
     def _compute_error_pose(self):
         """
         Updates error based on robot localization
@@ -93,47 +134,16 @@ def _run(self):
         rate = rospy.Rate(self.hz)
 
         while True:
-            v_cmd = w_cmd = 0
+            # Get state estimation
             xyt_err = self._compute_error_pose()
 
-            lin_err_abs = np.linalg.norm(xyt_err[0:2])
-            ang_err = xyt_err[2]
-
-            # Go to goal XY position if not there yet
-            if lin_err_abs > self.lin_error_tol:
-                heading_err = np.arctan2(xyt_err[1], xyt_err[0])
-                heading_err_abs = abs(heading_err)
-
-                # Compute linear velocity
-                v_raw = self._velocity_feedback_control(
-                    lin_err_abs, ACC_LIN, self.v_max, tol=self.lin_error_tol
-                )
-                k_proj = self._projection_velocity_multiplier(
-                    heading_err_abs, tol=self.ang_error_tol
-                )
-                v_limit = self._turn_rate_limit(
-                    lin_err_abs,
-                    heading_err_abs,
-                    self.w_max / 2.0,
-                    dead_zone=2.0 * self.lin_error_tol,
-                )
-                v_cmd = min(k_proj * v_raw, v_limit)
-
-                # Compute angular velocity
-                w_cmd = self._velocity_feedback_control(
-                    heading_err, ACC_ANG, self.w_max, tol=self.ang_error_tol / 2.0, use_acc=False
-                )
-
-            # Rotate to correct yaw if yaw tracking is on and XY position is at goal
-            elif abs(ang_err) > self.ang_error_tol:
-                # Compute angular velocity
-                w_cmd = self._velocity_feedback_control(
-                    ang_err, ACC_ANG, self.w_max, tol=self.ang_error_tol, use_acc=False
-                )
+            # Compute control
+            v_cmd, w_cmd = self.control(xyt_err)
 
             # Command robot
             with self.control_lock:
-                self.robot.set_velocity(v_cmd, w_cmd)
+                if self.active:
+                    self.robot.set_velocity(v_cmd, w_cmd)
 
             # Spin
             rate.sleep()