%-------------------------------------------------------------------------------------
% File:	   SymDyn_OneLeg3DOF.m
%
% Author: 	Filipe Silva
% Date:		01-Abril,2004
% Purpose:	Symbolic computation of the dynamic model of a 3-dof planar biped 
%           robot using the Lagrangian formulation
%-------------------------------------------------------------------------------------

clear all
clc
t0 = clock;

%-------------------------------------------------------------------------------------
%-- Symbolic variables 
%-------------------------------------------------------------------------------------
syms go t
syms l1 l2 l3					%-- link lengths -------------------------------------
syms r1 r2 r3					%-- link lengths -------------------------------------
syms m1 m2 m3					%-- link masses  -------------------------------------
ang1 = sym('ang1(t)');			%-- shoulder dof ------------------------------------- 
ang2 = sym('ang2(t)');			%-- elbow dof    -------------------------------------
ang3 = sym('ang3(t)');			%-- wrist dof    -------------------------------------
%-------------------------------------------------------------------------------------

%-------------------------------------------------------------------------------------
%-- Mass's coordinates in the reference system
%-------------------------------------------------------------------------------------
x1 = r1*cos(ang1);
y1 = r1*sin(ang1);

x2 = l1*cos(ang1)+r2*cos(ang1+ang2);
y2 = l1*sin(ang1)+r2*sin(ang1+ang2);

x3 = l1*cos(ang1)+l2*cos(ang1+ang2)+r3*cos(ang1+ang2+ang3);
y3 = l1*sin(ang1)+l2*sin(ang1+ang2)+r3*sin(ang1+ang2+ang3);
%-------------------------------------------------------------------------------------

%-------------------------------------------------------------------------------------
%-- Mass's velocities by differentiating
%-------------------------------------------------------------------------------------
dx1 = diff(x1); dy1 = diff(y1);
dx2 = diff(x2); dy2 = diff(y2);
dx3 = diff(x3); dy3 = diff(y3);
%-------------------------------------------------------------------------------------

%-------------------------------------------------------------------------------------
%-- Defining the kinetic and potential energy in a symbolic way
%-------------------------------------------------------------------------------------
K1 = 0.5*m1*(dx1^2+dy1^2); U1 = m1*go*y1;
K2 = 0.5*m2*(dx2^2+dy2^2); U2 = m2*go*y2;
K3 = 0.5*m3*(dx3^2+dy3^2); U3 = m3*go*y3;

K1 = simple( simplify(K1) ); %pretty(K1)
K2 = simple( simplify(K2) ); %pretty(K2)
K3 = simple( simplify(K3) ); %pretty(K3)
L = K1+K2+K3-U1-U2-U3; % pretty(L)
%-------------------------------------------------------------------------------------

%-------------------------------------------------------------------------------------
%-- General expression: Ti=d/dt(dL/ddqi)-dL/dqi
%-------------------------------------------------------------------------------------
Lagrange
%-------------------------------------------------------------------------------------
EXETIME = etime(clock,t0)
pause, close all, return