ft[clean]: Removes dangling specifications of physics engine realizations. Alligns quotation marks style.

Author: maxspahn

urdfenvs/sensors/physics_engine_interface.py

@@ -4,7 +4,6 @@
 
 import numpy as np
 from scipy.spatial.transform import Rotation
-#import pybullet
 
 class LinkIdNotFoundError(Exception):
     pass
@@ -65,118 +64,3 @@ def add_visualization_line(self, *args) -> None:
         )
 
 
-class PybulletInterface(PhysicsEngineInterface):
-    """
-    Physics engine interface for bullet.
-    """
-    def extract_link_id(self, robot, link_name: str) -> int:
-        number_links = pybullet.getNumJoints(robot)
-        joint_names = []
-        for i in range(number_links):
-            joint_name = pybullet.getJointInfo(robot, i)[1].decode("UTF-8")
-            joint_names.append(joint_name)
-            if joint_name == link_name:
-                return i
-        raise LinkIdNotFoundError(
-            f"Link with name {link_name} not found. "
-            f"Possible links are {joint_names}"
-        )
-
-
-    def get_obstacle_pose(self, obst_id: int, obst_name: str, movable: bool = False) -> None:
-        position, orientation = pybullet.getBasePositionAndOrientation(obst_id)
-        return position, orientation
-
-    def get_obstacle_velocity(self, obst_id: int, obst_name: str, movable: bool = False) -> None:
-        linear, angular = pybullet.getBaseVelocity(obst_id)
-        return linear, angular
-
-    def get_goal_pose(self, goal_id: int) -> Tuple[List[float]]:
-        position, orientation = pybullet.getBasePositionAndOrientation(goal_id)
-        return position, orientation
-
-    def get_goal_velocity(self, goal_id: int) -> Tuple[List[float]]:
-        linear, angular = pybullet.getBaseVelocity(goal_id)
-        return linear, angular
-
-    def get_link_position(self, robot, link_id) -> np.ndarray:
-        link_position = np.array(pybullet.getLinkState(robot, link_id)[0])
-        return link_position
-
-    def get_link_orientation(self, robot, link_id) -> np.ndarray:
-        link_orientation = np.array(pybullet.getLinkState(robot, link_id)[1])
-        return link_orientation
-
-    def ray_cast(
-        self,
-        ray_start: np.ndarray,
-        ray_end: np.ndarray,
-        ray_index: int,
-        ray_length: float
-    ) -> np.ndarray:
-        lidar = pybullet.rayTest(ray_start, ray_end)
-        return lidar[0][2] * ray_length
-
-    def clear_visualizations(self) -> None:
-        pybullet.removeAllUserDebugItems()
-
-    def add_visualization_line(
-        self, start_point: Tuple[float], end_point: Tuple[float]
-    ) -> None:
-        pybullet.addUserDebugLine(start_point, end_point)
-
-class MujocoInterface(PhysicsEngineInterface):
-    """
-    Physics engine interface for mujoco.
-    """
-    def extract_link_id(self, robot, link_name: str) -> int:
-        return self._data.body(link_name).id
-
-    def set_data(self, data) -> None:
-        """Set pointer to mujoco data to sensor."""
-        self._data = data
-
-    def get_link_position(self, robot, link_id) -> np.ndarray:
-        link_position = self._data.xpos[link_id]
-        return link_position
-
-    def get_link_orientation(self, robot, link_id) -> np.ndarray:
-        link_orientation_matrix = np.reshape(self._data.xmat[link_id], (3, 3))
-        return Rotation.from_matrix(link_orientation_matrix).as_quat()
-
-    def get_obstacle_pose(self, obst_id: int, obst_name: str, movable: bool = False) -> Tuple[List[float], List[float]]:
-        if movable:
-
-            free_joint_data = self._data.jnt(f"freejoint_{obst_name}").qpos
-            print(free_joint_data)
-            return free_joint_data[0:3].tolist(), free_joint_data[3:].tolist()
-        pos = self._data.body(obst_name).xpos
-        ori = self._data.body(obst_name).xquat
-        return pos.tolist(), ori.tolist()
-
-    def get_goal_pose(self, goal_id: int) -> Tuple[List[float]]:
-        pos = self._data.site(goal_id).xpos
-        goal_rotation = np.reshape(self._data.site(goal_id).xmat, (3, 3))
-        ori = Rotation.from_matrix(goal_rotation).as_quat()
-        return pos.tolist(), ori.tolist()
-
-    def get_goal_velocity(self, goal_id: int) -> Tuple[List[float]]:
-        return [0, 0, 0], [0, 0, 0]
-
-    def ray_cast(
-        self,
-        ray_start: np.ndarray,
-        ray_end: np.ndarray,
-        ray_index: int,
-        ray_length: float
-    ) -> np.ndarray:
-        if self._data.sensordata[ray_index] < 0:
-            ray_value = ray_length - (0.01 / 2)
-        else:
-            ray_value = self._data.sensordata[ray_index] - (0.01 / 2)
-        return ray_value
-
-    def get_obstacle_velocity(self, obst_id: int, obst_name: str, movable: bool = False) -> None:
-        raise NotImplementedError(
-            "Obstacle velocity not implemented for mujoco."
-        )

urdfenvs/urdf_common/differential_drive_robot.py

@@ -7,6 +7,7 @@
 
 from urdfenvs.urdf_common.generic_robot import ControlMode, GenericRobot
 
+
 class FacingDirectionUndefinedError(Exception):
     pass
 
@@ -25,22 +26,23 @@ class DifferentialDriveRobot(GenericRobot):
         The offset by which the initial position must be shifted to align
         observation with that position.
     """
+
     def __init__(
-            self,
-            n: int,
-            urdf_file: str,
-            mode: ControlMode,
-            actuated_wheels: List[str],
-            castor_wheels: List[str],
-            wheel_radius: float,
-            wheel_distance: float,
-            spawn_offset: np.ndarray = np.array([0.0, 0.0, 0.15]),
-            spawn_rotation: float = 0.0,
-            facing_direction: str = 'x',
-            not_actuated_joints: List[str] = []
+        self,
+        n: int,
+        urdf_file: str,
+        mode: ControlMode,
+        actuated_wheels: List[str],
+        castor_wheels: List[str],
+        wheel_radius: float,
+        wheel_distance: float,
+        spawn_offset: np.ndarray = np.array([0.0, 0.0, 0.15]),
+        spawn_rotation: float = 0.0,
+        facing_direction: str = "x",
+        not_actuated_joints: List[str] = [],
     ):
         """Constructor for differential drive robots."""
-        self._number_actuated_axes = int(len(actuated_wheels)/2)
+        self._number_actuated_axes = int(len(actuated_wheels) / 2)
         self._spawn_rotation = deepcopy(spawn_rotation)
         self._spawn_offset = deepcopy(spawn_offset)
         self._castor_wheels = castor_wheels
@@ -55,7 +57,14 @@ def set_degrees_of_freedom(self, n):
         if n > 0:
             self._n = n
         else:
-            self._n: int = self._urdf_robot.num_actuated_joints - len(self._actuated_wheels) - len(self._castor_wheels) - len(self._not_actuated_joints) + 2
+            self._n: int = (
+                self._urdf_robot.num_actuated_joints
+                - len(self._actuated_wheels)
+                - len(self._castor_wheels)
+                - len(self._not_actuated_joints)
+                + 2
+            )
+
     def ns(self) -> int:
         """Returns the number of degrees of freedom.
 
@@ -69,7 +78,8 @@ def get_velocity_spaces(self) -> tuple:
         control.
 
         Overrides velocity spaces from default, because a differential drive has limits in x,y and
-        theta direction, while the action space should be limited to the forward and angular velocity."""
+        theta direction, while the action space should be limited to the forward and angular velocity.
+        """
         ospace = self.get_observation_space()
         uu = np.concatenate(
             (self._limit_vel_forward_j[1, :], self._limit_vel_j[1, 3:]), axis=0
@@ -81,19 +91,22 @@ def get_velocity_spaces(self) -> tuple:
         return (ospace, aspace)
 
     def reset(
-            self,
-            pos: np.ndarray,
-            vel: np.ndarray,
-            mount_position: np.ndarray,
-            mount_orientation: np.ndarray,) -> None:
-        """ Reset simulation and add robot """
+        self,
+        pos: np.ndarray,
+        vel: np.ndarray,
+        mount_position: np.ndarray,
+        mount_orientation: np.ndarray,
+    ) -> None:
+        """Reset simulation and add robot"""
         logging.warning(
             "The argument 'mount_position' and 'mount_orientation' are \
 ignored for differential drive robots."
         )
         if hasattr(self, "_robot"):
             p.removeBody(self._robot)
-        base_orientation = p.getQuaternionFromEuler([0, 0, self._spawn_rotation + pos[2]])
+        base_orientation = p.getQuaternionFromEuler(
+            [0, 0, self._spawn_rotation + pos[2]]
+        )
         spawn_position = self._spawn_offset
         spawn_position[0:2] += pos[0:2]
         self._robot = p.loadURDF(
@@ -137,8 +150,8 @@ def check_state(self, pos: np.ndarray, vel: np.ndarray) -> tuple:
         return pos, vel
 
     def read_limits(self) -> None:
-        """ Set position, velocity, acceleration
-        and motor torque lower en upper limits """
+        """Set position, velocity, acceleration
+        and motor torque lower en upper limits"""
         self._limit_pos_j = np.zeros((2, self.ns()))
         self._limit_vel_j = np.zeros((2, self.ns()))
         self._limit_tor_j = np.zeros((2, self.n()))
@@ -260,7 +273,7 @@ def apply_base_velocity(self, vels: np.ndarray) -> None:
 
     def apply_velocity_action(self, vels: np.ndarray) -> None:
         """Applies angular velocities to the arm joints."""
-        self.apply_base_velocity(vels) 
+        self.apply_base_velocity(vels)
         for i in range(2, self._n):
             p.setJointMotorControl2(
                 self._robot,
@@ -278,7 +291,7 @@ def correct_base_orientation(self, pos_base: np.ndarray) -> np.ndarray:
         between -pi and pi.
         """
         pos_base[2] -= self._spawn_rotation
-        #TODO: The orientation is with respect to the spawn position
+        # 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':
@@ -335,24 +348,26 @@ def update_state(self) -> None:
             (v_right - v_left) * self._wheel_radius / self._wheel_distance
         )
 
-        if self._facing_direction == 'x':
+        if self._facing_direction == "x":
             jacobi_nonholonomic = np.array(
                 [[np.cos(pos_base[2]), 0], [np.sin(pos_base[2]), 0], [0, 1]]
             )
-        elif self._facing_direction == '-x':
+        elif self._facing_direction == "-x":
             jacobi_nonholonomic = np.array(
                 [[-np.cos(pos_base[2]), 0], [-np.sin(pos_base[2]), 0], [0, 1]]
             )
-        elif self._facing_direction == 'y':
+        elif self._facing_direction == "y":
             jacobi_nonholonomic = np.array(
                 [[-np.sin(pos_base[2]), 0], [np.cos(pos_base[2]), 0], [0, 1]]
             )
-        elif self._facing_direction == '-y':
+        elif self._facing_direction == "-y":
             jacobi_nonholonomic = np.array(
                 [[np.sin(pos_base[2]), 0], [-np.cos(pos_base[2]), 0], [0, 1]]
             )
         else:
-            raise FacingDirectionUndefinedError(f"Facing direction {self._facing_direction} undefined. Use 'x', '-x', 'y' or '-y'")
+            raise FacingDirectionUndefinedError(
+                f"Facing direction {self._facing_direction} undefined. Use 'x', '-x', 'y' or '-y'"
+            )
         velocity_base = np.dot(
             jacobi_nonholonomic, np.array([forward_velocity, angular_velocity])
         )
@@ -374,4 +389,3 @@ def update_state(self) -> None:
                 "forward_velocity": np.array([forward_velocity]),
             }
         }
-