Add inline documentation

This commit is contained in:
Kiana Sheibani 2022-12-13 10:39:18 -05:00
parent 93beffe52e
commit 6ee20dca0f
Signed by: toki
GPG key ID: 6CB106C25E86A9F7
4 changed files with 89 additions and 6 deletions

View file

@ -7,53 +7,93 @@ import public Kinematics.Joint
%default total %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 public export
ArmElement : Nat -> Type ArmElement : (n : Nat) -> Type
ArmElement n = List (Either (Link n) (Joint n)) ArmElement n = List (Either (Link n) (Joint n))
||| Count the number of joints in a robot arm.
public export public export
countJoints : ArmElement n -> Nat countJoints : ArmElement n -> Nat
countJoints [] = 0 countJoints [] = 0
countJoints (Left _ :: xs) = countJoints xs countJoints (Left _ :: xs) = countJoints xs
countJoints (Right _ :: xs) = S $ 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 public export
ArmConfig : ArmElement n -> Type ArmConfig : ArmElement n -> Type
ArmConfig arm = Vector (countJoints arm) Double ArmConfig arm = Vector (countJoints arm) Double
||| Get a list of the limits of each joint, in order.
export export
getLimits : (arm : ArmElement n) -> Vect (countJoints arm) (Double, Double) getLimits : (arm : ArmElement n) -> Vect (countJoints arm) (Double, Double)
getLimits [] = [] getLimits [] = []
getLimits (Left _ :: xs) = getLimits xs getLimits (Left _ :: xs) = getLimits xs
getLimits (Right (MkJoint _ l u) :: xs) = (l,u) :: 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 export
link : Vector n Double -> ArmElement n link : Vector n Double -> ArmElement n
link v = [Left $ cast (translate v)] 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 export
linkX : {n : _} -> Double -> ArmElement (1 + n) linkX : {n : _} -> Double -> ArmElement (1 + n)
linkX x = link $ vector (x :: replicate n 0) 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 export
revolute2D : (l, u : Double) -> ArmElement 2 revolute2D : (l, u : Double) -> ArmElement 2
revolute2D l u = [Right $ MkJoint Revolute l u] 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 export
revoluteX : (l, u : Double) -> ArmElement 3 revoluteX : (l, u : Double) -> ArmElement 3
revoluteX l u = [Left $ cast (Rotation.rotate3DY (pi/2)), revoluteX l u = [Left $ cast (Rotation.rotate3DY (pi/2)),
Right $ MkJoint Revolute l u, Right $ MkJoint Revolute l u,
Left $ cast (Rotation.rotate3DY (-pi/2))] 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 export
revoluteY : (l, u : Double) -> ArmElement 3 revoluteY : (l, u : Double) -> ArmElement 3
revoluteY l u = [Left $ cast (Rotation.rotate3DX (-pi/2)), revoluteY l u = [Left $ cast (Rotation.rotate3DX (-pi/2)),
Right $ MkJoint Revolute l u, Right $ MkJoint Revolute l u,
Left $ cast (Rotation.rotate3DX (pi/2))] 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 export
revoluteZ : (l, u : Double) -> ArmElement 3 revoluteZ : (l, u : Double) -> ArmElement 3
revoluteZ l u = [Right $ MkJoint Revolute l u] revoluteZ l u = [Right $ MkJoint Revolute l u]

View file

@ -7,6 +7,8 @@ import public Kinematics.Arm
%default total %default total
||| Calculate the homogeneous matrix representing the end affector's position,
||| given an arm and a vector of input joint values.
export export
forwardTransform : {n : _} -> (arm : ArmElement n) -> ArmConfig arm forwardTransform : {n : _} -> (arm : ArmElement n) -> ArmConfig arm
-> Maybe (Rigid n Double) -> 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 |] 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 export
forward : {n : _} -> (arm : ArmElement n) -> ArmConfig arm -> forward : {n : _} -> (arm : ArmElement n) -> ArmConfig arm ->
Maybe (Point n Double) Maybe (Point n Double)

View file

@ -25,10 +25,19 @@ initialSimplex arm =
Fin.range 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 export
inverse : {n : _} -> (fuel : Fuel) -> (arm : ArmElement n) -> Point n Double 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 where
sndLast : forall n. {auto 0 ok : IsSucc n} -> Vect (S n) a -> a sndLast : forall n. {auto 0 ok : IsSucc n} -> Vect (S n) a -> a
sndLast {n=S n,ok=ItIsSucc} v = last $ init v 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 : Simplex arm -> Simplex arm
sort s = believe_me $ Vect.fromList $ sortBy (compare `on` cost) $ toList s 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 Dry _ = Nothing
go (More fuel) simplex = do go (More fuel) simplex = do
guard (all (and . zipWith (\(a,b),x => a <= x && x <= b) 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) let simplex = unsafePerformIO (let s = sort simplex in printLn s $> s)
best = head simplex best = head simplex
cbest = !(cost best) cbest = !(cost best)
in if cbest < 0.00001 then Just best in if cbest < 0.0000001 then Just best
else let else let
worst = last simplex worst = last simplex
cworst = !(cost worst) cworst = !(cost worst)

View file

@ -6,11 +6,37 @@ import Data.NumIdr
%default total %default total
||| The type of a joint, one of `Revolute` or `Prismatic`.
|||
||| @ n The number of dimensions this joint operates in
public export 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) 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) 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 public export
record Joint n where record Joint n where
constructor MkJoint constructor MkJoint
@ -18,6 +44,9 @@ record Joint n where
l, u : Double 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 export
jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double) jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double)
jointAction {n=S n} (MkJoint Prismatic l u) x = jointAction {n=S n} (MkJoint Prismatic l u) x =