Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- q''[t] = Inverse[a(q[t],q'[t])].q'[t]
- n = 3 (* Sets exponent for the "a" matrix, fast if n=1, slow for n >=3*)
- q = {{q1[t]}, {q2[t]}, {q3[t]}, {q4[t]}} (* configuration variables*)
- dq = D[q, t] (* velocity variables*)
- ddq = D[dq, t] (* acceleration variables*)
- (* Setting up initial conditions*)
- q0 = Thread[Flatten@q == 0.1] /. t -> 0
- dq0 = Thread[Flatten@dq == -0.1] /. t -> 0
- (* Large matrix that needs to be inverted A = Inverse[a[q,dq]]*)
- A[q_, dq_, n_] := Inverse[
- {{2*Cos[dq[[1]]], 2*Sin[q[[4]]], q[[3]] + Sin[q[[4]]], Cos[dq[[1]]] + Sin[dq[[4]]]},
- {2*Sin[q[[4]]], 2*q[[1]], Cos[dq[[1]]] + Sin[q[[3]]], Cos[dq[[1]]] + dq[[1]]},
- {q[[3]] + Sin[q[[4]]], Cos[dq[[1]]] + Sin[q[[3]]], q[[4]] + Sin[q[[4]]], Cos[dq[[1]]] + q[[2]]},
- {Cos[dq[[1]]] + dq[[4]], Cos[dq[[1]]] + dq[[1]], Cos[dq[[1]]] + q[[2]], dq[[4]] + Tan[dq[[4]]]}}^n]
- {tvar, RHS} = Timing[Flatten[A[Flatten@q, Flatten@dq, n].dq]];
- Print[tvar] (* print execution time for evaluating A matrix*)
- ddqEqs = Thread[Flatten@ddq == RHS]; (* Setting up equations*)
- NDSolve[Join[ddqEqs, q0, dq0], Flatten@q, {t, 0, 1}] (* Run numerical integration*)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement