Add inline documentation

src/Kinematics/Arm.idr

@@ -7,53 +7,93 @@ import public Kinematics.Joint
 %default total
 
 
+||| The type of a robot arm, or elements of a robot arm.
+|||
+||| An `ArmElement` is a list of multiple *links* and *joints*, chained
+||| left-to-right. The left end is fixed at the origin, and the right end is
+||| allowed to move freely.
+|||
+||| Any two arm elements can be chained using the `<+>` operator, which attaches
+||| the right end of the first argument to the left end of the second. This is
+||| how all arms are constructed in this library's data model.
+|||
+||| @ n The number of dimensions the robot arm operates in. Convenience functions
+||| are provided for two- or three-dimensional arms, but theoretically any
+||| number of dimensions is supported.
 public export
-ArmElement : Nat -> Type
+ArmElement : (n : Nat) -> Type
 ArmElement n = List (Either (Link n) (Joint n))
 
 
+||| Count the number of joints in a robot arm.
 public export
 countJoints : ArmElement n -> Nat
 countJoints [] = 0
 countJoints (Left _ :: xs) = countJoints xs
 countJoints (Right _ :: xs) = S $ countJoints xs
 
+
+||| The type of an arm's joint values.
+||| If you see this in a function's type, it represents a `Vector` with length
+||| corresponding to the number of joints in the arm.
 public export
 ArmConfig : ArmElement n -> Type
 ArmConfig arm = Vector (countJoints arm) Double
 
 
+||| Get a list of the limits of each joint, in order.
 export
 getLimits : (arm : ArmElement n) -> Vect (countJoints arm) (Double, Double)
 getLimits [] = []
 getLimits (Left _ :: xs) = getLimits xs
 getLimits (Right (MkJoint _ l u) :: xs) = (l,u) :: getLimits xs
 
+
+||| A link pointing in the direction given by the input vector.
+||| This arm element is valid in any number of dimensions.
 export
 link : Vector n Double -> ArmElement n
 link v = [Left $ cast (translate v)]
 
+||| A link along the X axis of the specified length.
+||| This arm element is valid in any non-zero number of dimensions.
 export
 linkX : {n : _} -> Double -> ArmElement (1 + n)
 linkX x = link $ vector (x :: replicate n 0)
 
 
+||| A two-dimensional revolute joint with the given limits.
+|||
+||| @ l The lower limit angle of the joint
+||| @ u The upper limit angle of the joint
 export
 revolute2D : (l, u : Double) -> ArmElement 2
 revolute2D l u = [Right $ MkJoint Revolute l u]
 
+||| A three-dimensional revolute joint that rotates along the X axis.
+|||
+||| @ l The lower limit angle of the joint
+||| @ u The upper limit angle of the joint
 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))]
 
+||| A three-dimensional revolute joint that rotates along the Y axis.
+|||
+||| @ l The lower limit angle of the joint
+||| @ u The upper limit angle of the joint
 export
 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))]
 
+||| A three-dimensional revolute joint that rotates along the Z axis.
+|||
+||| @ l The lower limit angle of the joint
+||| @ u The upper limit angle of the joint
 export
 revoluteZ : (l, u : Double) -> ArmElement 3
 revoluteZ l u = [Right $ MkJoint Revolute l u]

src/Kinematics/Forward.idr

@@ -7,6 +7,8 @@ import public Kinematics.Arm
 %default total
 
 
+||| Calculate the homogeneous matrix representing the end affector's position,
+||| given an arm and a vector of input joint values.
 export
 forwardTransform : {n : _} -> (arm : ArmElement n) -> ArmConfig arm
                     -> Maybe (Rigid n Double)
@@ -19,6 +21,8 @@ forwardTransform arm = go arm . toVect
     go (Right j :: xs) (c :: cs) = [| jointAction j c *. go xs cs |]
 
 
+||| Calculate the position of the end affector, given an arm and a vetor of
+||| input joint values.
 export
 forward : {n : _} -> (arm : ArmElement n) -> ArmConfig arm ->
             Maybe (Point n Double)

src/Kinematics/Inverse.idr

@@ -25,10 +25,19 @@ initialSimplex arm =
           Fin.range
 
 
+||| Given an arm and an end affector position, try to find a joint configuration
+||| that approximates the given position. If no such configuration is found
+||| within the arm's joint limits, `Nothing` is returned.
+|||
+||| @ fuel A value limiting the number of iterations the optimization algorithm
+||| performs
 export
 inverse : {n : _} -> (fuel : Fuel) -> (arm : ArmElement n) -> Point n Double
-            -> {auto 0 ok : IsSucc (countJoints arm)} -> Maybe (ArmConfig arm)
+            -> Maybe (ArmConfig arm)
-inverse fuel arm p = go fuel !(initialSimplex arm)
+inverse fuel arm p =
+  case isItSucc (countJoints arm) of
+    No _ => Nothing
+    Yes ok => go fuel !(initialSimplex arm) {ok}
   where
     sndLast : forall n. {auto 0 ok : IsSucc n} -> Vect (S n) a -> a
     sndLast {n=S n,ok=ItIsSucc} v = last $ init v
@@ -41,7 +50,8 @@ inverse fuel arm p = go fuel !(initialSimplex arm)
     sort : Simplex arm -> Simplex arm
     sort s = believe_me $ Vect.fromList $ sortBy (compare `on` cost) $ toList s
 
-    go : Fuel -> Simplex arm -> Maybe (ArmConfig arm)
+    go : Fuel -> Simplex arm -> {auto 0 ok : IsSucc (countJoints arm)}
+            -> Maybe (ArmConfig arm)
     go Dry _ = Nothing
     go (More fuel) simplex = do
       guard (all (and . zipWith (\(a,b),x => a <= x && x <= b)
@@ -49,7 +59,7 @@ inverse fuel arm p = go fuel !(initialSimplex arm)
         let simplex = unsafePerformIO (let s = sort simplex in printLn s $> s)
             best = head simplex
             cbest = !(cost best)
-        in  if cbest < 0.00001 then Just best
+        in  if cbest < 0.0000001 then Just best
         else let
             worst = last simplex
             cworst = !(cost worst)

src/Kinematics/Joint.idr

@@ -6,11 +6,37 @@ import Data.NumIdr
 %default total
 
 
+||| The type of a joint, one of `Revolute` or `Prismatic`.
+|||
+||| @ n The number of dimensions this joint operates in
 public export
-data JointType : Nat -> Type where
+data JointType : (n : Nat) -> Type where
+  ||| A revolute joint rotates in a plane.
+  |||
+  ||| The primitive revolute joint element only rotates in the XY plane,
+  ||| or the plane composed of the first two axes. In order to construct a
+  ||| revolute joint that rotates in a different plane, links must be used to
+  ||| change the joint's orientation.
+  |||
+  ||| Revolute joints cannot be used in a space of less than two dimensions,
+  ||| as the concept of rotation cannot be defined in those cases.
   Revolute : JointType (2 + n)
+
+  ||| A prismatic joint moves linearly along an axis.
+  |||
+  ||| The primitive prismatic joint element only moves in the X axis, or the
+  ||| axis consisting of the first coordinate. In order to construct a prismatic
+  ||| joint that moves along a different axis, links must be used to change the
+  ||| joint's orientation.
+  |||
+  ||| Prismatic joints cannot be used in a zero-dimensional space, as they
+  ||| require at least one axis to exist.
   Prismatic : JointType (1 + n)
 
+
+||| A joint of a particular type, stored along with its limits.
+|||
+||| @ n The number of dimensions this joint operates in
 public export
 record Joint n where
   constructor MkJoint
@@ -18,6 +44,9 @@ record Joint n where
   l, u : Double
 
 
+||| Calculate the homogeneous matrix generated by a joint given an
+||| input value. `Nothing` is returned if the input value is outside
+||| the joint's limits.
 export
 jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double)
 jointAction {n=S n} (MkJoint Prismatic l u) x =