diff --git a/pyrep/robots/configuration_paths/arm_configuration_path.py b/pyrep/robots/configuration_paths/arm_configuration_path.py index 49bb13f..8614e73 100644 --- a/pyrep/robots/configuration_paths/arm_configuration_path.py +++ b/pyrep/robots/configuration_paths/arm_configuration_path.py @@ -25,6 +25,7 @@ def __init__(self, arm: 'Arm', # type: ignore self._drawing_handle = None self._path_done = False self._num_joints = arm.get_joint_count() + self._joint_position_action = None def __len__(self): return len(self._path_points) // self._num_joints @@ -102,6 +103,9 @@ def clear_visualization(self) -> None: if self._drawing_handle is not None: sim.simAddDrawingObjectItem(self._drawing_handle, None) + def get_executed_joint_position_action(self) -> np.ndarray: + return self._joint_position_action + def _get_rml_handle(self) -> int: dt = sim.simGetSimulationTimeStep() limits = np.array(self._arm.get_joint_upper_velocity_limits()) @@ -156,6 +160,7 @@ def _get_rml_handle(self) -> int: return rml_handle def _step_motion(self) -> int: + self._joint_position_action = None dt = sim.simGetSimulationTimeStep() lengths = self._get_path_point_lengths() state, posVelAccel = sim.simRMLStep(self._rml_handle, dt, 1) @@ -173,6 +178,7 @@ def _step_motion(self) -> int: offset:offset + len(self._arm.joints)] dx = p2 - p1 qs = p1 + dx * t + self._joint_position_action = qs self._arm.set_joint_target_positions(qs) break if state == 1: