anchor.diff:  	       (.* (.exp (.* tw5 q5)) 
   		   (.* (.exp  (.* tw6 q6)) H_T_0)))))))
   
  -(defun g_T_0 (q1 q2 q3 q4 q5 q6)
  -  (.* (.exp (.* tw1 q1)) H_T_0))
  -   
  -(setq d_h (der (d-var q1 g_T_0 ((q1 0.0) 
  -			    (q2 0.0)
  -			    (q3 0.0)
  -			    (q4 0.0)
  -			    (q5 0.0)
  -			    (q6 0.0) ))))
   
  -(setf (rot d_h) (.* (rot d_h) 2.0))
   
  -;;Spatial velocity
  -(setq ang-s (ang (.*-1 d_h (g_T_0 0 0 0 0 0 0))))
  -(setq lin-s (lin (.*-1 d_h (g_T_0 0 0 0 0 0 0))))
  +(let* ((q1_0 pi)
  +       (q2_0 0)
  +       (q3_0 0)
  +       (q4_0 0)
  +       (q5_0 0)
  +       (q6_0 0)
  +       (gt0 (g_t_0 q1_0  q2_0  q3_0  q4_0 q5_0  q6_0)))
  +  (progn 
  +    (setq d_h (der (d-var q5 g_T_0 ((q1 q1_0)
  +				    (q2 q2_0)
  +				    (q3 q3_0)
  +				    (q4 q4_0)
  +				    (q5 q5_0)
  +				    (q6 q6_0)))))
  +    ;;Spatial velocity
  +    (setq ang-s (ang (.*-1 d_h gt0)))
  +    (setq lin-s (lin (.*-1 d_h gt0)))
  +    (setq tw-r (.*-1 d_h gt0))
  +    ))
   
  -(.*-1 d_h (g_T_0 0 0 0 0 0 0))
   
  -(.* (.* (.* (rot d_h) (rot (inv (g_t_0 0 0 0 0 0 0)))) 2.0) (pos H_T_0))
  
positive.code: 		(0 0 0 1 ad)))
  
  
  (defun g_T_0# (q1 q2 q3 q4 q5 q6)
    (.* (.exp (.* tw6 q6)) H_T_0))
  
  
  (defun g_T_0 (q1 q2 q3 q4 q5 q6)
    (.* (.exp (.* tw1 q1))
     (.* (.exp (.* tw2 q2))
  
negative.code: (in-package #:lisphys)
  
  ;;Murray Ly Sastry p.90
  
  (setq l0 1.5)
  (setq l1 1.5)
  