loop_canonical.sa 2.0 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374
  1. semantic adaptation reactive moore LoopSA loop_sa
  2. at "./path/to/LoopSA.fmu"
  3. for inner fmu WindowSA window_sa
  4. at "./path/to/WindowSA.fmu"
  5. with input ports displacement (rad), speed (rad/s), reaction_force (N)
  6. with output ports disp (m), tau (N.m)
  7. for inner fmu Obstacle obstacle
  8. at "./path/to/Obstacle.fmu"
  9. with input ports disp (m)
  10. with output ports reaction_force (m)
  11. coupled as window_sa.disp -> obstacle.disp,
  12. obstacle.reaction_force -> window_sa.reaction_force
  13. input ports displacement, speed
  14. output ports tau
  15. param MAXITER := 10, REL_TOL := 1e-05, ABS_TOL := 1e-05,
  16. INIT_LOOP_SA_DISPLACEMENT := 0.0,
  17. INIT_LOOP_SA_SPEED := 0.0,
  18. INIT_WINDOW_SA_DISP := 0.0,
  19. INIT_WINDOW_SA_TAU := 0.0,
  20. INIT_OBSTACLE_REACTION_FORCE := 0.0;
  21. control var prev_disp := 0.0;
  22. control rules {
  23. var repeat := false;
  24. for (var iter in 0 .. MAXITER) {
  25. save_state(obstacle);
  26. save_state(window_sa);
  27. obstacle.disp := prev_disp;
  28. do_step(obstacle,t,H);
  29. window_sa.reaction_force := stored_obstacle_reaction_force;
  30. do_step(window_sa,t,H);
  31. repeat := is_close(prev_disp, stored_window_sa_disp, REL_TOL, ABS_TOL);
  32. prev_disp := stored_window_sa_disp;
  33. if (repeat) {
  34. break;
  35. } else {
  36. rollback(obstacle);
  37. rollback(window_sa);
  38. }
  39. }
  40. return H;
  41. }
  42. in var stored_loop_sa_displacement := INIT_LOOP_SA_DISPLACEMENT,
  43. stored_loop_sa_speed := INIT_LOOP_SA_SPEED;
  44. in rules {
  45. true -> {
  46. stored_loop_sa_displacement := loop_sa.displacement;
  47. stored_loop_sa_speed := loop_sa.speed;
  48. } --> {
  49. window_sa.displacement := stored_loop_sa_displacement;
  50. window_sa.speed := stored_loop_sa_speed;
  51. };
  52. }
  53. out var stored_window_sa_disp := INIT_WINDOW_SA_DISP,
  54. stored_window_sa_tau := INIT_WINDOW_SA_TAU,
  55. stored_obstacle_reaction_force := INIT_OBSTACLE_REACTION_FORCE;
  56. out rules{
  57. true -> {
  58. stored_window_sa_disp := window_sa.disp;
  59. stored_window_sa_tau := window_sa.tau;
  60. stored_obstacle_reaction_force := obstacle.tau;
  61. } --> {
  62. loop_sa.tau := stored_window_sa_tau;
  63. };
  64. }