Merge pull request #188 from maxspahn/fix-non-holonomic-jacobians

fix[nonholonomic]: Adds facing direction for non-holonomic robots.

examples/albert.py

@@ -14,7 +14,8 @@ def run_albert(n_steps=1000, render=False, goal=True, obstacles=True):
             castor_wheels=["rotacastor_right_joint", "rotacastor_left_joint"],
             wheel_radius = 0.08,
             wheel_distance = 0.494,
-            spawn_rotation = np.pi/2,
+            spawn_rotation = 0,
+            facing_direction = '-y',
         ),
     ]
     env: UrdfEnv = gym.make(

examples/boxer.py

@@ -31,7 +31,7 @@ def run_boxer(n_steps=1000, render=False, goal=True, obstacles=True):
     history = []
     for _ in range(n_steps):
         ob, _, _, _ = env.step(action)
-        print(ob['robot_0']['joint_state']['position'][2])
+        print(ob['robot_0']['joint_state']['velocity'][0:2])
         history.append(ob)
     env.close()
     return history

pyproject.toml

@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "urdfenvs"
-version = "0.7.5"
+version = "0.7.6"
 description = "Simple simulation environment for robots, based on the urdf files."
 authors = ["Max Spahn <m.spahn@tudelft.nl>"]
 maintainers = [

urdfenvs/robots/generic_urdf/generic_diff_drive_robot.py

@@ -16,6 +16,7 @@ def __init__(
             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] = [],
     ):
         super().__init__(-1,
@@ -27,6 +28,7 @@ def __init__(
                          wheel_distance,
                          spawn_offset=spawn_offset,
                          spawn_rotation=spawn_rotation,
+                         facing_direction=facing_direction,
                          not_actuated_joints=not_actuated_joints)
 
     def set_joint_names(self):

urdfenvs/urdf_common/differential_drive_robot.py

@@ -6,6 +6,9 @@
 
 from urdfenvs.urdf_common.generic_robot import ControlMode, GenericRobot
 
+class FacingDirectionUndefinedError(Exception):
+    pass
+
 
 class DifferentialDriveRobot(GenericRobot):
     """Differential drive robot.
@@ -32,6 +35,7 @@ def __init__(
             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."""
@@ -43,6 +47,7 @@ def __init__(
         self._wheel_radius = wheel_radius
         self._wheel_distance = wheel_distance
         self._not_actuated_joints = not_actuated_joints
+        self._facing_direction = facing_direction
         super().__init__(n, urdf_file, mode)
 
     def set_degrees_of_freedom(self, n):
@@ -319,9 +324,24 @@ def update_state(self) -> None:
             (v_right - v_left) * self._wheel_radius / self._wheel_distance
         )
 
-        jacobi_nonholonomic = np.array(
-            [[np.cos(pos_base[2]), 0], [np.sin(pos_base[2]), 0], [0, 1]]
-        )
+        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':
+            jacobi_nonholonomic = np.array(
+                [[-np.cos(pos_base[2]), 0], [-np.sin(pos_base[2]), 0], [0, 1]]
+            )
+        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':
+            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'")
         velocity_base = np.dot(
             jacobi_nonholonomic, np.array([forward_velocity, angular_velocity])
         )