resolved code review comments

examples/agv_robot.py

@@ -1,16 +1,20 @@
-"""A simple class that encapsulates mobile robots."""
-import numpy as np
+"""This is an example for an AGV following a set path using a custom controller."""
 import pybullet as p
-import unittest
 import os
-import pybullet_data
+
 
 import pybullet_industrial as pi
 import pybullet_data
 import time
 import os
 
 def trajectory_follower_controller(distance,angle,target_angle_error):
+    """ A simple trajectory following controller that neglects the target_angle_error and simply
+        orients the robot towards the current target position.
+        The controller is a simple proportional controller that
+        takes the distance and angle to the target
+        and returns a linear and angular velocity command to the robot.
+        """
     kp_lin=1
     kp_ang=1
 

src/pybullet_industrial/agv_robots.py

@@ -1,4 +1,4 @@
-"""A simple class that encapsulates mobile robots."""
+"""A simple class that encapsulates mobile robots moving on the x-y plane in pybullet."""
 import numpy as np
 import pybullet as p
 
@@ -20,7 +20,7 @@ def __init__(self,urdf_model: str, start_position: np.array, start_orientation:
                                                       the linear and angular velocity of the robot.
                                                       Defaults to None. In this case a standard
                                                       position controller is used.
-        
+
         """
 
         urdf_flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
@@ -73,10 +73,11 @@ def _calculate_position_error(self):
         return distance_to_target, angle_to_target, target_angle_error
 
     def standard_position_controller(self,distance,angle,target_angle_error):
-        pass
+        raise NotImplementedError("The standard position controller has to be implemented in a subclass.")
+
 
     def set_velocity(self,velocity_vector):
-        pass
+        raise NotImplementedError("The set_velocity method has to be implemented in a subclass.")
 
     def update_position_loop(self):
         """Updates the position of the robot in a loop.
@@ -176,7 +177,7 @@ def __init__(self, urdf_model: str, start_position: np.array, start_orientation:
         p.setJointMotorControl2(self.urdf, self.left_wheel_index, p.VELOCITY_CONTROL)
         p.setJointMotorControl2(self.urdf, self.right_wheel_index, p.VELOCITY_CONTROL)
 
-    def _calculate_wheel_comands(self, linear_velocity: float, angular_velocity: float):
+    def _calculate_wheel_comands(self, velocity_vector):
         """Calculates the wheel commands for a differential drive robot.
 
         Args:
@@ -186,6 +187,9 @@ def _calculate_wheel_comands(self, linear_velocity: float, angular_velocity: flo
         Returns:
             np.array: The wheel commands.
         """
+        linear_velocity = velocity_vector[0]
+        angular_velocity = velocity_vector[1]
+
         turning_contribution = angular_velocity * self.track_width / (2*self.wheel_radius)
         driving_contribution = linear_velocity / self.wheel_radius
         left_wheel_velocity = driving_contribution - turning_contribution
@@ -211,7 +215,7 @@ def set_velocity(self, velocity_vector):
                                     -self.max_angular_velocity,
                                     self.max_angular_velocity)
 
-        wheel_commands = self._calculate_wheel_comands(linear_velocity, angular_velocity)
+        wheel_commands = self._calculate_wheel_comands([linear_velocity, angular_velocity])
         p.setJointMotorControl2(self.urdf,
                                 self.left_wheel_index,
                                 p.VELOCITY_CONTROL,
@@ -222,6 +226,18 @@ def set_velocity(self, velocity_vector):
                                 targetVelocity=wheel_commands[1])
 
     def standard_position_controller(self,distance,angle,target_angle_error):
+        """ A simple position controller for a differential drive robot.
+
+
+        Args:
+            distance (float): the distance to the target position
+            angle (float): the angle to the target position in radians from -pi to pi
+            target_angle_error (float): the angle error between the robot orientation and the target orientation
+                                         in radians from -pi to pi
+
+        Returns:
+            List: A velocity vector containing the linear and angular velocity of the robot.
+        """
         kp_lin=1
         kp_ang= 0.8
         kp_target_ang = 0.4