[ci skip] set_joint_positions bug fix.

Author: stepjam

pyrep/objects/joint.py

@@ -14,6 +14,7 @@ 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
@@ -42,12 +43,9 @@ def set_joint_position(self, position: float) -> None:
         :param positions: A list of positions of the joints (angular or linear
             values depending on the joint type).
         """
-
-        prev_mode = self.get_joint_mode()
-        self.set_joint_mode(JointMode.IK)
+        sim.simSetConfigurationTree(self._config_tree)
         sim.simSetJointPosition(self._handle, position)
         self.set_joint_target_position(position)
-        self.set_joint_mode(prev_mode)
 
     def get_joint_target_position(self) -> float:
         """Retrieves the target position of a joint.

pyrep/robots/robot_component.py

@@ -23,6 +23,8 @@ def __init__(self, count: int, name: str, joint_names: List[str],
         # Joint handles
         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.
@@ -83,14 +85,11 @@ def set_joint_positions(self, positions: List[float]) -> None:
             values depending on the joint type).
         """
         self._assert_len(positions)
-
-        prev_mode = self.get_joint_modes()
-        self.set_joint_mode(JointMode.IK)
-        [j.set_joint_position(p)  # type: ignore
-         for j, p in zip(self.joints, positions)]
-        [j.set_joint_target_position(p)  # type: ignore
+        sim.simSetConfigurationTree(self._config_tree)
+        [sim.simSetJointPosition(jh, p)  # type: ignore
+         for jh, p in zip(self._joint_handles, positions)]
+        [j.set_joint_target_position(p)
          for j, p in zip(self.joints, positions)]
-        self.set_joint_mode(prev_mode[0])
 
     def get_joint_target_positions(self) -> List[float]:
         """Retrieves the target positions of the joints.