diff --git a/omnigibson/controllers/ik_controller.py b/omnigibson/controllers/ik_controller.py
index d1caca293..4a7f445b2 100644
--- a/omnigibson/controllers/ik_controller.py
+++ b/omnigibson/controllers/ik_controller.py
@@ -6,7 +6,7 @@
 from omnigibson.controllers import ControlType, ManipulationController
 from omnigibson.controllers.joint_controller import JointController
 from omnigibson.macros import create_module_macros, gm
-from omnigibson.utils.control_utils import IKSolver
+from omnigibson.utils.control_utils import IKSolver, orientation_error
 from omnigibson.utils.processing_utils import MovingAverageFilter
 from omnigibson.utils.python_utils import assert_valid_key
 from omnigibson.utils.ui_utils import create_module_logger
@@ -137,6 +137,12 @@ def limiter(target_pos: Array[float], target_quat: Array[float], control_dict: D
             else MovingAverageFilter(obs_dim=control_dim, filter_width=smoothing_filter_size)
         )
         assert mode in IK_MODES, f"Invalid ik mode specified! Valid options are: {IK_MODES}, got: {mode}"
+
+        # If mode is absolute pose, make sure command input limits / output limits are None
+        if mode == "absolute_pose":
+            assert command_input_limits is None, "command_input_limits should be None if using absolute_pose mode!"
+            assert command_output_limits is None, "command_output_limits should be None if using absolute_pose mode!"
+
         self.mode = mode
         self.workspace_pose_limiter = workspace_pose_limiter
         self.task_name = task_name
@@ -363,9 +369,8 @@ def compute_control(self, goal_dict, control_dict):
                 # Compute the pose error. Note that this is computed NOT in the EEF frame but still
                 # in the base frame.
                 pos_err = target_pos - pos_relative
-                quat_err = T.quat_mul(T.quat_inverse(quat_relative), target_quat)
-                aa_err = T.quat2axisangle(quat_err)
-                err = th.cat([pos_err, aa_err])
+                ori_err = orientation_error(T.quat2mat(target_quat), T.quat2mat(quat_relative))
+                err = th.cat([pos_err, ori_err])
 
                 # Use the jacobian to compute a local approximation
                 j_eef = control_dict[f"{self.task_name}_jacobian_relative"][:, self.dof_idx]
@@ -373,15 +378,11 @@ def compute_control(self, goal_dict, control_dict):
                 delta_j = j_eef_pinv @ err
                 target_joint_pos = current_joint_pos + delta_j
 
-                # Check if the target joint position is within the joint limits
-                if not th.all(
-                    th.logical_and(
-                        self._control_limits[ControlType.get_type("position")][0][self.dof_idx] <= target_joint_pos,
-                        target_joint_pos <= self._control_limits[ControlType.get_type("position")][1][self.dof_idx],
-                    )
-                ):
-                    # If not, we'll just use the current joint position
-                    target_joint_pos = None
+                # Clip values to be within the joint limits
+                target_joint_pos = target_joint_pos.clamp(
+                    min=self._control_limits[ControlType.get_type("position")][0][self.dof_idx],
+                    max=self._control_limits[ControlType.get_type("position")][1][self.dof_idx],
+                )
 
             if target_joint_pos is None:
                 # Print warning that we couldn't find a valid solution, and return the current joint configuration
diff --git a/omnigibson/controllers/osc_controller.py b/omnigibson/controllers/osc_controller.py
index 079a81a1b..fe823b20c 100644
--- a/omnigibson/controllers/osc_controller.py
+++ b/omnigibson/controllers/osc_controller.py
@@ -169,6 +169,12 @@ def limiter(target_pos: Array[float], target_quat: Array[float], control_dict: D
         # By default, the input limits are set as 1, so we modify this to have a correct range.
         # The output orientation limits are also set to be values assuming delta commands, so those are updated too
         assert_valid_key(key=mode, valid_keys=OSC_MODES, name="OSC mode")
+
+        # If mode is absolute pose, make sure command input limits / output limits are None
+        if mode == "absolute_pose":
+            assert command_input_limits is None, "command_input_limits should be None if using absolute_pose mode!"
+            assert command_output_limits is None, "command_output_limits should be None if using absolute_pose mode!"
+
         self.mode = mode
         if self.mode == "pose_absolute_ori":
             if command_input_limits is not None:
@@ -309,8 +315,8 @@ def _update_goal(self, command, control_dict):
                         frame to control, computed in its local frame (e.g.: robot base frame)
         """
         # Grab important info from control dict
-        pos_relative = th.tensor(control_dict[f"{self.task_name}_pos_relative"])
-        quat_relative = th.tensor(control_dict[f"{self.task_name}_quat_relative"])
+        pos_relative = control_dict[f"{self.task_name}_pos_relative"].clone()
+        quat_relative = control_dict[f"{self.task_name}_quat_relative"].clone()
 
         # Convert position command to absolute values if needed
         if self.mode == "absolute_pose":
@@ -434,8 +440,8 @@ def compute_control(self, goal_dict, control_dict):
 
     def compute_no_op_goal(self, control_dict):
         # No-op is maintaining current pose
-        target_pos = th.tensor(control_dict[f"{self.task_name}_pos_relative"])
-        target_quat = th.tensor(control_dict[f"{self.task_name}_quat_relative"])
+        target_pos = control_dict[f"{self.task_name}_pos_relative"].clone()
+        target_quat = control_dict[f"{self.task_name}_quat_relative"].clone()
 
         # Convert quat into eef ori mat
         return dict(
@@ -491,8 +497,11 @@ def _compute_osc_torques(
     # For angular velocity, this is just the base angular velocity
     # For linear velocity, this is the base linear velocity PLUS the net linear velocity experienced
     #   due to the base linear velocity
-    lin_vel_err = base_lin_vel + th.linalg.cross(base_ang_vel, ee_pos)
-    vel_err = th.cat((lin_vel_err, base_ang_vel)) - ee_vel
+    # For angular velocity, we need to make sure we compute the difference between the base and eef velocity
+    # properly, not simply "subtraction" as in the linear case
+    lin_vel_err = base_lin_vel + th.linalg.cross(base_ang_vel, ee_pos) - ee_vel[:3]
+    ang_vel_err = T.quat2axisangle(T.quat_multiply(T.axisangle2quat(-ee_vel[3:]), T.axisangle2quat(base_ang_vel)))
+    vel_err = th.cat((lin_vel_err, ang_vel_err))
 
     # Determine desired wrench
     err = th.unsqueeze(kp * err + kd * vel_err, dim=-1)
diff --git a/omnigibson/objects/controllable_object.py b/omnigibson/objects/controllable_object.py
index 3427c3c55..0ea7f3eed 100644
--- a/omnigibson/objects/controllable_object.py
+++ b/omnigibson/objects/controllable_object.py
@@ -260,7 +260,11 @@ def update_controller_mode(self):
                 self._joints[self.dof_names_ordered[dof]].set_control_type(
                     control_type=control_type,
                     kp=self.default_kp if control_type == ControlType.POSITION else None,
-                    kd=self.default_kd if control_type == ControlType.VELOCITY else None,
+                    kd=(
+                        self.default_kd
+                        if control_type == ControlType.POSITION or control_type == ControlType.VELOCITY
+                        else None
+                    ),
                 )
 
     def _generate_controller_config(self, custom_config=None):
@@ -561,15 +565,15 @@ def deploy_control(self, control, control_type):
         # set the targets for joints
         if using_pos:
             ControllableObjectViewAPI.set_joint_position_targets(
-                self.articulation_root_path, positions=th.tensor(pos_vec), indices=th.tensor(pos_idxs)
+                self.articulation_root_path, positions=th.tensor(pos_vec, dtype=th.float), indices=th.tensor(pos_idxs)
             )
         if using_vel:
             ControllableObjectViewAPI.set_joint_velocity_targets(
-                self.articulation_root_path, velocities=th.tensor(vel_vec), indices=th.tensor(vel_idxs)
+                self.articulation_root_path, velocities=th.tensor(vel_vec, dtype=th.float), indices=th.tensor(vel_idxs)
             )
         if using_eff:
             ControllableObjectViewAPI.set_joint_efforts(
-                self.articulation_root_path, efforts=th.tensor(eff_vec), indices=th.tensor(eff_idxs)
+                self.articulation_root_path, efforts=th.tensor(eff_vec, dtype=th.float), indices=th.tensor(eff_idxs)
             )
 
     def get_control_dict(self):
diff --git a/omnigibson/prims/joint_prim.py b/omnigibson/prims/joint_prim.py
index 9aff52f80..92831a50b 100644
--- a/omnigibson/prims/joint_prim.py
+++ b/omnigibson/prims/joint_prim.py
@@ -187,8 +187,9 @@ def set_control_type(self, control_type, kp=None, kd=None):
         assert_valid_key(key=control_type, valid_keys=ControlType.VALID_TYPES, name="control type")
         if control_type == ControlType.POSITION:
             assert kp is not None, "kp gain must be specified for setting POSITION control!"
-            assert kd is None, "kd gain must not be specified for setting POSITION control!"
-            kd = 0.0
+            if kd is None:
+                # kd could have bene optionally set, if not, then set 0 as default
+                kd = 0.0
         elif control_type == ControlType.VELOCITY:
             assert kp is None, "kp gain must not be specified for setting VELOCITY control!"
             assert kd is not None, "kd gain must be specified for setting VELOCITY control!"
diff --git a/omnigibson/prims/rigid_prim.py b/omnigibson/prims/rigid_prim.py
index 901da0bc4..d58a45e1e 100644
--- a/omnigibson/prims/rigid_prim.py
+++ b/omnigibson/prims/rigid_prim.py
@@ -496,7 +496,7 @@ def mass(self, mass):
         Args:
             mass (float): mass of the rigid body in kg.
         """
-        self._rigid_prim_view.set_masses([mass])
+        self._rigid_prim_view.set_masses(th.tensor([mass]))
 
     @property
     def density(self):
@@ -523,7 +523,7 @@ def density(self, density):
         Args:
             density (float): density of the rigid body in kg / m^3.
         """
-        self._rigid_prim_view.set_densities([density])
+        self._rigid_prim_view.set_densities(th.tensor([density]))
 
     @cached_property
     def kinematic_only(self):
diff --git a/omnigibson/robots/fetch.py b/omnigibson/robots/fetch.py
index ef6311484..2d5c3f91a 100644
--- a/omnigibson/robots/fetch.py
+++ b/omnigibson/robots/fetch.py
@@ -63,7 +63,7 @@ def __init__(
         disable_grasp_handling=False,
         # Unique to Fetch
         rigid_trunk=False,
-        default_trunk_offset=0.365,
+        default_trunk_offset=0.2,
         default_reset_mode="untuck",
         default_arm_pose="vertical",
         **kwargs,
diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py
index 763f40e96..9112c7de6 100644
--- a/omnigibson/robots/manipulation_robot.py
+++ b/omnigibson/robots/manipulation_robot.py
@@ -413,11 +413,14 @@ def _add_arm_control_dict(self, fcns, arm):
         # -n_joints because there may be an additional 6 entries at the beginning of the array, if this robot does
         # not have a fixed base (i.e.: the 6DOF --> "floating" joint)
         # see self.get_relative_jacobian() for more info
+        # We also count backwards for the link frame because if the robot is fixed base, the jacobian returned has one
+        # less index than the number of links. This is presumably because the 1st link of a fixed base robot will
+        # always have a zero jacobian since it can't move. Counting backwards resolves this issue.
         start_idx = 0 if self.fixed_base else 6
         eef_link_idx = self._articulation_view.get_body_index(self.eef_links[arm].body_name)
         fcns[f"eef_{arm}_jacobian_relative"] = lambda: ControllableObjectViewAPI.get_relative_jacobian(
             self.articulation_root_path
-        )[eef_link_idx, :, start_idx : start_idx + self.n_joints]
+        )[-(self.n_links - eef_link_idx), :, start_idx : start_idx + self.n_joints]
 
     def _get_proprioception_dict(self):
         dic = super()._get_proprioception_dict()
diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py
index ed93baf33..472ed0439 100644
--- a/omnigibson/robots/tiago.py
+++ b/omnigibson/robots/tiago.py
@@ -72,9 +72,9 @@ def __init__(
         # Unique to Tiago
         variant="default",
         rigid_trunk=False,
-        default_trunk_offset=0.365,
+        default_trunk_offset=0.2,
         default_reset_mode="untuck",
-        default_arm_pose="vertical",
+        default_arm_pose="diagonal15",
         **kwargs,
     ):
         """
@@ -214,15 +214,15 @@ def untucked_default_joint_pos(self):
                 )
             elif self.default_arm_pose == "diagonal15":
                 pos[self.arm_control_idx[arm]] = th.tensor(
-                    [0.90522, -0.42811, 2.23505, 1.64627, 0.76867, -0.79464, 2.05251]
+                    [0.90522, -0.42811, 2.23505, 1.64627, 0.76867, -0.79464, -1.08908]
                 )
             elif self.default_arm_pose == "diagonal30":
                 pos[self.arm_control_idx[arm]] = th.tensor(
-                    [0.71883, -0.02787, 1.86002, 1.52897, 0.52204, -0.99741, 2.03113]
+                    [0.71883, -0.02787, 1.86002, 1.52897, 0.52204, -0.99741, -1.11046]
                 )
             elif self.default_arm_pose == "diagonal45":
                 pos[self.arm_control_idx[arm]] = th.tensor(
-                    [0.66058, -0.14251, 1.77547, 1.43345, 0.65988, -1.02741, 1.81302]
+                    [0.66058, -0.14251, 1.77547, 1.43345, 0.65988, -1.02741, -1.32857]
                 )
             elif self.default_arm_pose == "horizontal":
                 pos[self.arm_control_idx[arm]] = th.tensor(
@@ -337,18 +337,6 @@ def control_limits(self):
         limits["velocity"][1][self.base_idx[3:]] = m.MAX_ANGULAR_VELOCITY
         return limits
 
-    def get_control_dict(self):
-        # Modify the right hand's pos_relative in the z-direction based on the trunk's value
-        # We do this so we decouple the trunk's dynamic value from influencing the IK controller solution for the right
-        # hand, which does not control the trunk
-        fcns = super().get_control_dict()
-        native_fcn = fcns.get_fcn("eef_right_pos_relative")
-        fcns["eef_right_pos_relative"] = lambda: (
-            native_fcn() + th.tensor([0, 0, -self.get_joint_positions()[self.trunk_control_idx[0]]])
-        )
-
-        return fcns
-
     @property
     def default_proprio_obs(self):
         obs_keys = super().default_proprio_obs
diff --git a/omnigibson/scenes/scene_base.py b/omnigibson/scenes/scene_base.py
index 0c4647213..afaf36b87 100644
--- a/omnigibson/scenes/scene_base.py
+++ b/omnigibson/scenes/scene_base.py
@@ -180,7 +180,7 @@ def robots(self):
         Returns:
             list of BaseRobot: Robot(s) that are currently in this scene
         """
-        return list(self.object_registry("category", robot_macros.ROBOT_CATEGORY, []))
+        return list(sorted(self.object_registry("category", robot_macros.ROBOT_CATEGORY, []), key=lambda x: x.name))
 
     @property
     def systems(self):
diff --git a/omnigibson/utils/transform_utils.py b/omnigibson/utils/transform_utils.py
index 1413a28ec..fe35d60bb 100644
--- a/omnigibson/utils/transform_utils.py
+++ b/omnigibson/utils/transform_utils.py
@@ -697,7 +697,7 @@ def axisangle2quat(vec, eps=1e-6):
     angle = th.norm(vec, dim=-1, keepdim=True)
 
     # Create return array
-    quat = th.zeros(th.prod(th.tensor(input_shape)), 4, device=vec.device)
+    quat = th.zeros(th.prod(th.tensor(input_shape, dtype=th.int)), 4, device=vec.device)
     quat[:, 3] = 1.0
 
     # Grab indexes where angle is not zero an convert the input to its quaternion form
diff --git a/omnigibson/utils/ui_utils.py b/omnigibson/utils/ui_utils.py
index d2757134a..d8b0ccf47 100644
--- a/omnigibson/utils/ui_utils.py
+++ b/omnigibson/utils/ui_utils.py
@@ -790,7 +790,7 @@ def keyboard_event_handler(self, event, *args, **kwargs):
                 )
                 new_arm = self.ik_arms[self.active_arm_idx]
                 self.keypress_mapping.update(self.generate_ik_keypress_mapping(self.controller_info[new_arm]))
-                print(f"Now controlling arm {new_arm} with IK")
+                print(f"Now controlling arm {new_arm} EEF")
 
             elif (
                 event.input in {lazy.carb.input.KeyboardInput.KEY_5, lazy.carb.input.KeyboardInput.KEY_6}
diff --git a/omnigibson/utils/usd_utils.py b/omnigibson/utils/usd_utils.py
index 62744471a..a338c09cd 100644
--- a/omnigibson/utils/usd_utils.py
+++ b/omnigibson/utils/usd_utils.py
@@ -926,13 +926,27 @@ def get_position_orientation(self, prim_path):
             return self.get_root_transform(prim_path)
 
     def get_linear_velocity(self, prim_path):
+        if self._base_footprint_link_names[prim_path] is not None:
+            link_name = self._base_footprint_link_names[prim_path]
+            return self.get_link_linear_velocity(prim_path, link_name)
+        else:
+            return self.get_root_linear_velocity(prim_path)
+
+    def get_angular_velocity(self, prim_path):
+        if self._base_footprint_link_names[prim_path] is not None:
+            link_name = self._base_footprint_link_names[prim_path]
+            return self.get_link_angular_velocity(prim_path, link_name)
+        else:
+            return self.get_root_angular_velocity(prim_path)
+
+    def get_root_linear_velocity(self, prim_path):
         if "root_velocities" not in self._read_cache:
             self._read_cache["root_velocities"] = self._view.get_root_velocities()
 
         idx = self._idx[prim_path]
         return self._read_cache["root_velocities"][idx][:3]
 
-    def get_angular_velocity(self, prim_path):
+    def get_root_angular_velocity(self, prim_path):
         if "root_velocities" not in self._read_cache:
             self._read_cache["root_velocities"] = self._view.get_root_velocities()
 
@@ -942,12 +956,14 @@ def get_angular_velocity(self, prim_path):
     def get_relative_linear_velocity(self, prim_path):
         orn = self.get_position_orientation(prim_path)[1]
         linvel = self.get_linear_velocity(prim_path)
+        # x.T --> transpose (inverse) orientation
         return T.quat2mat(orn).T @ linvel
 
     def get_relative_angular_velocity(self, prim_path):
         orn = self.get_position_orientation(prim_path)[1]
         angvel = self.get_angular_velocity(prim_path)
-        return T.mat2euler(T.quat2mat(orn).T @ T.euler2mat(angvel))
+        # x.T --> transpose (inverse) orientation
+        return T.quat2mat(orn).T @ angvel
 
     def get_joint_positions(self, prim_path):
         if "dof_positions" not in self._read_cache:
@@ -1009,7 +1025,7 @@ def get_link_relative_position_orientation(self, prim_path, link_name):
         # Compute the relative position and orientation
         return T.relative_pose_transform(pos, orn, world_pos, world_orn)
 
-    def get_link_relative_linear_velocity(self, prim_path, link_name):
+    def get_link_linear_velocity(self, prim_path, link_name):
         if "link_velocities" not in self._read_cache:
             self._read_cache["link_velocities"] = self._view.get_link_velocities()
 
@@ -1018,13 +1034,18 @@ def get_link_relative_linear_velocity(self, prim_path, link_name):
         vel = self._read_cache["link_velocities"][idx][link_idx]
         linvel = vel[:3]
 
+        return linvel
+
+    def get_link_relative_linear_velocity(self, prim_path, link_name):
+        linvel = self.get_link_linear_velocity(prim_path, link_name)
+
         # Get the root world transform too
         _, world_orn = self.get_position_orientation(prim_path)
 
         # Compute the relative position and orientation
         return T.quat2mat(world_orn).T @ linvel
 
-    def get_link_relative_angular_velocity(self, prim_path, link_name):
+    def get_link_angular_velocity(self, prim_path, link_name):
         if "link_velocities" not in self._read_cache:
             self._read_cache["link_velocities"] = self._view.get_link_velocities()
 
@@ -1033,11 +1054,16 @@ def get_link_relative_angular_velocity(self, prim_path, link_name):
         vel = self._read_cache["link_velocities"][idx][link_idx]
         angvel = vel[3:]
 
+        return angvel
+
+    def get_link_relative_angular_velocity(self, prim_path, link_name):
+        angvel = self.get_link_angular_velocity(prim_path, link_name)
+
         # Get the root world transform too
         _, world_orn = self.get_position_orientation(prim_path)
 
         # Compute the relative position and orientation
-        return T.mat2euler(T.quat2mat(world_orn).T @ T.euler2mat(angvel))
+        return T.quat2mat(world_orn).T @ angvel
 
     def get_jacobian(self, prim_path):
         if "jacobians" not in self._read_cache:
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
new file mode 100644
index 000000000..07142882c
--- /dev/null
+++ b/tests/test_controllers.py
@@ -0,0 +1,291 @@
+import numpy as np
+import pytest
+import torch as th
+
+import omnigibson as og
+import omnigibson.utils.transform_utils as T
+from omnigibson.robots import LocomotionRobot
+
+
+def test_arm_control():
+    # Create env
+    cfg = {
+        "scene": {
+            "type": "Scene",
+        },
+        "objects": [],
+        "robots": [
+            {
+                "type": "FrankaPanda",
+                "obs_modalities": "rgb",
+                "position": [150, 150, 100],
+                "orientation": [0, 0, 0, 1],
+                "action_normalize": False,
+            },
+            {
+                "type": "Fetch",
+                "default_trunk_offset": 0.2,
+                "obs_modalities": "rgb",
+                "position": [150, 150, 105],
+                "orientation": [0, 0, 0, 1],
+                "action_normalize": False,
+            },
+            {
+                "type": "Tiago",
+                "obs_modalities": "rgb",
+                "position": [150, 150, 110],
+                "orientation": [0, 0, 0, 1],
+                "action_normalize": False,
+            },
+        ],
+    }
+    env = og.Environment(configs=cfg)
+
+    # Define error functions to use
+    def check_zero_error(curr_position, init_position, tol=1e-2):
+        return th.norm(curr_position - init_position).item() < tol
+
+    def check_forward_error(curr_position, init_position, tol=1e-2, forward_tol=1e-2):
+        # x should be positive
+        return (curr_position[0] - init_position[0]).item() > forward_tol and th.norm(
+            curr_position[[1, 2]] - init_position[[1, 2]]
+        ).item() < tol
+
+    def check_side_error(curr_position, init_position, tol=1e-2, side_tol=1e-2):
+        # y should be positive
+        return (curr_position[1] - init_position[1]).item() > side_tol and th.norm(
+            curr_position[[0, 2]] - init_position[[0, 2]]
+        ).item() < tol
+
+    def check_up_error(curr_position, init_position, tol=1e-2, up_tol=1e-2):
+        # z should be positive
+        return (curr_position[2] - init_position[2]).item() > up_tol and th.norm(
+            curr_position[[0, 1]] - init_position[[0, 1]]
+        ).item() < tol
+
+    def check_ori_error(curr_orientation, init_orientation, tol=0.1):
+        ori_err_normalized = th.norm(
+            T.quat2axisangle(T.mat2quat(T.quat2mat(init_orientation).T @ T.quat2mat(curr_orientation)))
+        ).item() / (np.pi * 2)
+        ori_err = np.abs(np.pi * 2 * (np.round(ori_err_normalized) - ori_err_normalized))
+        return ori_err < tol
+
+    # All functions take in (target, curr, init) tuple
+    err_checks = {
+        "pose_delta_ori": {
+            "zero": {
+                "pos": lambda target, curr, init: check_zero_error(curr, init),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "forward": {
+                "pos": lambda target, curr, init: check_forward_error(curr, init),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "side": {
+                "pos": lambda target, curr, init: check_side_error(curr, init),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "up": {
+                "pos": lambda target, curr, init: check_up_error(curr, init),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "rotate": {
+                "pos": lambda target, curr, init: check_zero_error(curr, init),
+                "ori": None,
+            },
+            "base_move": {
+                "pos": lambda target, curr, init: check_zero_error(
+                    curr, init, tol=0.02
+                ),  # Slightly bigger tolerance with base moving
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+        },
+        "absolute_pose": {
+            "zero": {
+                "pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "forward": {
+                "pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "side": {
+                "pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "up": {
+                "pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+            "rotate": {
+                "pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
+                "ori": lambda target, curr, init: check_ori_error(target, curr, tol=0.05),
+            },
+            "base_move": {
+                "pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
+                "ori": lambda target, curr, init: check_ori_error(curr, init),
+            },
+        },
+    }
+
+    n_steps = {
+        "pose_delta_ori": {
+            "zero": 10,
+            "forward": 10,
+            "side": 10,
+            "up": 10,
+            "rotate": 10,
+            "base_move": 30,
+        },
+        "absolute_pose": {
+            "zero": 50,
+            "forward": 50,
+            "side": 50,
+            "up": 50,
+            "rotate": 50,
+            "base_move": 50,
+        },
+    }
+
+    for controller in ["InverseKinematicsController", "OperationalSpaceController"]:
+
+        for controller_mode in ["pose_delta_ori", "absolute_pose"]:
+            controller_kwargs = {
+                "mode": controller_mode,
+            }
+            if controller_mode == "absolute_pose":
+                controller_kwargs["command_input_limits"] = None
+                controller_kwargs["command_output_limits"] = None
+            actions = {
+                "zero": dict(),
+                "forward": dict(),
+                "side": dict(),
+                "up": dict(),
+                "rotate": dict(),
+                "base_move": dict(),
+            }
+
+            for i, robot in enumerate(env.robots):
+                controller_config = {f"arm_{arm}": {"name": controller, **controller_kwargs} for arm in robot.arm_names}
+                robot.set_position_orientation(
+                    th.tensor([0.0, i * 5.0, 0.0]), T.euler2quat(th.tensor([0.0, 0.0, np.pi / 3]))
+                )
+                robot.reset()
+                robot.keep_still()
+                robot.reload_controllers(controller_config)
+
+                for _ in range(10):
+                    og.sim.step_physics()
+
+                # Define actions to use
+                zero_action = th.zeros(robot.action_dim)
+                forward_action = th.zeros(robot.action_dim)
+                side_action = th.zeros(robot.action_dim)
+                up_action = th.zeros(robot.action_dim)
+                rot_action = th.zeros(robot.action_dim)
+
+                # Populate actions and targets
+                for arm in robot.arm_names:
+                    c_name = f"arm_{arm}"
+                    start_idx = 0
+                    init_eef_pos, init_eef_quat = robot.get_relative_eef_pose(arm)
+                    for c in robot.controller_order:
+                        if c == c_name:
+                            break
+                        start_idx += robot.controllers[c].command_dim
+                    if controller_mode == "pose_delta_ori":
+                        forward_action[start_idx] = 0.1
+                        side_action[start_idx + 1] = 0.1
+                        up_action[start_idx + 2] = 0.1
+                        rot_action[start_idx + 3] = 0.1
+                    elif controller_mode == "absolute_pose":
+                        for act in [zero_action, forward_action, side_action, up_action, rot_action]:
+                            act[start_idx : start_idx + 3] = init_eef_pos.clone()
+                            act[start_idx + 3 : start_idx + 6] = T.quat2axisangle(init_eef_quat.clone())
+                        forward_action[start_idx] += 0.1
+                        side_action[start_idx + 1] += 0.1
+                        up_action[start_idx + 2] += 0.1
+                        rot_action[start_idx + 3 : start_idx + 6] = T.quat2axisangle(
+                            T.quat_multiply(T.euler2quat(th.tensor([th.pi / 10, 0, 0])), init_eef_quat.clone())
+                        )
+
+                    else:
+                        raise ValueError(f"Got invalid controller mode: {controller}")
+                actions["zero"][robot.name] = zero_action
+                actions["forward"][robot.name] = forward_action
+                actions["side"][robot.name] = side_action
+                actions["up"][robot.name] = up_action
+                actions["rotate"][robot.name] = rot_action
+
+                # Add base movement action if locomotion robot
+                base_move_action = zero_action.clone()
+                if isinstance(robot, LocomotionRobot):
+                    c_name = "base"
+                    start_idx = 0
+                    for c in robot.controller_order:
+                        if c == c_name:
+                            break
+                        start_idx += robot.controllers[c].command_dim
+                    base_move_action[start_idx] = 0.1
+                actions["base_move"][robot.name] = base_move_action
+
+            # Take 5 steps
+            for _ in range(5):
+                env.step(actions["zero"])
+
+            # Update initial state
+            env.scene.update_initial_state()
+
+            # For each action set, reset all robots, then run actions and see if arm moves in expected way
+            for action_name, action in actions.items():
+
+                # Reset env
+                env.reset()
+                for robot in env.robots:
+                    robot.keep_still()
+
+                # Record initial poses
+                initial_eef_pose = dict()
+                for i, robot in enumerate(env.robots):
+                    initial_eef_pose[robot.name] = {
+                        arm: robot.get_relative_eef_pose(arm=arm) for arm in robot.arm_names
+                    }
+
+                # Take 10 steps with given action and check for error
+                for _ in range(n_steps[controller_mode][action_name]):
+                    env.step(action)
+
+                for i, robot in enumerate(env.robots):
+                    for arm in robot.arm_names:
+
+                        # Make sure no arm joints are at their limit
+                        normalized_qpos = robot.get_joint_positions(normalized=True)[robot.arm_control_idx[arm]]
+                        assert not th.any(th.abs(normalized_qpos) == 1.0), (
+                            f"controller [{controller}], mode [{controller_mode}], robot [{robot.model_name}], arm [{arm}], action [{action_name}]:\n"
+                            f"Some joints are at their limit (normalized values): {normalized_qpos}"
+                        )
+
+                        init_pos, init_quat = initial_eef_pose[robot.name][arm]
+                        curr_pos, curr_quat = robot.get_relative_eef_pose(arm=arm)
+                        arm_controller = robot.controllers[f"arm_{arm}"]
+                        arm_goal = arm_controller.goal
+                        target_pos = arm_goal["target_pos"]
+                        target_quat = (
+                            arm_goal["target_quat"]
+                            if controller == "InverseKinematicsController"
+                            else T.mat2quat(arm_goal["target_ori_mat"])
+                        )
+                        pos_check = err_checks[controller_mode][action_name]["pos"]
+                        if pos_check is not None:
+                            is_valid_pos = pos_check(target_pos, curr_pos, init_pos)
+                            assert is_valid_pos, (
+                                f"Got mismatch for controller [{controller}], mode [{controller_mode}], robot [{robot.model_name}], action [{action_name}]\n"
+                                f"target_pos: {target_pos}, curr_pos: {curr_pos}, init_pos: {init_pos}"
+                            )
+                        ori_check = err_checks[controller_mode][action_name]["ori"]
+                        if ori_check is not None:
+                            is_valid_ori = ori_check(target_quat, curr_quat, init_quat)
+                            assert is_valid_ori, (
+                                f"Got mismatch for controller [{controller}], mode [{controller_mode}], robot [{robot.model_name}], action [{action_name}]\n"
+                                f"target_quat: {target_quat}, curr_quat: {curr_quat}, init_quat: {init_quat}"
+                            )