dynamically decide how actions are interpreted

Author: Abhiram824

robosuite/controllers/parts/mobile_base/joint_vel.py

@@ -206,6 +206,18 @@ def set_goal(self, action, set_qpos=None):
         curr_theta = T.mat2euler(curr_ori)[2]  # np.arctan2(curr_pos[1], curr_pos[0])
         theta = curr_theta - init_theta
 
+        forward_jnt = None
+        forward_jnt_axis = None
+        for jnt in self.joint_names:
+            if "joint_mobile_forward" in jnt:
+                forward_jnt = jnt
+                forward_jnt_axis = self.sim.model.jnt_axis[self.sim.model.joint_name2id(jnt)]
+                break
+        
+        # reorder action if forward axis is y axis
+        if forward_jnt and (forward_jnt_axis == np.array([0, 1, 0])).all():
+            action = np.copy([action[i] for i in [1, 0, 2]])
+
         # input raw base action is delta relative to current pose of base
         # controller expects deltas relative to initial pose of base at start of episode
         # transform deltas from current base pose coordinates to initial base pose coordinates