From fa4f4d36429de9188f60d392b84398e9962702ef Mon Sep 17 00:00:00 2001 From: Max Spahn Date: Mon, 25 Sep 2023 11:07:27 +0200 Subject: [PATCH] fix[diffdrive]: Fixes orientaion of diff drive when facing direction is changed. --- pyproject.toml | 2 +- urdfenvs/urdf_common/differential_drive_robot.py | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 70ac7cc..0939db2 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [tool.poetry] name = "urdfenvs" -version = "0.8.15" +version = "0.8.16" description = "Simple simulation environment for robots, based on the urdf files." authors = ["Max Spahn "] maintainers = [ diff --git a/urdfenvs/urdf_common/differential_drive_robot.py b/urdfenvs/urdf_common/differential_drive_robot.py index f1508c3..8893e1a 100644 --- a/urdfenvs/urdf_common/differential_drive_robot.py +++ b/urdfenvs/urdf_common/differential_drive_robot.py @@ -277,7 +277,17 @@ def correct_base_orientation(self, pos_base: np.ndarray) -> np.ndarray: here. The function also makes sure that the orientation is always between -pi and pi. """ - pos_base -= self._spawn_rotation + pos_base[2] -= self._spawn_rotation + #TODO: The orientation is with respect to the spawn position + # If this is changed as suggested below, it breaks the velocities. + """ + if self._facing_direction == '-y': + pos_base[2] -= np.pi/2 + elif self._facing_direction == 'y': + pos_base[2] += np.pi/2 + elif self._facing_direction == '-x': + pos_base[2] + np.pi + """ if pos_base[2] < -np.pi: pos_base[2] += 2 * np.pi return pos_base @@ -314,7 +324,7 @@ def update_state(self) -> None: ] ) # make sure that the rotation is within -pi and pi - self.correct_base_orientation(pos_base) + pos_base = self.correct_base_orientation(pos_base) # wheel velocities vel_wheels = p.getJointStates(self._robot, self._robot_joints) v_right = vel_wheels[0][1]