diff --git a/src/Kinematics/Arm.idr b/src/Kinematics/Arm.idr index 6dd50b4..38ed8f4 100644 --- a/src/Kinematics/Arm.idr +++ b/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] diff --git a/src/Kinematics/Forward.idr b/src/Kinematics/Forward.idr index 3e715ed..f18b2a6 100644 --- a/src/Kinematics/Forward.idr +++ b/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) diff --git a/src/Kinematics/Inverse.idr b/src/Kinematics/Inverse.idr index fbbc1e2..b3da2dd 100644 --- a/src/Kinematics/Inverse.idr +++ b/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) -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) diff --git a/src/Kinematics/Joint.idr b/src/Kinematics/Joint.idr index 7a75d6a..d177c49 100644 --- a/src/Kinematics/Joint.idr +++ b/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 =