compute_accel.m

%Computes the linear and angular acceleration of the box
%given its current position and orientation
%INPUTS:
%x: current x position of the box
%y: current y position of the box
%theta: current orientation of the box
%box_params: a struct containing the parameters that describe the system
%Fields:
%box_params.m: mass of the box
%box_params.I: moment of inertia w/respect to centroid
%box_params.g: acceleration due to gravity
%box_params.k_list: list of spring stiffnesses
%box_params.l0_list: list of spring natural lengths
%box_params.P_world: 2 x n list of static mounting
% points for the spring (in the world frame)
%box_params.P_box: 2 x n list of mounting points
% for the spring (in the box frame)
%
%OUTPUTS
%ax: x acceleration of the box
%ay: y acceleration of the box
%atheta: angular acceleration of the box
function [ax,ay,atheta] = compute_accel(x,y,theta,box_params)
    m=box_params.m;
    I=box_params.I;
    g=box_params.g;
    k_list=box_params.k_list;
    l0_list=box_params.l0_list;
    P_world=box_params.P_world;
    P_box=box_params.P_box;
    Plist_box = compute_rbt(x,y,theta,P_box);
    Plist_world = P_world;
    F_total=0;
    PxF_total=0;
    for i=1:length(k_list)
        PA=Plist_box(:,i);
        PB=Plist_world(:,i);
        F = compute_spring_force(k_list(i),l0_list(i),PB,PA);
        F_total=F_total+F;
        r = PA - [x; y];

        PxF_total=PxF_total+(r(1)*F(2) - r(2)*F(1));
    end
    a = [0; -g] + (1/m)*F_total;
    ax = a(1);  ay = a(2);
    atheta=PxF_total/I;
end