Factor out joint limits

Kinematics/Arm.idr

@@ -27,8 +27,7 @@ export
 getLimits : (arm : ArmElement n) -> Vect (countJoints arm) (Double, Double)
 getLimits [] = []
 getLimits (Left _ :: xs) = getLimits xs
-getLimits (Right (Revolute a b) :: xs) = (a,b) :: getLimits xs
-getLimits (Right (Prismatic a b) :: xs) = (a,b) :: getLimits xs
+getLimits (Right (MkJoint _ l u) :: xs) = (l,u) :: getLimits xs
 
 export
 link : Vector n Double -> ArmElement n
@@ -38,24 +37,23 @@ export
 linkX : {n : _} -> Double -> ArmElement (1 + n)
 linkX x = link $ vector (x :: replicate n 0)
 
-export
-revolute2D : (a, b : Double) -> ArmElement 2
-revolute2D a b = [Right $ Revolute a b]
 
 export
-revoluteX : (a, b : Double) -> ArmElement 3
-revoluteX a b = [Left $ cast (Rotation.rotate3DY (pi/2)),
-                 Right $ Revolute a b,
+revolute2D : (l, u : Double) -> ArmElement 2
+revolute2D l u = [Right $ MkJoint Revolute l u]
+
+export
+revoluteX : (l, u : Double) -> ArmElement 3
+revoluteX l u = [Left $ cast (Rotation.rotate3DY (pi/2)),
+                 Right $ MkJoint Revolute l u,
                  Left $ cast (Rotation.rotate3DY (-pi/2))]
 
 export
-revoluteY : (a, b : Double) -> ArmElement 3
-revoluteY a b = [Left $ cast (Rotation.rotate3DX (-pi/2)),
-                 Right $ Revolute a b,
+revoluteY : (l, u : Double) -> ArmElement 3
+revoluteY l u = [Left $ cast (Rotation.rotate3DX (-pi/2)),
+                 Right $ MkJoint Revolute l u,
                  Left $ cast (Rotation.rotate3DX (pi/2))]
 
 export
-revoluteZ : (a, b : Double) -> ArmElement 3
-revoluteZ a b = [Right $ Revolute a b]
-
-
+revoluteZ : (l, u : Double) -> ArmElement 3
+revoluteZ l u = [Right $ MkJoint Revolute l u]

Kinematics/Joint.idr

@@ -7,20 +7,28 @@ import Data.NumIdr
 
 
 public export
-data Joint : Nat -> Type where
-  Revolute : (a, b : Double) -> Joint (2 + n)
-  Prismatic : (a, b : Double) -> Joint (1 + n)
+data JointType : Nat -> Type where
+  Revolute : JointType (2 + n)
+  Prismatic : JointType (1 + n)
+
+public export
+record Joint n where
+  constructor MkJoint
+  ty : JointType n
+  l, u : Double
 
 
 export
 jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double)
-jointAction {n=S n} (Prismatic a b) x =
-  guard (a < x && x < b)
+jointAction {n=S n} (MkJoint Prismatic l u) x =
+  guard (l <= x && x <= u)
     $> cast (translate $ vector $ x :: replicate n 0)
-jointAction {n=S (S n)} (Revolute a b) x =
-  guard (a < x && x < b)
+jointAction {n=S (S n)} (MkJoint Revolute l u) x =
+  guard (l <= x && x <= u)
     $> unsafeMkTrans (indexSetRange [EndBound 2,EndBound 2]
                         (rewrite rangeLenZ 2 in rotate2D x) identity)
+-- Idris isn't smart enough to recognize the above two clauses cover all inputs
+jointAction _ _ = Nothing
 
 
 -- Links are directly represented by rigid transformations, i.e. rotations