Merge pull request #1019 from StanfordVL/feat/curobo-improved

Feat/curobo improved

.github/workflows/tests.yml

@@ -21,6 +21,7 @@ jobs:
       matrix:
         test_file:
           - test_controllers
+          - test_curobo
           - test_data_collection
           - test_dump_load_states
           - test_envs
@@ -51,7 +52,7 @@ jobs:
 
       - name: Install
         working-directory: omnigibson-src
-        run: pip install -e .[dev,primitives]
+        run: pip install -e .[dev,primitives] --no-build-isolation
 
       - name: Print env
         run: printenv

docker/prod.Dockerfile

@@ -44,7 +44,7 @@ RUN wget --no-verbose -O /cuda-keyring.deb https://developer.download.nvidia.com
   DEBIAN_FRONTEND=noninteractive apt-get install -y cuda-toolkit-11-8 && \
   TORCH_CUDA_ARCH_LIST='7.5;8.0;8.6+PTX' PATH=/usr/local/cuda-11.8/bin:$PATH LD_LIBRARY_PATH=/usr/local/cuda-11.8/lib64:$LD_LIBRARY_PATH \
     micromamba run -n omnigibson pip install \
-    git+https://github.com/StanfordVL/curobo@06d8c79b660db60c2881e9319e60899cbde5c5b5#egg=nvidia_curobo \
+    git+https://github.com/StanfordVL/curobo@6a4eb2ca8677829b0f57451ad107e0a3186525e9#egg=nvidia_curobo \
     --no-build-isolation > /dev/null && \
   apt-get remove -y cuda-toolkit-11-8 && apt-get autoremove -y && apt-get autoclean -y && rm -rf /var/lib/apt/lists/*
 

omnigibson/controllers/ik_controller.py

@@ -47,8 +47,9 @@ def __init__(
             th.tensor([-0.2, -0.2, -0.2, -0.5, -0.5, -0.5], dtype=th.float32),
             th.tensor([0.2, 0.2, 0.2, 0.5, 0.5, 0.5], dtype=th.float32),
         ),
-        kp=None,
-        damping_ratio=None,
+        pos_kp=None,
+        pos_damping_ratio=None,
+        vel_kp=None,
         use_impedances=True,
         mode="pose_delta_ori",
         smoothing_filter_size=None,
@@ -82,10 +83,12 @@ def __init__(
                 then all inputted command values will be scaled from the input range to the output range.
                 If either is None, no scaling will be used. If "default", then this range will automatically be set
                 to the @control_limits entry corresponding to self.control_type
-            kp (None or float): The proportional gain applied to the joint controller. If None, a default value
-                will be used. Only relevant if @use_impedances=True
-            damping_ratio (None or float): The damping ratio applied to the joint controller. If None, a default
-                value will be used. Only relevant if @use_impedances=True
+            pos_kp (None or float): If @motor_type is "position" and @use_impedances=True, this is the
+                proportional gain applied to the joint controller. If None, a default value will be used.
+            pos_damping_ratio (None or float): If @motor_type is "position" and @use_impedances=True, this is the
+                damping ratio applied to the joint controller. If None, a default value will be used.
+            vel_kp (None or float): If @motor_type is "velocity" and @use_impedances=True, this is the
+                proportional gain applied to the joint controller. If None, a default value will be used.
             use_impedances (bool): If True, will use impedances via the mass matrix to modify the desired efforts
                 applied
             mode (str): mode to use when computing IK. In all cases, position commands are 3DOF delta (dx,dy,dz)
@@ -172,8 +175,9 @@ def limiter(target_pos: Array[float], target_quat: Array[float], control_dict: D
             control_freq=control_freq,
             control_limits=control_limits,
             dof_idx=dof_idx,
-            kp=kp,
-            damping_ratio=damping_ratio,
+            pos_kp=pos_kp,
+            pos_damping_ratio=pos_damping_ratio,
+            vel_kp=vel_kp,
             motor_type="position",
             use_delta_commands=False,
             use_impedances=use_impedances,

omnigibson/controllers/joint_controller.py

@@ -43,8 +43,9 @@ def __init__(
         dof_idx,
         command_input_limits="default",
         command_output_limits="default",
-        kp=None,
-        damping_ratio=None,
+        pos_kp=None,
+        pos_damping_ratio=None,
+        vel_kp=None,
         use_impedances=False,
         use_gravity_compensation=False,
         use_cc_compensation=True,
@@ -74,10 +75,12 @@ def __init__(
                 then all inputted command values will be scaled from the input range to the output range.
                 If either is None, no scaling will be used. If "default", then this range will automatically be set
                 to the @control_limits entry corresponding to self.control_type
-            kp (None or float): If @motor_type is "position" or "velocity" and @use_impedances=True, this is the
+            pos_kp (None or float): If @motor_type is "position" and @use_impedances=True, this is the
                 proportional gain applied to the joint controller. If None, a default value will be used.
-            damping_ratio (None or float): If @motor_type is "position" and @use_impedances=True, this is the
+            pos_damping_ratio (None or float): If @motor_type is "position" and @use_impedances=True, this is the
                 damping ratio applied to the joint controller. If None, a default value will be used.
+            vel_kp (None or float): If @motor_type is "velocity" and @use_impedances=True, this is the
+                proportional gain applied to the joint controller. If None, a default value will be used.
             use_impedances (bool): If True, will use impedances via the mass matrix to modify the desired efforts
                 applied
             use_gravity_compensation (bool): If True, will add gravity compensation to the computed efforts. This is
@@ -97,16 +100,20 @@ def __init__(
 
         # Store control gains
         if self._motor_type == "position":
-            kp = m.DEFAULT_JOINT_POS_KP if kp is None else kp
-            damping_ratio = m.DEFAULT_JOINT_POS_DAMPING_RATIO if damping_ratio is None else damping_ratio
+            pos_kp = m.DEFAULT_JOINT_POS_KP if pos_kp is None else pos_kp
+            pos_damping_ratio = m.DEFAULT_JOINT_POS_DAMPING_RATIO if pos_damping_ratio is None else pos_damping_ratio
         elif self._motor_type == "velocity":
-            kp = m.DEFAULT_JOINT_VEL_KP if kp is None else kp
-            assert damping_ratio is None, "Cannot set damping_ratio for JointController with motor_type=velocity!"
+            vel_kp = m.DEFAULT_JOINT_VEL_KP if vel_kp is None else vel_kp
+            assert (
+                pos_damping_ratio is None
+            ), "Cannot set pos_damping_ratio for JointController with motor_type=velocity!"
         else:  # effort
-            assert kp is None, "Cannot set kp for JointController with motor_type=effort!"
-            assert damping_ratio is None, "Cannot set damping_ratio for JointController with motor_type=effort!"
-        self.kp = kp
-        self.kd = None if damping_ratio is None else 2 * math.sqrt(self.kp) * damping_ratio
+            assert pos_kp is None, "Cannot set pos_kp for JointController with motor_type=effort!"
+            assert pos_damping_ratio is None, "Cannot set pos_damping_ratio for JointController with motor_type=effort!"
+            assert vel_kp is None, "Cannot set vel_kp for JointController with motor_type=effort!"
+        self.pos_kp = pos_kp
+        self.pos_kd = None if pos_kp is None or pos_damping_ratio is None else 2 * math.sqrt(pos_kp) * pos_damping_ratio
+        self.vel_kp = vel_kp
         self._use_impedances = use_impedances
         self._use_gravity_compensation = use_gravity_compensation
         self._use_cc_compensation = use_cc_compensation
@@ -134,11 +141,10 @@ def __init__(
         )
 
     def _update_goal(self, command, control_dict):
-        # Compute the base value for the command
-        base_value = control_dict[f"joint_{self._motor_type}"][self.dof_idx]
-
         # If we're using delta commands, add this value
         if self._use_delta_commands:
+            # Compute the base value for the command
+            base_value = control_dict[f"joint_{self._motor_type}"][self.dof_idx]
 
             # Apply the command to the base value.
             target = base_value + command
@@ -198,11 +204,11 @@ def compute_control(self, goal_dict, control_dict):
                 # Run impedance controller -- effort = pos_err * kp + vel_err * kd
                 position_error = target - base_value
                 vel_pos_error = -control_dict[f"joint_velocity"][self.dof_idx]
-                u = position_error * self.kp + vel_pos_error * self.kd
+                u = position_error * self.pos_kp + vel_pos_error * self.pos_kd
             elif self._motor_type == "velocity":
                 # Compute command torques via PI velocity controller plus gravity compensation torques
                 velocity_error = target - base_value
-                u = velocity_error * self.kp
+                u = velocity_error * self.vel_kp
             else:  # effort
                 u = target
 

omnigibson/controllers/null_joint_controller.py

@@ -19,8 +19,9 @@ def __init__(
         command_input_limits="default",
         command_output_limits="default",
         default_command=None,
-        kp=None,
-        damping_ratio=None,
+        pos_kp=None,
+        pos_damping_ratio=None,
+        vel_kp=None,
         use_impedances=False,
     ):
         """
@@ -47,10 +48,12 @@ def __init__(
                 to the @control_limits entry corresponding to self.control_type
             default_command (None or n-array): if specified, should be the same length as @dof_idx, specifying
                 the default control for this controller to output
-            kp (None or float): If @motor_type is "position" or "velocity" and @use_impedances=True, this is the
+            pos_kp (None or float): If @motor_type is "position" and @use_impedances=True, this is the
                 proportional gain applied to the joint controller. If None, a default value will be used.
-            damping_ratio (None or float): If @motor_type is "position" and @use_impedances=True, this is the
+            pos_damping_ratio (None or float): If @motor_type is "position" and @use_impedances=True, this is the
                 damping ratio applied to the joint controller. If None, a default value will be used.
+            vel_kp (None or float): If @motor_type is "velocity" and @use_impedances=True, this is the
+                proportional gain applied to the joint controller. If None, a default value will be used.
             use_impedances (bool): If True, will use impedances via the mass matrix to modify the desired efforts
                 applied
         """
@@ -65,8 +68,9 @@ def __init__(
             dof_idx=dof_idx,
             command_input_limits=command_input_limits,
             command_output_limits=command_output_limits,
-            kp=kp,
-            damping_ratio=damping_ratio,
+            pos_kp=pos_kp,
+            pos_damping_ratio=pos_damping_ratio,
+            vel_kp=vel_kp,
             use_impedances=use_impedances,
             use_delta_commands=False,
         )

omnigibson/envs/env_base.py

@@ -266,11 +266,11 @@ def _load_robots(self):
             assert og.sim.is_stopped(), "Simulator must be stopped before loading robots!"
 
             # Iterate over all robots to generate in the robot config
-            for i, robot_config in enumerate(self.robots_config):
+            for robot_config in self.robots_config:
                 # Add a name for the robot if necessary
                 if "name" not in robot_config:
                     robot_config["name"] = "robot_" + "".join(random.choices(string.ascii_lowercase, k=6))
-
+                robot_config = deepcopy(robot_config)
                 position, orientation = robot_config.pop("position", None), robot_config.pop("orientation", None)
                 pose_frame = robot_config.pop("pose_frame", "scene")
                 if position is not None:
@@ -303,6 +303,7 @@ def _load_objects(self):
             if "name" not in obj_config:
                 obj_config["name"] = f"obj{i}"
             # Pop the desired position and orientation
+            obj_config = deepcopy(obj_config)
             position, orientation = obj_config.pop("position", None), obj_config.pop("orientation", None)
             # Make sure robot exists, grab its corresponding kwargs, and create / import the robot
             obj = create_class_from_registry_and_config(
@@ -334,6 +335,7 @@ def _load_external_sensors(self):
                 if "relative_prim_path" not in sensor_config:
                     sensor_config["relative_prim_path"] = f"/{sensor_config['name']}"
                 # Pop the desired position and orientation
+                sensor_config = deepcopy(sensor_config)
                 position, orientation = sensor_config.pop("position", None), sensor_config.pop("orientation", None)
                 pose_frame = sensor_config.pop("pose_frame", "scene")
                 # Pop whether or not to include this sensor in the observation