Maybe I find a bug!

[Mike1425217072]

In function [t0dot,tLdot]=Accelerations(t0,tL,P0,pm,Bi0,Bij,u0,um,u0dot,umdot,robot)

line 49, %Base-link Omega0=[SkewSym(t0(1:3)), zeros(3,3); zeros(3,3), zeros(3,3)];

This should be:

Omega0=[SkewSym(t0(1:3)), zeros(3,3); zeros(3,3), SkewSym(t0(1:3))];

right?