Turtlebot Training

[bethlow]

This PR adds neural net training capabilities to the turtlebot controller code base. The training logs in Tensorboard have been updated to include the current state of git commits. Training is up-to-date with main branch functionalities and the new rollout plot set-up. This feature was needed to allow training of controllers for implementation on the turtlebot hardware.

[dawsonc]

When I pull this branch and run pytest ., I get the following error:

neural_clbf/systems/tests/test_turtlebot.py FF                                                                                                                  [100%]

============================================================================== FAILURES ===============================================================================
_________________________________________________________________________ test_turtlebot_init _________________________________________________________________________

    def test_turtlebot_init():
        """Test initialization of TurtleBot3"""
        # Test instantiation with valid parameters
        valid_params = {
            "R": 0.1,
            "L": 0.5,
        }

        turtlebot = TurtleBot(valid_params)
        assert turtlebot is not None
        assert turtlebot.n_dims == 3
        assert turtlebot.n_controls == 2

        # Check control limits
        upper_lim, lower_lim = turtlebot.control_limits
        expected_upper = 1000 * torch.ones(2)
        expected_lower = -1000 * (torch.ones(2))
>       assert torch.allclose(upper_lim, expected_upper, atol=0.1)
E       assert False
E        +  where False = <built-in method allclose of type object at 0x7efff8070ec0>(tensor([1000.0000,    6.2832]), tensor([1000., 1000.]), atol=0.1)
E        +    where <built-in method allclose of type object at 0x7efff8070ec0> = torch.allclose

neural_clbf/systems/tests/test_turtlebot.py:25: AssertionError
_______________________________________________________________________ test_turtlebot_dynamics _______________________________________________________________________

    def test_turtlebot_dynamics():
        """Test the dynamics of the TurtleBot3"""
        # Create the turtlebot system
        params = {"R": 0.07, "L": 0.3}
        turtlebot = TurtleBot(params)

        # The dynamics should be fixed at the orgin with zero controls
        x_origin = torch.zeros((1, turtlebot.n_dims))
        u_eq = torch.zeros((1, turtlebot.n_controls))
        xdot = turtlebot.closed_loop_dynamics(x_origin, u_eq)
        assert torch.allclose(xdot, torch.zeros((1, turtlebot.n_dims)))

        # If linear velocity is increased and angular velocity constant
        # then we should experience positive position change in x and y, and no
        # change in theta
        u = u_eq[0] + 1.0
>       xdot = turtlebot.closed_loop_dynamics(x_origin, u)

neural_clbf/systems/tests/test_turtlebot.py:68: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _

self = <neural_clbf.systems.turtlebot.TurtleBot object at 0x7eff6d729dc0>, x = tensor([[0., 0., 0.]]), u = tensor([1., 1.]), params = None

    def closed_loop_dynamics(
        self, x: torch.Tensor, u: torch.Tensor, params: Optional[Scenario] = None
    ) -> torch.Tensor:
        """
        Return the state derivatives at state x and control input u

            dx/dt = f(x) + g(x) u

        args:
            x: bs x self.n_dims tensor of state
            u: bs x self.n_controls tensor of controls
            params: a dictionary giving the parameter values for the system. If None,
                    default to the nominal parameters used at initialization
        returns:
            xdot: bs x self.n_dims tensor of time derivatives of x
        """
        # Get the control-affine dynamics
        f, g = self.control_affine_dynamics(x, params=params)
        # Compute state derivatives using control-affine form
>       xdot = f + torch.bmm(g, u.unsqueeze(-1))
E       RuntimeError: Expected 3-dimensional tensor, but got 2-dimensional tensor for argument #2 'batch2' (while checking arguments for bmm)

neural_clbf/systems/control_affine_system.py:367: RuntimeError

Can you please look into this? Also, please run brunette . and make sure that running flake8 does not return any errors.