Add inline documentation
This commit is contained in:
parent
93beffe52e
commit
6ee20dca0f
|
@ -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]
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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)
|
||||
inverse fuel arm p = go fuel !(initialSimplex arm)
|
||||
-> Maybe (ArmConfig 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)
|
||||
|
|
|
@ -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 =
|
||||
|
|
Loading…
Reference in a new issue