check for MotionType.DYNAMIC before trying to set joint motors (#1648)

facebookresearch · Oct 23, 2023 · afe4058 · afe4058
1 parent 8dc9aed
commit afe4058
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 13 deletions.
diff --git a/habitat-lab/habitat/articulated_agents/manipulator.py b/habitat-lab/habitat/articulated_agents/manipulator.py
@@ -312,13 +312,16 @@ def gripper_joint_pos(self, ctrl: List[float]):
 
     def set_gripper_target_state(self, gripper_state: float) -> None:
         """Set the gripper motors to a desired symmetric state of the gripper [0,1] -> [open, closed]"""
-        for i, jidx in enumerate(self.params.gripper_joints):
-            delta = (
-                self.params.gripper_closed_state[i]
-                - self.params.gripper_open_state[i]
-            )
-            target = self.params.gripper_open_state[i] + delta * gripper_state
-            self._set_motor_pos(jidx, target)
+        if self.sim_obj.motion_type == MotionType.DYNAMIC:
+            for i, jidx in enumerate(self.params.gripper_joints):
+                delta = (
+                    self.params.gripper_closed_state[i]
+                    - self.params.gripper_open_state[i]
+                )
+                target = (
+                    self.params.gripper_open_state[i] + delta * gripper_state
+                )
+                self._set_motor_pos(jidx, target)
 
     def close_gripper(self) -> None:
         """Set gripper to the close state"""
@@ -418,9 +421,9 @@ def arm_motor_pos(self):
     def arm_motor_pos(self, ctrl: List[float]) -> None:
         """Set the desired target of the arm joint motors."""
         self._validate_arm_ctrl_input(ctrl)
-
-        for i, jidx in enumerate(self.params.arm_joints):
-            self._set_motor_pos(jidx, ctrl[i])
+        if self.sim_obj.motion_type == MotionType.DYNAMIC:
+            for i, jidx in enumerate(self.params.arm_joints):
+                self._set_motor_pos(jidx, ctrl[i])
 
     @property
     def arm_motor_forces(self) -> np.ndarray:

diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py
@@ -29,6 +29,7 @@
 )
 from habitat.tasks.rearrange.rearrange_sim import RearrangeSim
 from habitat.tasks.rearrange.utils import rearrange_collision, rearrange_logger
+from habitat_sim.physics import MotionType
 
 
 @registry.register_task_action
@@ -146,9 +147,13 @@ def step(self, delta_pos, should_step=True, *args, **kwargs):
 
         # The actual joint positions
         self._sim: RearrangeSim
-        self.cur_articulated_agent.arm_motor_pos = (
-            delta_pos + self.cur_articulated_agent.arm_motor_pos
-        )
+        if (
+            self.cur_articulated_agent.sim_obj.motion_type
+            == MotionType.DYNAMIC
+        ):
+            self.cur_articulated_agent.arm_motor_pos = (
+                delta_pos + self.cur_articulated_agent.arm_motor_pos
+            )
 
 
 @registry.register_task_action