Merge pull request #353 from StanfordVL/bug-fix

bug fix

omnigibson/examples/robots/robot_control_example.py

@@ -120,8 +120,9 @@ def main(random_selection=False, headless=False, short_exec=False):
         orientation=np.array([0.56829048, 0.09569975, 0.13571846, 0.80589577]),
     )
 
-    # Reset environment
+    # Reset environment and robot
     env.reset()
+    robot.reset()
 
     # Create teleop controller
     action_generator = KeyboardRobotController(robot=robot)

omnigibson/objects/controllable_object.py

@@ -376,9 +376,8 @@ def deploy_control(self, control, control_type, indices=None, normalized=False):
                  indices are being set. In this case, the length of @control must be the same length as @indices!
             indices (None or k-array): If specified, should be k (k < n) length array of specific DOF controls to deploy.
                 Default is None, which assumes that all joints are being set.
-            normalized (bool or array of bool): Whether the inputted joint controls should be interpreted as normalized
-                values. A single bool can be specified for the entire @control, or an array can be specified for
-                individual values. Default is False, corresponding to all @control assumed to be not normalized
+            normalized (bool): Whether the inputted joint controls should be interpreted as normalized
+                values. Expects a single bool for the entire @control. Default is False.
         """
         # Run sanity check
         if indices is None:
@@ -396,11 +395,13 @@ def deploy_control(self, control, control_type, indices=None, normalized=False):
 
         # Standardize normalized input
         n_indices = len(indices)
-        normalized = normalized if isinstance(normalized, Iterable) else [normalized] * n_indices
 
         # Loop through controls and deploy
         # We have to use delicate logic to account for the edge cases where a single joint may contain > 1 DOF
         # (e.g.: spherical joint)
+        pos_vec, pos_idxs, using_pos = [], [], False
+        vel_vec, vel_idxs, using_vel = [], [], False
+        eff_vec, eff_idxs, using_eff = [], [], False
         cur_indices_idx = 0
         while cur_indices_idx != n_indices:
             # Grab the current DOF index we're controlling and find the corresponding joint
@@ -428,21 +429,36 @@ def deploy_control(self, control, control_type, indices=None, normalized=False):
                 ctrl = control[cur_ctrl_idx]
 
             # Deploy control based on type
-            ctrl_type, norm = control_type[cur_ctrl_idx], normalized[cur_ctrl_idx]       # In multi-DOF joint case all values were already checked to be the same
+            ctrl_type = control_type[cur_ctrl_idx]       # In multi-DOF joint case all values were already checked to be the same
             if ctrl_type == ControlType.EFFORT:
-                joint.set_effort(ctrl, normalized=norm)
+                eff_vec.append(ctrl)
+                eff_idxs.append(cur_ctrl_idx)
+                using_eff = True
             elif ctrl_type == ControlType.VELOCITY:
-                joint.set_vel(ctrl, normalized=norm, drive=True)
+                vel_vec.append(ctrl)
+                vel_idxs.append(cur_ctrl_idx)
+                using_vel = True
             elif ctrl_type == ControlType.POSITION:
-                joint.set_pos(ctrl, normalized=norm, drive=True)
+                pos_vec.append(ctrl)
+                pos_idxs.append(cur_ctrl_idx)
+                using_pos = True
             elif ctrl_type == ControlType.NONE:
                 # Set zero efforts
-                joint.set_effort(0, normalized=False)
+                eff_vec.append(0)
+                eff_idxs.append(cur_ctrl_idx)
+                using_eff = True
             else:
                 raise ValueError("Invalid control type specified: {}".format(ctrl_type))
-
             # Finally, increment the current index based on how many DOFs were just controlled
             cur_indices_idx += joint_dof
+
+        # set the targets for joints
+        if using_pos:
+            self.set_joint_positions(positions=np.array(pos_vec), indices=np.array(pos_idxs), drive=True, normalized=normalized)
+        if using_vel:
+            self.set_joint_velocities(velocities=np.array(vel_vec), indices=np.array(vel_idxs), drive=True, normalized=normalized)
+        if using_eff:
+            self.set_joint_efforts(efforts=np.array(eff_vec), indices=np.array(eff_idxs), normalized=normalized)
 
     def get_control_dict(self):
         """

omnigibson/prims/joint_prim.py

@@ -387,7 +387,7 @@ def stiffness(self):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        stiffnesses, _ = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0]
+        stiffnesses = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0]
         return stiffnesses[0][0]
 
     @stiffness.setter
@@ -412,7 +412,7 @@ def damping(self):
         """
         # Only support revolute and prismatic joints for now
         assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
-        _, dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)[0]
+        dampings = self._articulation_view.get_gains(joint_indices=self.dof_indices)[1]
         return dampings[0][0]
 
     @damping.setter

omnigibson/prims/rigid_prim.py

@@ -89,6 +89,14 @@ def _post_load(self):
         # run super first
         super()._post_load()
 
+        # Apply rigid body and mass APIs
+        if not self._prim.HasAPI(UsdPhysics.RigidBodyAPI):
+            UsdPhysics.RigidBodyAPI.Apply(self._prim)
+        if not self._prim.HasAPI(PhysxSchema.PhysxRigidBodyAPI):
+            PhysxSchema.PhysxRigidBodyAPI.Apply(self._prim)
+        if not self._prim.HasAPI(UsdPhysics.MassAPI):
+            UsdPhysics.MassAPI.Apply(self._prim)
+
         # Only create contact report api if we're not visual only
         if not self._visual_only:
             PhysxSchema.PhysxContactReportAPI(self._prim) if \

omnigibson/robots/tiago.py

@@ -299,7 +299,7 @@ def _post_load(self):
         position, orientation = self.get_position_orientation()
         # Set the world-to-base fixed joint to be at the robot's current pose
         self._world_base_fixed_joint_prim.GetAttribute("physics:localPos0").Set(tuple(position))
-        self._world_base_fixed_joint_prim.GetAttribute("physics:localRot0").Set(Gf.Quatf(*orientation[[3, 0, 1, 2]]))
+        self._world_base_fixed_joint_prim.GetAttribute("physics:localRot0").Set(Gf.Quatf(*orientation[[3, 0, 1, 2]].tolist()))
 
     def _initialize(self):
         # Run super method first
@@ -734,8 +734,7 @@ def set_position_orientation(self, position=None, orientation=None):
             # Move the joint frame for the world_base_joint
             if self._world_base_fixed_joint_prim is not None:
                 self._world_base_fixed_joint_prim.GetAttribute("physics:localPos0").Set(tuple(position))
-                self._world_base_fixed_joint_prim.GetAttribute("physics:localRot0").Set(Gf.Quatf(
-                    orientation[3], orientation[0], orientation[1], orientation[2]))
+                self._world_base_fixed_joint_prim.GetAttribute("physics:localRot0").Set(Gf.Quatf(*orientation[[3, 0, 1, 2]].tolist()))
 
     def set_linear_velocity(self, velocity: np.ndarray):
         # Transform the desired linear velocity from the world frame to the root_link ("base_footprint_x") frame

omnigibson/scenes/scene_base.py

@@ -472,7 +472,7 @@ def reset(self):
         # Reset the states of all objects (including robots), including (non-)kinematic states and internal variables.
         assert self._initial_state is not None
         self.load_state(self._initial_state)
-        og.sim.step()
+        og.sim.step_physics()
 
     @property
     def n_floors(self):

setup.py

@@ -21,28 +21,28 @@
     zip_safe=False,
     packages=find_packages(),
     install_requires=[
-        "gym>=0.26",
-        "numpy>=1.20.0",
-        "GitPython",
-        "transforms3d>=0.3.1",
-        "networkx>=2.0",
-        "PyYAML",
-        "addict",
-        "ipython",
-        "future",
-        "trimesh",
-        "h5py",
-        "cryptography",
+        "gym~=0.26.2",
+        "numpy~=1.23.5",
+        "GitPython~=3.1.40",
+        "transforms3d~=0.4.1",
+        "networkx~=3.2.1",
+        "PyYAML~=6.0.1",
+        "addict~=2.4.0",
+        "ipython~=8.19.0",
+        "future~=0.18.3",
+        "trimesh~=4.0.8",
+        "h5py~=3.10.0",
+        "cryptography~=41.0.7",
         "bddl~=3.1.0",
-        "opencv-python",
-        "nest_asyncio",
-        "imageio",
-        "imageio-ffmpeg",
-        "termcolor",
-        "progressbar",
-        "pymeshlab",
-        "click",
-        "aenum",
+        "opencv-python~=4.8.1",
+        "nest_asyncio~=1.5.8",
+        "imageio~=2.33.1",
+        "imageio-ffmpeg~=0.4.9",
+        "termcolor~=2.4.0",
+        "progressbar~=2.5",
+        "pymeshlab~=2023.12",
+        "click~=8.1.7",
+        "aenum~=3.1.15",
     ],
     tests_require=[],
     python_requires=">=3",