double pendulum from SICM

double-pendulum.scm

;; the lagrangian for two unconstrained masses in gravity
;; (this is just 1/2 m v^2 - mgh from high school)
(define ((L-ms-in-g m-tuple g) local)
 (let ((v-tuple (velocities local))
       (q-tuple (coordinate local)))
   (let ((momenta (uptuple-map (lambda (mi vi) (* (/ 1 2) mi (norm vi))) m-tuple v-tuple))
         (potentials (uptuple-map (lambda (mi qi) (* mi g (ref qi 1))) m-tuple q-tuple)))
     (let ((T (uptuple-reduce + momenta))
           (V (uptuple-reduce + potentials)))
       (- T V)))))

;; a coordinate transformation that maps the pendulum angle coordinates
;; (theta0 theta1) into the ((x0 y0) (x1 y1)) coordinates
(define ((angles->rect ls) local)
 (let ((thetas (coordinate local)))
   (define (theta->xy theta l)
       (let ((x (* l (sin theta)))
             (y (- 0 (* l (cos theta)))))
         (up x y)))
   (let ((incs (uptuple-map theta->xy thetas ls)))
       (uptuple-cumu + incs))))

(define F (angles->rect (up 'l_0 'l_1)))

;; some general calculus to convert the coordinate transformation to a local tuple transformation
;; (basically just generating the velocity mapping for us based on the coordinate mapping above)
(define ((F->C F) local)
 (->local (time local)
          (F local)
          (+ (((partial 0) F) local)
             (* (((partial 1) F) local) 
                (velocity local)))))

;; now we just call it on a symbolically-parameterized path
(define q (up (literal-function 'theta_0) (literal-function 'theta_1)))
(show-expression (((Lagrange-equations (compose L (F->C F))) q) 't))