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. OK, I Understand
 
Top