Reverted set_joint_position(s) to having disable_dynamics flag.

Author: stepjam

examples/example_panda_ik.py

@@ -32,7 +32,9 @@
     input('Press key to run solve_ik_via_sampling...')
     new_joint_pos = agent.solve_ik_via_sampling([x, y, z - 0.4], quaternion=q)[0]
 
-agent.set_joint_positions(new_joint_pos)
+# Because the arm is in Forxe/Torque mode, we need to temporarily disable
+# dynamics in order to instantaneously move joints.
+agent.set_joint_positions(new_joint_pos, disable_dynamics=True)
 input('Press any key to finish...')
 
 pr.stop()

pyrep/backend/utils.py

@@ -2,6 +2,7 @@
 import io
 import sys
 from contextlib import contextmanager
+from threading import Lock
 from typing import List, Tuple
 import pyrep
 from pyrep.backend import sim
@@ -16,6 +17,8 @@
 from pyrep.objects.camera import Camera
 from pyrep.objects.octree import Octree
 
+step_lock = Lock()
+
 
 def to_type(handle: int) -> Object:
     """Converts an object handle to the correct sub-type.

pyrep/objects/joint.py

@@ -1,5 +1,5 @@
 from typing import Tuple, List, Union
-from pyrep.backend import sim
+from pyrep.backend import sim, utils
 from pyrep.const import JointType, JointMode
 from pyrep.objects.object import Object, object_type_to_class
 from pyrep.const import ObjectType
@@ -14,7 +14,6 @@ class Joint(Object):
 
     def __init__(self, name_or_handle: Union[str, int]):
         super().__init__(name_or_handle)
-        self._config_tree = self.get_configuration_tree()
 
     def _get_requested_type(self) -> ObjectType:
         return ObjectType.JOINT
@@ -37,16 +36,41 @@ def get_joint_position(self) -> float:
         """
         return sim.simGetJointPosition(self._handle)
 
-    def set_joint_position(self, position: float) -> None:
+    def set_joint_position(self, position: float,
+                           disable_dynamics: bool = False) -> None:
         """Sets the intrinsic position of the joint.
 
+        :param disable_dynamics: If True, then the position can be set even
+            when the joint mode is in Force mode. It will disable dynamics,
+            move the joint, and then re-enable dynamics.
+
         :param positions: A list of positions of the joints (angular or linear
             values depending on the joint type).
         """
-        sim.simSetConfigurationTree(self._config_tree)
+        if not disable_dynamics:
+            sim.simSetJointPosition(self._handle, position)
+            return
+
+        is_model = self.is_model()
+        if not is_model:
+            self.set_model(True)
+
+        prior = sim.simGetModelProperty(self.get_handle())
+        p = prior | sim.sim_modelproperty_not_dynamic
+        # Disable the dynamics
+        sim.simSetModelProperty(self._handle, p)
+        with utils.step_lock:
+            sim.simExtStep(True)  # Have to step for changes to take effect
+
         sim.simSetJointPosition(self._handle, position)
         self.set_joint_target_position(position)
 
+        # Re-enable the dynamics
+        sim.simSetModelProperty(self._handle, prior)
+        self.set_model(is_model)
+        with utils.step_lock:
+            sim.simExtStep(True)  # Have to step for changes to take effect
+
     def get_joint_target_position(self) -> float:
         """Retrieves the target position of a joint.
 

pyrep/pyrep.py

@@ -11,7 +11,6 @@
 import sys
 import time
 import threading
-from threading import Lock
 from typing import Tuple, List
 import warnings
 
@@ -57,7 +56,6 @@ def __init__(self):
 
         self._ui_thread = None
         self._responsive_ui_thread = None
-        self._step_lock = Lock()
 
         self._init_thread_id = None
         self._shutting_down = False
@@ -85,10 +83,11 @@ def _run_ui_thread(self, scene_file: str, headless: bool,
 
     def _run_responsive_ui_thread(self) -> None:
         while True:
-            with self._step_lock:
-                if self._shutting_down or sim.simExtGetExitRequest():
-                    break
-                sim.simExtStep(False)
+            if not self.running:
+                with utils.step_lock:
+                    if self._shutting_down or sim.simExtGetExitRequest():
+                        break
+                    sim.simExtStep(False)
             time.sleep(0.01)
         # If the exit request was from the UI, then call shutdown, otherwise
         # shutdown caused this thread to terminate.
@@ -221,7 +220,7 @@ def step(self) -> None:
         If the physics simulation is not running, then this will only update
         the UI.
         """
-        with self._step_lock:
+        with utils.step_lock:
             sim.simExtStep()
 
     def step_ui(self) -> None:
@@ -231,7 +230,7 @@ def step_ui(self) -> None:
         simulation is running.
         This is only applicable when PyRep was launched without a responsive UI.
         """
-        with self._step_lock:
+        with utils.step_lock:
             sim.simExtStep(False)
 
     def set_simulation_timestep(self, dt: float) -> None:

pyrep/robots/robot_component.py

@@ -2,7 +2,7 @@
 
 from pyrep.objects.shape import Shape
 
-from pyrep.backend import sim
+from pyrep.backend import sim, utils
 from pyrep.const import JointType
 from pyrep.objects.object import Object
 from pyrep.objects.joint import Joint
@@ -24,7 +24,6 @@ def __init__(self, count: int, name: str, joint_names: List[str],
         self.joints = [Joint(jname + suffix)
                        for jname in joint_names]
         self._joint_handles = [j.get_handle() for j in self.joints]
-        self._config_tree = self.get_configuration_tree()
 
     def copy(self) -> 'RobotComponent':
         """Copy and pastes the arm in the scene.
@@ -76,21 +75,47 @@ def get_joint_positions(self) -> List[float]:
         """
         return [j.get_joint_position() for j in self.joints]
 
-    def set_joint_positions(self, positions: List[float]) -> None:
+    def set_joint_positions(self, positions: List[float],
+                            disable_dynamics: bool = False) -> None:
         """Sets the intrinsic position of the joints.
 
         See :py:meth:`Joint.set_joint_position` for more information.
 
+        :param disable_dynamics: If True, then the position can be set even
+            when the joint mode is in Force mode. It will disable dynamics,
+            move the joint, and then re-enable dynamics.
+
         :param positions: A list of positions of the joints (angular or linear
             values depending on the joint type).
         """
         self._assert_len(positions)
-        sim.simSetConfigurationTree(self._config_tree)
+        if not disable_dynamics:
+            [sim.simSetJointPosition(jh, p)  # type: ignore
+             for jh, p in zip(self._joint_handles, positions)]
+            return
+
+        is_model = self.is_model()
+        if not is_model:
+            self.set_model(True)
+
+        prior = sim.simGetModelProperty(self.get_handle())
+        p = prior | sim.sim_modelproperty_not_dynamic
+        # Disable the dynamics
+        sim.simSetModelProperty(self._handle, p)
+        with utils.step_lock:
+            sim.simExtStep(True)  # Have to step for changes to take effect
+
         [sim.simSetJointPosition(jh, p)  # type: ignore
          for jh, p in zip(self._joint_handles, positions)]
-        [j.set_joint_target_position(p)
+        [j.set_joint_target_position(p)  # type: ignore
          for j, p in zip(self.joints, positions)]
 
+        # Re-enable the dynamics
+        sim.simSetModelProperty(self._handle, prior)
+        self.set_model(is_model)
+        with utils.step_lock:
+            sim.simExtStep(True)  # Have to step for changes to take effect
+
     def get_joint_target_positions(self) -> List[float]:
         """Retrieves the target positions of the joints.