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?

[YukiCR]

Hi,
as far as I'm concerned, the calculation of Omega0 is right.
in line 63, we have

t0dot = Omega0*P0*u0+P0*u0dot;

which is
$\dot{t}_0 = (\Omega_0 P_0 )u_0+ P_0 \dot{u}_0$.
so Omega0*P0 should be $\frac{\mathrm{d}}{\mathrm{d}t}P_0$,
since P0 is

[R,           zeros(3);
 zeros(3),   eye(3)];

it's derivative will be the derivative of the top left part of P0, i.e,
R_dot = SkewSym(t0(1:3))*R