RATTLE algorithm

RATTLE

Constrained mechanical systems form an important class of differential equations on manifolds. For all the theory that I present here and I've used for the implementation I refer to "Geometric Numerical Integration" by Hairer, Lubich and Wanner.

Consider a mechanical system described by position coordinates \(q_1,\dots q_d\) and suppose that the motion is constrained to satisfy \(g(\mathbf q) = \mathbf 0\) where \(g:\mathbb R^d \rightarrow \mathbb R^m \), with \(m \lt d \). So that, the equations of motion governing the system become: $$ \left\{ \begin{array}{l} \dot{\mathbf q} = \frac{\partial H}{\partial \mathbf p} \\ \dot{\mathbf p} = -\frac{\partial H}{\partial \mathbf q} - G(q)^T\lambda \\ g(\mathbf q) = \mathbf 0 \end{array}\right. $$ where \(G(\mathbf q) = \dfrac{\partial g(\mathbf q)}{\partial \mathbf q} \).

Symplectic Euler method can be extended to constrained systems but we focus on SHAKE and RATTLE algorithms. SHAKE is a 2-steps algorithm so that, since I'm implementing only 1-step algorithms and the overall structure of solvers and integrators is made for 1-step solvers, I implemented just RATTLE algorithm.

odeRATTLE

The RATTLE algorithm implemented works with any general Hamiltonian \(H(\mathbf q,\mathbf p \) and is defined as follows: $$ \left\{\begin{array}{l} \mathbf p_{n+\frac{1}{2}} = \mathbf p_n -\frac{h}{2}\big(H_{\mathbf q}(\mathbf q_n,\mathbf p_{n+\frac{1}{2}}) + G(\mathbf q_n)^T \mathbf {\lambda}_n \big) \\ \mathbf q_{n+1} = \mathbf q_n +\frac{h}{2} \big( H_{\mathbf p}(\mathbf q_n,\mathbf p_{n+\frac{1}{2}}) + H_{\mathbf p}(\mathbf q_{n+1},\mathbf p_{n+\frac{1}{2}}) \big) \\ g(\mathbf q_{n+1}) = \mathbf 0 \\ \mathbf p_{n+1} = \mathbf p_{n+\frac{1}{2}} -\frac{h}{2}\big(H_{\mathbf q}(\mathbf q_{n+1},\mathbf p_{n+\frac{1}{2}}) + G(\mathbf q_{n+1})^T \mathbf{\mu}_n \big) \\ G(\mathbf q_{n+1}) H_{\mathbf p}(\mathbf q_{n+1},\mathbf p_{n+1}) = \mathbf 0 \end{array}\right. $$ where \( h=\Delta t=t_{k+1}-t_k\) and \(\mathbf{\mu}_n \),\( \mathbf{\mu}_n \) are Lagrangian multipliers nedded to impose the constraints.

It can be demonstrated that this numerical method is symmetric, symplectic and convergent of order 2.

The following code represent it's implementation:

function [t_next,x_next,err]=rattle(f,t,x,dt,options)
  H = odeget(options,'HamiltonianHessFcn',[],'fast');
  GG = odeget(options,'ConstraintHessFcn',[],'fast');
  if( ~isempty(H) && ~isempty(GG) )
    fsolve_opts = optimset('Jacobian','on');
  else
    fsolve_opts = optimset('Jacobian','off');
  end
  g = odeget(options,'ConstraintFcn',[],'fast');
  G = odeget(options,'ConstraintGradFcn',[],'fast');
  c_nb = odeget(options,'ConstraintsNb',[],'fast');

  dim = length(x)/2;
  q0 = x(1:dim);
  p0 = x(dim+1:end);

  RATTLE = @(y)constr_sys(y,dim,c_nb,f,H,g,G,GG,t,q0,p0,dt);
  y0 = [q0;p0;p0;zeros(2*c_nb,1)];
  y0 = fsolve(RATTLE,y0,fsolve_opts);
  t_next = t+dt;
  x_next = [y0(1:dim);y0(2*dim+1:3*dim)];

  if(nargout==3)
    dt = dt/2;
    RATTLE = @(y)constr_sys(y,dim,c_nb,f,H,g,G,GG,t,q0,p0,dt);
    y0 = [q0;p0;p0;zeros(2*c_nb,1)];
    y0 = fsolve(RATTLE,y0,fsolve_opts);

    q0 = y0(1:dim);
    p0 = y0(2*dim+1:3*dim);
    t = t+dt;

    RATTLE = @(y)constr_sys(y,dim,c_nb,f,H,g,G,GG,t,q0,p0,dt);
    y0 = [q0;p0;p0;zeros(2*c_nb,1)];
    y0 = fsolve(RATTLE,y0,fsolve_opts);

    x_est = [y0(1:dim);y0(2*dim+1:3*dim)];
    err = norm(x_est-x_next,2);
  end
end

function [F,J] = constr_sys(y,dim,c_nb,f,H,g,G,GG,t,q0,p0,dt)
  F = zeros(3*dim+2*c_nb,1);
  F(1:dim) = y(1:dim) - q0 - (dt/2).*(f(t,[q0; ...
        y(dim+1:2*dim)])(1:dim) + f(t+dt,y(1:2*dim))(1:dim));
  F(dim+1:2*dim) = y(dim+1:2*dim) - p0 - (dt/2).*(f(t,[q0; ...
   y(dim+1:2*dim)])(dim+1:end) - G(q0)'*y(3*dim+1:3*dim+c_nb));
  F(2*dim+1:3*dim) = y(2*dim+1:3*dim) - y(dim+1:2*dim) - ...
             (dt/2)*(f(t+dt,y(1:2*dim))(dim+1:end) - ...
             G(y(1:dim))'*y(3*dim+c_nb+1:end));
  F(3*dim+1:3*dim+c_nb) = g(y(1:dim));
  F(3*dim+c_nb+1:end) = G(y(1:dim))*(f(t+dt,[y(1:dim); ...
                                 y(2*dim+1:3*dim)])(1:dim));

  if( nargout==2 )
    J = zeros(3*dim+2*c_nb,3*dim+2*c_nb);
    J(1:dim,1:dim) = eye(dim) - ...
              (dt/2)*(H(t+dt,y(1:2*dim))(dim+1:end,1:dim));
    J(1:dim,dim+1:2*dim) = -(dt/2)*(H(t,[q0; ...
          y(dim+1:2*dim)])(dim+1:end,dim+1:end) + ...
            H(t+dt,y(1:2*dim))(dim+1:end,dim+1:end));
    J(dim+1:2*dim,dim+1:2*dim) = eye(dim) + ...
        (dt/2)*(H(t,[q0;y(dim+1:2*dim)])(1:dim,dim+1:end));
    J(dim+1:2*dim,3*dim+1:3*dim+c_nb) = (dt/2)*G(q0)';
    J(2*dim+1:3*dim,1:dim) = (dt/2)*(H(t+dt, ...
                            y(1:2*dim))(1:dim,1:dim));
    for k = 1:1:c_nb
      J(2*dim+1:3*dim,1:dim) = J(2*dim+1:3*dim,1:dim) - ...
            (dt/2)*(y(3*dim+c_nb+k)*(GG(y(1:dim))(:,:,k)));
    end
    J(2*dim+1:3*dim,dim+1:2*dim) = -eye(dim) + ...
               (dt/2)*(H(t+dt,y(1:2*dim))(1:dim,dim+1:end));
    J(2*dim+1:3*dim,2*dim+1:3*dim) = eye(dim) + ...
               (dt/2)*(H(t+dt,y(1:2*dim))(1:dim,dim+1:end));
    J(2*dim+1:3*dim,3*dim+c_nb+1:end) = (dt/2)*G(y(1:dim))';
    J(3*dim+1:3*dim+c_nb,1:dim) = G(y(1:dim));
    J(3*dim+c_nb+1:end,1:dim) = G(y(1:dim))* ...
      (H(t+dt,[y(1:dim);y(2*dim+1:3*dim)])(dim+1:end,1:dim));
    for k = 1:1:c_nb
      J(3*dim+c_nb+k,1:dim) = J(3*dim+c_nb+k,1:dim) + ...
          ((GG(y(1:dim))(:,:,k))*(f(t+dt,[y(1:dim); ...
                     y(2*dim+1:3*dim)])(1:dim)))';
    end
    J(3*dim+c_nb+1:end,2*dim+1:3*dim) = G(y(1:dim))* ...
        (H(t+dt,[y(1:dim);y(2*dim+1:3*dim)]) ...
                  (dim+1:end,dim+1:end));
  end
end

It works with any number of constraint, unless this is equal or greater to system dimension. As usual, all the source code is available at my public repository octave-odepkg.