unicycle_move_to_pose

Move to specified pose

Daniel Ingram (daniel-s-ingram)

Atsushi Sakai(@Atsushi_twi) Vikas Dhiman (github.com/wecacuee)

P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7

AckermannDrive

DynamicsModel for AckermannDrive

ẋ = v cos(θ) ẏ = v sin(θ) θ̇ = (v/L) tan(ϕ)

L is the distance between front and back wheels state = [x, y, θ] input = [v, v tan(ϕ)]

Source code in bayes_cbf/unicycle_move_to_pose.py

class AckermannDrive:
    """
    DynamicsModel for AckermannDrive

    ẋ = v cos(θ)
    ẏ = v sin(θ)
    θ̇ = (v/L) tan(ϕ)

    L is the distance between front and back wheels
    state = [x, y, θ]
    input = [v, v tan(ϕ)]
    """
    state_size = 3
    ctrl_size = 2
    def __init__(self, L = 0.2, kernel_diag_A=[1.0, 1.0, 1.0]):
        self.L = L
        self.current_state = None
        self.kernel_diag_A = torch.tensor(kernel_diag_A)

    def set_init_state(self, x):
        self.current_state = x

    def f_func(self, x):
        """
        ṡ = f(s) + G(s) u

        s = [x, y, θ]
        u = [v, v tan(ϕ)]

               [ 0 ]
        f(s) = [ 0 ]
               [ 0 ]
        """
        return torch.zeros_like(x)

    def g_func(self, state_in):
        """
        ṡ = f(s) + G(s) u

        s = [x, y, θ]
        u = [v, v tan(ϕ)]

                [ cos(θ) , 0  ]
        G(s)u = [ sin(θ) , 0  ] [ v ]
                [ 0        1/L] [ v tan(ø) ]
        """
        assert state_in.shape[-1] == self.state_size
        state = state_in.unsqueeze(0) if state_in.dim() <= 1 else state_in

        x, y, theta = state[..., 0:1], state[..., 1:2], state[..., 2:3]
        theta_cos = theta.cos().unsqueeze(-1)
        theta_sin = theta.sin().unsqueeze(-1)
        zeros_ = torch.zeros_like(theta_cos)
        inv_L = torch.ones_like(theta_cos) / self.L
        gX = torch.cat([torch.cat([theta_cos, zeros_], dim=-1),
                        torch.cat([theta_sin, zeros_], dim=-1),
                        torch.cat([zeros_,    inv_L], dim=-1)], dim=-2)
        return gX.squeeze(0) if state_in.dim() <= 1 else gX

    def F_func(self, X):
        return torch.cat([self.f_func(X).unsqueeze(-1), self.g_func(X)], dim=-1)

    def fu_func_gp(self, u):
        f, g = self.f_func, self.g_func
        n = self.state_size
        self.kernel_diag_A = torch_to(self.kernel_diag_A,
                                     device=u.device,
                                     dtype=u.dtype)
        A = torch.diag(self.kernel_diag_A)
        u_hom = torch.cat([torch.tensor([1.]), u])
        B = torch.eye(self.ctrl_size + 1)
        return GaussianProcess(
            mean = lambda x: f(x) + g(x) @ u,
            knl = lambda x, xp:  (u_hom @ B @ u_hom) * A,
            shape=(n,),
            name="AckermannDrive")

    def step(self, u_torch, dt):
        x = self.current_state
        xdot = self.f_func(x) + self.g_func(x) @ u_torch
        self.current_state = x + xdot * dt
        return dict(xdot = xdot,
                    x = self.current_state)

    def custom_predict_fullmat(self, X):
        A = torch.diag(self.kernel_diag_A)
        B = torch.eye(self.ctrl_size + 1)
        b = X.shape[0] if X.ndim > 1 else 1
        bI = torch.eye(b)
        return (self.F_func(X).transpose(-2, -1).reshape(-1),
                torch_kron(bI,
                           torch_kron(B, A, batch_dims=0),
                           batch_dims=0))

f_func(x)

ṡ = f(s) + G(s) u

s = [x, y, θ] u = [v, v tan(ϕ)]

   [ 0 ]

f(s) = [ 0 ] [ 0 ]

Source code in bayes_cbf/unicycle_move_to_pose.py

def f_func(self, x):
    """
    ṡ = f(s) + G(s) u

    s = [x, y, θ]
    u = [v, v tan(ϕ)]

           [ 0 ]
    f(s) = [ 0 ]
           [ 0 ]
    """
    return torch.zeros_like(x)

g_func(state_in)

ṡ = f(s) + G(s) u

s = [x, y, θ] u = [v, v tan(ϕ)]

    [ cos(θ) , 0  ]

G(s)u = [ sin(θ) , 0 ] [ v ] [ 0 1/L] [ v tan(ø) ]

Source code in bayes_cbf/unicycle_move_to_pose.py

def g_func(self, state_in):
    """
    ṡ = f(s) + G(s) u

    s = [x, y, θ]
    u = [v, v tan(ϕ)]

            [ cos(θ) , 0  ]
    G(s)u = [ sin(θ) , 0  ] [ v ]
            [ 0        1/L] [ v tan(ø) ]
    """
    assert state_in.shape[-1] == self.state_size
    state = state_in.unsqueeze(0) if state_in.dim() <= 1 else state_in

    x, y, theta = state[..., 0:1], state[..., 1:2], state[..., 2:3]
    theta_cos = theta.cos().unsqueeze(-1)
    theta_sin = theta.sin().unsqueeze(-1)
    zeros_ = torch.zeros_like(theta_cos)
    inv_L = torch.ones_like(theta_cos) / self.L
    gX = torch.cat([torch.cat([theta_cos, zeros_], dim=-1),
                    torch.cat([theta_sin, zeros_], dim=-1),
                    torch.cat([zeros_,    inv_L], dim=-1)], dim=-2)
    return gX.squeeze(0) if state_in.dim() <= 1 else gX

CLFCartesian

Source code in bayes_cbf/unicycle_move_to_pose.py

class CLFCartesian:
    def __init__(self,
                 Kp = torch.tensor([9., 15., 40.])/10.):
        self.Kp = Kp

    def clf_terms(self, state, state_goal):
        rho, alpha, beta = cartesian2polar(state, state_goal)
        x,y, theta = state
        x_goal, y_goal, theta_goal = state_goal
        return torch.tensor((0.5 * self.Kp[0] * rho ** 2,
                         self.Kp[1] * (1-torch.cos(alpha)),
                         self.Kp[2] * (1-torch.cos(beta))
        ))

    def _grad_clf_terms_wrt_goal(self, state, state_goal):
        """
        >>> self = CLFCartesian()
        >>> x0 = torch.rand(3)
        >>> x0_goal = torch.rand(3)
        >>> ajac = self._grad_clf_terms_wrt_goal(x0, x0_goal)[:, 0]
        >>> njac = numerical_jac(lambda xg: self.clf_terms(x0, xg)[0], x0_goal, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        >>> ajac = self._grad_clf_terms_wrt_goal(x0, x0_goal)[:, 1]
        >>> njac = numerical_jac(lambda xg: self.clf_terms(x0, xg)[1], x0_goal, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        >>> ajac = self._grad_clf_terms_wrt_goal(x0, x0_goal)[:, 2]
        >>> njac = numerical_jac(lambda xg: self.clf_terms(x0, xg)[2], x0_goal, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        x_diff, y_diff, theta_diff = state_goal - state
        rho, alpha, beta = cartesian2polar(state, state_goal)
        return torch.tensor([[self.Kp[0] * x_diff,
                          self.Kp[1] * torch.sin(alpha) * y_diff / (rho**2),
                          self.Kp[2] * torch.sin(beta) * y_diff / (rho**2)
                          ],
                         [self.Kp[0] * y_diff,
                          - self.Kp[1] * torch.sin(alpha) * x_diff / (rho**2),
                          - self.Kp[2] * torch.sin(beta) * x_diff / (rho**2)],
                         [0, 0,
                          self.Kp[2] * torch.sin(beta)]
                         ])

    def _grad_clf_terms(self, state, state_goal):
        """
        >>> self = CLFCartesian()
        >>> x0 = torch.rand(3)
        >>> x0_goal = torch.rand(3)
        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 0]
        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[0], x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 1]
        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[1], x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 2]
        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[2], x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        x_diff, y_diff, theta_diff = state_goal - state
        rho, alpha, beta = cartesian2polar(state, state_goal)
        return torch.tensor([[- self.Kp[0] * x_diff,
                          - self.Kp[1] * torch.sin(alpha) * y_diff / (rho**2),
                          - self.Kp[2] * torch.sin(beta) * y_diff / (rho**2)
                          ],
                         [- self.Kp[0] * y_diff,
                          self.Kp[1] * torch.sin(alpha) * x_diff / (rho**2),
                          self.Kp[2] * torch.sin(beta) * x_diff / (rho**2)],
                         [0, self.Kp[1] * torch.sin(alpha), 0]
                         ])
    def grad_clf(self, state, state_goal):
        """
        >>> self = CLFCartesian()
        >>> x0 = torch.rand(3)
        >>> x0_goal = torch.rand(3)
        >>> ajac = self.grad_clf(x0, x0_goal)
        >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal).sum(), x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        return self._grad_clf_terms(state, state_goal).sum(axis=-1)

    def grad_clf_wrt_goal(self, state, state_goal):
        """
        >>> self = CLFCartesian()
        >>> x0 = torch.rand(3)
        >>> x0_goal = torch.rand(3)
        >>> ajac = self.grad_clf_wrt_goal(x0, x0_goal)
        >>> njac = numerical_jac(lambda xg: self.clf_terms(x0, xg).sum(), x0_goal, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        return self._grad_clf_terms_wrt_goal(state, state_goal).sum(axis=-1)


    def isconverged(self, x, state_goal):
        rho, alpha, beta = cartesian2polar(x, state_goal)
        return rho < 1e-3

grad_clf(state, state_goal)

self = CLFCartesian() x0 = torch.rand(3) x0_goal = torch.rand(3) ajac = self.grad_clf(x0, x0_goal) njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal).sum(), x0, 1e-6)[0] testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)

Source code in bayes_cbf/unicycle_move_to_pose.py

def grad_clf(self, state, state_goal):
    """
    >>> self = CLFCartesian()
    >>> x0 = torch.rand(3)
    >>> x0_goal = torch.rand(3)
    >>> ajac = self.grad_clf(x0, x0_goal)
    >>> njac = numerical_jac(lambda x: self.clf_terms(x, x0_goal).sum(), x0, 1e-6)[0]
    >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
    """
    return self._grad_clf_terms(state, state_goal).sum(axis=-1)

grad_clf_wrt_goal(state, state_goal)

self = CLFCartesian() x0 = torch.rand(3) x0_goal = torch.rand(3) ajac = self.grad_clf_wrt_goal(x0, x0_goal) njac = numerical_jac(lambda xg: self.clf_terms(x0, xg).sum(), x0_goal, 1e-6)[0] testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)

Source code in bayes_cbf/unicycle_move_to_pose.py

def grad_clf_wrt_goal(self, state, state_goal):
    """
    >>> self = CLFCartesian()
    >>> x0 = torch.rand(3)
    >>> x0_goal = torch.rand(3)
    >>> ajac = self.grad_clf_wrt_goal(x0, x0_goal)
    >>> njac = numerical_jac(lambda xg: self.clf_terms(x0, xg).sum(), x0_goal, 1e-6)[0]
    >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
    """
    return self._grad_clf_terms_wrt_goal(state, state_goal).sum(axis=-1)

CLFPolar

Source code in bayes_cbf/unicycle_move_to_pose.py

class CLFPolar:
    def __init__(self,
                 Kp = torch.tensor([6., 15., 40., 0.])/10.):
        self.Kp = Kp

    def clf_terms(self, polar, state_goal):
        return self._clf_terms(polar, state_goal)

    def _clf_terms(self, polar, state_goal):
        rho, alpha, beta = polar
        return torch.tensor((0.5 * self.Kp[0] * rho ** 2,
                         self.Kp[1] * cosdist(alpha, 0),
                         self.Kp[2] * cosdist(beta, 0),
                         self.Kp[3] * (1-torch.cos(beta - alpha))
        ))

    def grad_clf(self, polar, state_goal):
        """
        >>> self = CLFPolar()
        >>> x0 = torch.rand(3)
        >>> state_goal = torch.rand(3)
        >>> ajac = self.grad_clf(x0, state_goal)
        >>> njac = numerical_jac(lambda x: self._clf_terms(x, state_goal).sum(), x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        return self._grad_clf_terms(polar, state_goal).sum(axis=-1)

    def grad_clf_wrt_goal(self, polar, state_goal):
        return torch.zeros_like(state_goal)

    def _grad_clf_terms(self, polar, state_goal):
        """
        >>> self = CLFPolar()
        >>> x0 = torch.rand(3)
        >>> x0_goal = torch.rand(3)
        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 0]
        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[0], x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 1]
        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[1], x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 2]
        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[2], x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        >>> ajac = self._grad_clf_terms(x0, x0_goal)[:, 3]
        >>> njac = numerical_jac(lambda x: self.clf_terms(x,x0_goal)[3], x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        rho, alpha, beta = polar
        return torch.tensor([[self.Kp[0] * rho,  0, 0, 0],
                         [0, self.Kp[1] * torch.sin(alpha), 0, - self.Kp[3] * torch.sin(beta - alpha)],
                         [0,  0, self.Kp[2] * torch.sin(beta), self.Kp[3] * torch.sin(beta - alpha)]])

    def isconverged(self, x, state_goal):
        rho, alpha, beta = cartesian2polar(x, state_goal)
        return rho < 1e-3

grad_clf(polar, state_goal)

self = CLFPolar() x0 = torch.rand(3) state_goal = torch.rand(3) ajac = self.grad_clf(x0, state_goal) njac = numerical_jac(lambda x: self._clf_terms(x, state_goal).sum(), x0, 1e-6)[0] testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)

Source code in bayes_cbf/unicycle_move_to_pose.py

def grad_clf(self, polar, state_goal):
    """
    >>> self = CLFPolar()
    >>> x0 = torch.rand(3)
    >>> state_goal = torch.rand(3)
    >>> ajac = self.grad_clf(x0, state_goal)
    >>> njac = numerical_jac(lambda x: self._clf_terms(x, state_goal).sum(), x0, 1e-6)[0]
    >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
    """
    return self._grad_clf_terms(polar, state_goal).sum(axis=-1)

ControllerCLF

Aicardi, M., Casalino, G., Bicchi, A., & Balestrino, A. (1995). Closed loop steering of unicycle like vehicles via Lyapunov techniques. IEEE Robotics & Automation Magazine, 2(1), 27-35.

Source code in bayes_cbf/unicycle_move_to_pose.py

class ControllerCLF:
    """
    Aicardi, M., Casalino, G., Bicchi, A., & Balestrino, A. (1995). Closed loop steering of unicycle like vehicles via Lyapunov techniques. IEEE Robotics & Automation Magazine, 2(1), 27-35.
    """
    def __init__(self, # simulation parameters
                 planner,
                 u_dim = 2,
                 coordinate_converter = None, # cartesian2polar/ lambda x, xg: x
                 dynamics = None, # PolarDynamics()/CartesianDynamics()
                 clf = None, # CLFPolar()/CLFCartesian()
                 clf_gamma = 10,
                 clf_relax_weight = 10,
                 cbfs = [],
                 cbf_gammas = [],
                 max_risk=1e-2,
                 visualizer = None):
        self.planner = planner
        self.u_dim = 2
        self.coordinate_converter = coordinate_converter
        self.dynamics = dynamics
        self.clf = clf
        self.clf_gamma = 10
        self.clf_relax_weight = 10
        self.cbfs = cbfs
        self.cbf_gammas = cbf_gammas
        self.max_risk = max_risk
        self.visualizer = visualizer

    @property
    def model(self):
        return self.dynamics

    def _clc(self, x, state_goal, t):
        polar = self.coordinate_converter(x, state_goal)
        fx = self.dynamics.f_func(polar)
        gx = self.dynamics.g_func(polar)
        gclf = self.clf.grad_clf(polar, state_goal)
        gclf_goal = self.clf.grad_clf_wrt_goal(polar, state_goal)
        TBLOG.add_scalar("x_0", x[0], t)
        bfa = to_numpy(gclf @ gx)
        b = to_numpy(gclf @ fx
                     + gclf_goal @ self.planner.dot_plan(t)
                     + self.clf_gamma * self.clf.clf_terms(polar, state_goal).sum())
        return bfa, b

    def _cbcs(self, x_in, state_goal, t):
        x = self.coordinate_converter(x_in, state_goal)
        fx = self.dynamics.f_func(x)
        gx = self.dynamics.g_func(x)
        for cbf, cbf_gamma in zip(self.cbfs, self.cbf_gammas):
            gcbf = cbf.grad_cbf(x)
            cbfx = cbf.cbf(x)
            TBLOG.add_scalar("cbf", cbfx, t)
            yield to_numpy(gcbf @ gx), to_numpy(gcbf @ fx + cbf_gamma * cbfx)

    def _ctrl_ref(self, x, u):
        return np.zeros_like(x)

    def control(self, x_torch, t):
        state_goal = self.planner.plan(t)
        import cvxpy as cp # pip install cvxpy
        x = x_torch
        uvar = cp.Variable(self.u_dim)
        uvar.value = np.zeros(self.u_dim)
        relax = cp.Variable(1)
        obj = cp.Minimize(cp.sum_squares(uvar) + self.clf_relax_weight * relax)
        clc_bfa, clc_b = self._clc(x, state_goal, t)
        constraints = [
            uvar >= np.array([-10., -np.pi*5]),
            uvar <= np.array([10., np.pi*5]),
            clc_bfa @ uvar + clc_b - relax <= 0]
        for cbc_bfa, cbc_b in self._cbcs(x, state_goal, t):
            constraints.append(cbc_bfa @ uvar + cbc_b >= 0)

        problem = cp.Problem(obj, constraints)
        problem.solve(solver='GUROBI')
        if problem.status not in ["infeasible", "unbounded"]:
            # Otherwise, problem.value is inf or -inf, respectively.
            # print("Optimal value: %s" % problem.value)
            pass
        else:
            raise ValueError(problem.status)
        # for variable in problem.variables():
        #     print("Variable %s: value %s" % (variable.name(), variable.value))
        uopt =  torch_to(torch.from_numpy(uvar.value),
                        device=getattr(x_torch, 'device', None),
                        dtype=x_torch.dtype)
        if hasattr(self.dynamics, 'train'):
            self.dynamics.train(x_torch, uopt)
        return uopt

    def isconverged(self, state, state_goal):
        return self.clf.isconverged(state, state_goal)

ObstacleCBF

Source code in bayes_cbf/unicycle_move_to_pose.py

class ObstacleCBF:
    def __init__(self, center, radius, term_weights=[0.5, 0.5]):
        self.center = center
        self.radius = radius
        self.term_weights = term_weights

    def _cbf_radial(self, state):
        return ((state[:2] - self.center)**2).sum() - self.radius**2

    def _cbf_heading(self, state):
        good_heading = state[:2] - self.center
        good_heading_norm = good_heading / torch.norm(good_heading)
        return torch.cos(state[2]) * good_heading_norm[0] + torch.sin(state[2]) * good_heading_norm[1]

    def _cbf_terms(self, state):
        return [self._cbf_radial(state), self._cbf_heading(state)]

    def cbf(self, state):
        self.center, self.radius = map(partial(torch_to,
                                               device=state.device,
                                               dtype=state.dtype),
                                       (self.center, self.radius))
        return sum(w * t for w, t in zip(self.term_weights, self._cbf_terms(state)))

    def _grad_cbf_radial(self, state):
        """
        >>> self = ObstacleCBF(torch.rand(2), torch.rand(1))
        >>> x0 = torch.rand(3)
        >>> ajac = self._grad_cbf_radial(x0)
        >>> njac = numerical_jac(lambda x: self._cbf_radial(x), x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        gcbf = torch.zeros_like(state)
        gcbf[..., :2] = 2*(state[..., :2]-self.center)
        return gcbf

    def _grad_cbf_heading(self, state):
        """
        α = atan2(y, x)

        [∂ cos(α-θ),     ∂ cos(α - θ)  ∂ cos(θ - α) ]
        [------------,   ------------, -----------  ]
        [∂ x             ∂ y           ∂ θ          ]
        >>> self = ObstacleCBF(torch.rand(2), torch.rand(1))
        >>> x0 = torch.rand(3)
        >>> ajac = self._grad_cbf_heading(x0)
        >>> njac = numerical_jac(lambda x: self._cbf_heading(x), x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        gcbf = torch.zeros_like(state)
        θ = state[..., 2]
        good_heading = state[..., :2] - self.center
        ρ = torch.norm(good_heading)
        α = torch.atan2(good_heading[..., 1], good_heading[..., 0])
        # ∂ / ∂ x cos(α-θ) = sin(α - θ) y / ρ²
        gcbf[..., 0] = (α - θ).sin() * good_heading[..., 1] / ρ**2
        # ∂ / ∂ y cos(α-θ) = - sin(α - θ) x / ρ²
        gcbf[..., 1] = - (α - θ).sin() * good_heading[..., 0] / ρ**2
        # ∂ / ∂ θ cos(θ-α) = - sin(θ - α)
        gcbf[..., 2] = - (θ-α).sin()
        return gcbf

    def _grad_cbf_terms(self, state):
        return [self._grad_cbf_radial(state), self._grad_cbf_heading(state)]

    def grad_cbf(self, state):
        """
        >>> self = ObstacleCBF(torch.rand(2), torch.rand(1))
        >>> x0 = torch.rand(3)
        >>> ajac = self.grad_cbf(x0)
        >>> njac = numerical_jac(lambda x: self.cbf(x), x0, 1e-6)[0]
        >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
        """
        self.center, self.radius = map(partial(torch_to,
                                               device=state.device,
                                               dtype=state.dtype),
                                       (self.center, self.radius))
        return sum(w * t for w, t in zip(self.term_weights,
                                         self._grad_cbf_terms(state)))

grad_cbf(state)

self = ObstacleCBF(torch.rand(2), torch.rand(1)) x0 = torch.rand(3) ajac = self.grad_cbf(x0) njac = numerical_jac(lambda x: self.cbf(x), x0, 1e-6)[0] testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)

Source code in bayes_cbf/unicycle_move_to_pose.py

def grad_cbf(self, state):
    """
    >>> self = ObstacleCBF(torch.rand(2), torch.rand(1))
    >>> x0 = torch.rand(3)
    >>> ajac = self.grad_cbf(x0)
    >>> njac = numerical_jac(lambda x: self.cbf(x), x0, 1e-6)[0]
    >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
    """
    self.center, self.radius = map(partial(torch_to,
                                           device=state.device,
                                           dtype=state.dtype),
                                   (self.center, self.radius))
    return sum(w * t for w, t in zip(self.term_weights,
                                     self._grad_cbf_terms(state)))

cartesian2polar(state, state_goal)

rho is the distance between the robot and the goal position : \sqrt((x-x)^2 + (y-y)^2) alpha is the heading of the robot relative the angle to the goal : theta - atan2((y-y),(x-x)) beta is the goal position relative to the angle to the goal : theta - atan2((y-y),(x*-x))

state = torch.rand(3) torch.tensor([2, 2, 2math.pi]) - torch.tensor([1, 1, math.pi]) state_goal = torch.rand(3) torch.tensor([2, 2, 2math.pi]) - torch.tensor([1, 1, math.pi]) polar = cartesian2polar(state, state_goal) statep = polar2cartesian(polar, state_goal) testing.assert_allclose(state, statep)

Source code in bayes_cbf/unicycle_move_to_pose.py

def cartesian2polar(state: CartesianState, state_goal : CartesianState) -> PolarState:
    """
    rho is the distance between the robot and the goal position
    : \sqrt((x*-x)^2 + (y*-y)^2)
    alpha is the heading of the robot relative the angle to the goal
    : theta - atan2((y*-y),(x*-x))
    beta is the goal position relative to the angle to the goal
    : theta* - atan2((y*-y),(x*-x))

    >>> state = torch.rand(3)* torch.tensor([2, 2, 2*math.pi]) - torch.tensor([1, 1, math.pi])
    >>> state_goal = torch.rand(3)* torch.tensor([2, 2, 2*math.pi]) - torch.tensor([1, 1, math.pi])
    >>> polar = cartesian2polar(state, state_goal)
    >>> statep = polar2cartesian(polar, state_goal)
    >>> testing.assert_allclose(state, statep)
    """
    x, y, theta = state
    x_goal, y_goal, theta_goal = state_goal

    x_diff = x_goal - x
    y_diff = y_goal - y

    # reparameterization
    rho = torch.sqrt(x_diff**2 + y_diff**2)
    assert rho.abs() > 1e-6, "Invalid conversion"
    phi = torch.atan2(y_diff, x_diff)
    alpha = angdiff(theta, phi)
    beta = angdiff(theta_goal , phi)
    return torch.tensor((rho, alpha, beta))

move_to_pose(state_start, state_goal, dt=0.01, show_animation=True, controller=None, dynamics=CartesianDynamics(), visualizer=None)

rho is the distance between the robot and the goal position alpha is the angle to the goal relative to the heading of the robot beta is the angle between the robot's position and the goal position plus the goal angle

Kp_rhorho and Kp_alphaalpha drive the robot along a line towards the goal Kp_beta*beta rotates the line so that it is parallel to the goal angle

Source code in bayes_cbf/unicycle_move_to_pose.py

def move_to_pose(state_start, state_goal,
                 dt = 0.01,
                 show_animation = True,
                 controller=None,
                 dynamics=CartesianDynamics(),
                 visualizer=None
):
    """
    rho is the distance between the robot and the goal position
    alpha is the angle to the goal relative to the heading of the robot
    beta is the angle between the robot's position and the goal position plus the goal angle

    Kp_rho*rho and Kp_alpha*alpha drive the robot along a line towards the goal
    Kp_beta*beta rotates the line so that it is parallel to the goal angle
    """
    visualizer = (Visualizer(NoPlanner(state_goal), dt)
                  if visualizer is None else
                  visualizer)
    state = state_start.clone()
    count = 0
    dynamics.set_init_state(state)
    while not controller.isconverged(state, state_goal):
        x, y, theta = state

        # control
        ctrl = controller.control(state, t=count)

        # simulation
        state = dynamics.step(ctrl, dt)['x']

        if show_animation:  # pragma: no cover
            visualizer.setStateCtrl(state, ctrl, t=count)
        count = count + 1

numerical_jac(func, x0_in, eps, dtype=torch.float64)

def func(x): return torch.tensor([torch.cos(x[0]), torch.sin(x[1])]) def jacfunc(x): return torch.tensor([[-torch.sin(x[0]), 0], [0, torch.cos(x[1])]]) x0 = torch.rand(2) njac = numerical_jac(func, x0, 1e-6) ajac = jacfunc(x0) testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)

Source code in bayes_cbf/unicycle_move_to_pose.py

def numerical_jac(func, x0_in, eps, dtype=torch.float64):
    """
    >>> def func(x): return torch.tensor([torch.cos(x[0]), torch.sin(x[1])])
    >>> def jacfunc(x): return torch.tensor([[-torch.sin(x[0]), 0], [0, torch.cos(x[1])]])
    >>> x0 = torch.rand(2)
    >>> njac = numerical_jac(func, x0, 1e-6)
    >>> ajac = jacfunc(x0)
    >>> testing.assert_allclose(njac, ajac, rtol=1e-3, atol=1e-4)
    """
    x0 = torch_to(x0_in, dtype=dtype)
    f0 = func(x0)
    m = f0.shape[-1] if len(f0.shape) else 1
    jac = torch.empty((m, x0.shape[-1]), dtype=x0.dtype)
    Dx = eps * torch.eye(x0.shape[-1], dtype=x0.dtype)
    XpDx = x0 + Dx
    for c in range(x0.shape[-1]):
        jac[:, c:c+1] = (func(XpDx[c, :]).reshape(-1, 1) - f0.reshape(-1, 1)) / eps

    return torch_to(jac, dtype=x0_in.dtype,
                    device=getattr(x0_in, 'device', None))

polar2cartesian(x, state_goal)

rho is the distance between the robot and the goal position : \sqrt((x-x)^2 + (y-y)^2)

alpha is the heading of the robot relative the angle to the goal : theta - atan2((y-y),(x-x))

beta is the goal position relative to the angle to the goal : theta - atan2((y-y),(x*-x))

polar = (torch.rand(3) * torch.tensor([1, 2math.pi, 2math.pi]) - ... torch.tensor([0, math.pi, math.pi])) state_goal = (torch.rand(3) * torch.tensor([2, 2, 2*math.pi]) - ... torch.tensor([1, 1, math.pi])) state = polar2cartesian(polar, state_goal) polarp = cartesian2polar(state, state_goal) testing.assert_allclose(polar, polarp)

Source code in bayes_cbf/unicycle_move_to_pose.py

def polar2cartesian(x: PolarState, state_goal : CartesianState) -> CartesianState:
    """
    rho is the distance between the robot and the goal position
    : \sqrt((x*-x)^2 + (y*-y)^2)

    alpha is the heading of the robot relative the angle to the goal
    : theta - atan2((y*-y),(x*-x))

    beta is the goal position relative to the angle to the goal
    : theta* - atan2((y*-y),(x*-x))

    >>> polar = (torch.rand(3) * torch.tensor([1, 2*math.pi, 2*math.pi]) -
    ...         torch.tensor([0, math.pi, math.pi]))
    >>> state_goal = (torch.rand(3) * torch.tensor([2, 2, 2*math.pi]) -
    ...              torch.tensor([1, 1, math.pi]))
    >>> state = polar2cartesian(polar, state_goal)
    >>> polarp = cartesian2polar(state, state_goal)
    >>> testing.assert_allclose(polar, polarp)
    """
    rho, alpha, beta = x
    x_goal, y_goal, theta_goal = state_goal
    phi = angdiff(theta_goal, beta)
    theta = normalize_radians(phi + alpha)
    x_diff = rho * torch.cos(phi)
    y_diff = rho * torch.sin(phi)
    return torch.tensor([x_goal - x_diff,
                     y_goal - y_diff,
                     theta])

track_trajectory_ackerman_clf_bayesian(x, x_g, dt=None, cbfs=None, cbf_gammas=None, numSteps=None, enable_learning=True, mean_dynamics_gen=partial(AckermannDrive, L=10.0), true_dynamics_gen=partial(AckermannDrive, L=1.0), visualizer_class=Visualizer, controller_class=ControllerCLFBayesian, train_every_n_steps=20, **kw)

mean_dynamics is either ZeroDynamicsModel(m = 2, n = 3) or AckermannDrive(L = 10.0)

Source code in bayes_cbf/unicycle_move_to_pose.py

def track_trajectory_ackerman_clf_bayesian(x, x_g, dt = None,
                                           cbfs = None,
                                           cbf_gammas = None,
                                           numSteps = None,
                                           enable_learning = True,
                                           mean_dynamics_gen=partial(AckermannDrive,
                                                                     L = 10.0),
                                           true_dynamics_gen=partial(AckermannDrive,
                                                                     L = 1.0),
                                           visualizer_class=Visualizer,
                                           controller_class=ControllerCLFBayesian,
                                           train_every_n_steps = 20,
                                           **kw):
    """
    mean_dynamics is either ZeroDynamicsModel(m = 2, n = 3) or AckermannDrive(L = 10.0)
    """
    visualizer = visualizer_class(
        PiecewiseLinearPlanner(x, x_g, numSteps, dt),
        dt,
        cbfs = cbfs(x, x_g)
    )
    return sample_generator_trajectory(
        dynamics_model=true_dynamics_gen(),
        D=numSteps,
        controller=controller_class(
            PiecewiseLinearPlanner(x, x_g, numSteps, dt,
                                   frac_time_to_reach_goal=0.95),
            coordinate_converter = lambda x, x_g: x,
            dynamics = LearnedShiftInvariantDynamics(
                dt = dt,
                mean_dynamics = mean_dynamics_gen(),
                enable_learning = enable_learning,
                train_every_n_steps = train_every_n_steps
            ),
            # dynamics = ZeroDynamicsBayesian(m = 2, n = 3),
            clf = CLFCartesian(
                Kp = torch.tensor([0.9, 1.5, 0.])
            ),
            cbfs = cbfs(x , x_g),
            cbf_gammas = cbf_gammas,
            visualizer = visualizer
        ).control,
        visualizer = visualizer,
        x0=x,
        dt=dt,
        **kw)