Fix/osc

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,25 +369,20 @@ 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]
                 j_eef_pinv = th.linalg.pinv(j_eef)
                 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

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)

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):

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!"

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):

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,

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()

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

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):

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

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}

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: