• API
• FAQ
• Tools
• Archive
SHARE
TWEET

# Untitled

a guest Jun 18th, 2019 60 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. q''[t] = Inverse[a(q[t],q'[t])].q'[t]
2.
3. n = 3 (* Sets exponent for the "a" matrix, fast if n=1, slow for n >=3*)
4.
5. q = {{q1[t]}, {q2[t]}, {q3[t]}, {q4[t]}} (* configuration variables*)
6. dq = D[q, t] (* velocity variables*)
7. ddq = D[dq, t] (* acceleration variables*)
8.
9. (* Setting up initial conditions*)
10. q0 = Thread[Flatten@q == 0.1] /. t -> 0
11. dq0 = Thread[Flatten@dq == -0.1] /. t -> 0
12.
13. (* Large matrix that needs to be inverted A = Inverse[a[q,dq]]*)
14. A[q_, dq_, n_] := Inverse[
15. {{2*Cos[dq[[1]]], 2*Sin[q[[4]]], q[[3]] + Sin[q[[4]]], Cos[dq[[1]]] + Sin[dq[[4]]]},
16.  {2*Sin[q[[4]]], 2*q[[1]], Cos[dq[[1]]] + Sin[q[[3]]], Cos[dq[[1]]] + dq[[1]]},
17.  {q[[3]] + Sin[q[[4]]], Cos[dq[[1]]] + Sin[q[[3]]], q[[4]] + Sin[q[[4]]], Cos[dq[[1]]] + q[[2]]},
18.  {Cos[dq[[1]]] + dq[[4]], Cos[dq[[1]]] + dq[[1]], Cos[dq[[1]]] + q[[2]], dq[[4]] + Tan[dq[[4]]]}}^n]
19.
20. {tvar, RHS} = Timing[Flatten[A[Flatten@q, Flatten@dq, n].dq]];
21. Print[tvar] (* print execution time for evaluating A matrix*)
22.
23. ddqEqs = Thread[Flatten@ddq == RHS]; (* Setting up equations*)
24. NDSolve[Join[ddqEqs, q0, dq0], Flatten@q, {t, 0, 1}] (* Run numerical integration*)
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top