Fix set joint position (#289)

* set robot joint fix step then enable dynamics

* doc string fix

* for set joint position set step then enable dynamics

* give the user an option to change disable_dynamics option

* give the user an option to change disable_dynamics option

Author: lianghongzhuo

pyrep/objects/joint.py

@@ -44,7 +44,7 @@ def set_joint_position(self, position: float,
             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
+        :param position: Position of a joint (angular or linear
             values depending on the joint type).
         """
         if not disable_dynamics:
@@ -65,11 +65,11 @@ def set_joint_position(self, position: float,
         sim.simSetJointPosition(self._handle, position)
         self.set_joint_target_position(position)
 
+        with utils.step_lock:
+            sim.simExtStep(True)  # Have to step for changes to take effect
         # 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/robots/configuration_paths/arm_configuration_path.py

@@ -52,18 +52,18 @@ def step(self) -> bool:
         self._path_done = done
         return done
 
-    def set_to_start(self) -> None:
+    def set_to_start(self, disable_dynamics=False) -> None:
         """Sets the arm to the beginning of this path.
         """
         start_config = self._path_points[:len(self._arm.joints)]
-        self._arm.set_joint_positions(start_config)
+        self._arm.set_joint_positions(start_config, disable_dynamics=disable_dynamics)
         self._path_done = False
 
-    def set_to_end(self) -> None:
+    def set_to_end(self, disable_dynamics=False) -> None:
         """Sets the arm to the end of this path.
         """
         final_config = self._path_points[-len(self._arm.joints):]
-        self._arm.set_joint_positions(final_config)
+        self._arm.set_joint_positions(final_config, disable_dynamics=disable_dynamics)
 
     def visualize(self) -> None:
         """Draws a visualization of the path in the scene.

pyrep/robots/robot_component.py

@@ -109,12 +109,11 @@ def set_joint_positions(self, positions: List[float],
          for jh, p in zip(self._joint_handles, positions)]
         [j.set_joint_target_position(p)  # type: ignore
          for j, p in zip(self.joints, positions)]
-
+        with utils.step_lock:
+            sim.simExtStep(True)  # Have to step for changes to take effect
         # 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.