diff --git a/Kinematics/Arm.idr b/Kinematics/Arm.idr new file mode 100644 index 0000000..7bbed04 --- /dev/null +++ b/Kinematics/Arm.idr @@ -0,0 +1,61 @@ +module Kinematics.Arm + +import Data.Vect +import Data.NumIdr +import public Kinematics.Joint + +%default total + + +public export +ArmElement : Nat -> Type +ArmElement n = List (Either (Link n) (Joint n)) + + +public export +countJoints : ArmElement n -> Nat +countJoints [] = 0 +countJoints (Left _ :: xs) = countJoints xs +countJoints (Right _ :: xs) = S $ countJoints xs + +public export +ArmConfig : ArmElement n -> Type +ArmConfig arm = Vector (countJoints arm) Double + + +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 + +export +link : Vector n Double -> ArmElement n +link v = [Left $ cast (translate v)] + +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, + 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, + Left $ cast (Rotation.rotate3DX (pi/2))] + +export +revoluteZ : (a, b : Double) -> ArmElement 3 +revoluteZ a b = [Right $ Revolute a b] + + diff --git a/Kinematics/Forward.idr b/Kinematics/Forward.idr new file mode 100644 index 0000000..3e715ed --- /dev/null +++ b/Kinematics/Forward.idr @@ -0,0 +1,26 @@ +module Kinematics.Forward + +import Data.Vect +import Data.NumIdr +import public Kinematics.Arm + +%default total + + +export +forwardTransform : {n : _} -> (arm : ArmElement n) -> ArmConfig arm + -> Maybe (Rigid n Double) +forwardTransform arm = go arm . toVect + where + go : (arm : ArmElement n) -> Vect (countJoints arm) Double + -> Maybe (Rigid n Double) + go [] _ = Just identity + go (Left l :: xs) cs = map (l *.) (go xs cs) + go (Right j :: xs) (c :: cs) = [| jointAction j c *. go xs cs |] + + +export +forward : {n : _} -> (arm : ArmElement n) -> ArmConfig arm -> + Maybe (Point n Double) +forward arm cs = map (fromVector . getTranslationVector . getHMatrix) + $ forwardTransform arm cs diff --git a/Kinematics/Inverse.idr b/Kinematics/Inverse.idr new file mode 100644 index 0000000..fbbc1e2 --- /dev/null +++ b/Kinematics/Inverse.idr @@ -0,0 +1,70 @@ +module Kinematics.Inverse + +import Data.List +import Data.Vect +import Data.Fuel +import Data.NumIdr +import public Kinematics.Arm +import Kinematics.Forward + +%default total + + +Simplex : ArmElement n -> Type +Simplex arm = Vect (S $ countJoints arm) (ArmConfig arm) + +initialSimplex : (arm : ArmElement n) -> Maybe (Simplex arm) +initialSimplex arm = + let limits = getLimits arm + orig = vector $ map (\(a,b) => (a+b)/2) limits + variance = vector $ map (\(a,b) => a*0.4+b*0.6) limits + in guard (all (uncurry (<)) limits) + $> map (\case + FZ => orig + FS i => indexSet [i] (variance !! i) orig) + Fin.range + + +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) + where + sndLast : forall n. {auto 0 ok : IsSucc n} -> Vect (S n) a -> a + sndLast {n=S n,ok=ItIsSucc} v = last $ init v + + cost : ArmConfig arm -> Maybe Double + cost c = map (\p' => normSq $ p -. p') (forward arm c) + + -- The standard library currently doesn't have sorting for Vects, + -- so we have to improvise a bit. + 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 Dry _ = Nothing + go (More fuel) simplex = do + guard (all (and . zipWith (\(a,b),x => a <= x && x <= b) + (getLimits arm) . toVect) simplex) *> + 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 + else let + worst = last simplex + cworst = !(cost worst) + centroid = sum (init simplex) *. (recip $ cast {to=Double} $ countJoints arm) + costcen = !(cost centroid) + vertexr = centroid *. 2.0 - worst + cvr = !(cost vertexr) + in if cvr >= cbest && cvr < !(cost $ sndLast simplex) + then go fuel $ replaceAt last vertexr simplex + else if cvr < cbest + then let vertexe = centroid + 2.0 *. (vertexr - centroid) + in go fuel $ replaceAt last (if !(cost vertexe) < cvr then vertexe else vertexr) simplex + else let + comp = cvr < cworst + vertexc = centroid + 0.5 *. ((if comp then vertexr else worst) - centroid) + in if costcen < min cvr cworst + then go fuel $ replaceAt last vertexc simplex + else go fuel $ best :: map (\vrt => best + 0.5 *. (vrt - best)) (tail simplex) diff --git a/Kinematics/Joint.idr b/Kinematics/Joint.idr new file mode 100644 index 0000000..6b7b4f5 --- /dev/null +++ b/Kinematics/Joint.idr @@ -0,0 +1,32 @@ +module Kinematics.Joint + +import Data.Vect +import Data.NumIdr + +%default total + + +public export +data Joint : Nat -> Type where + Revolute : (a, b : Double) -> Joint (2 + n) + Prismatic : (a, b : Double) -> Joint (1 + n) + + +export +jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double) +jointAction {n=S n} (Prismatic a b) x = + guard (a < x && x < b) + $> cast (translate $ vector $ x :: replicate n 0) +jointAction {n=S (S n)} (Revolute a b) x = + guard (a < x && x < b) + $> unsafeMkTrans (indexSetRange [EndBound 2,EndBound 2] + (rewrite rangeLenZ 2 in rotate2D x) identity) + + +-- Links are directly represented by rigid transformations, i.e. rotations +-- composed with translations. The rotation component allows the link to modify +-- the orientation of the next joint, but reflections are disallowed to enforce +-- the right-hand rule for all joints. +public export +Link : Nat -> Type +Link n = Rigid n Double diff --git a/Main.idr b/Main.idr deleted file mode 100644 index 8984ee2..0000000 --- a/Main.idr +++ /dev/null @@ -1,156 +0,0 @@ -module Main - -import Data.List -import Data.Vect -import Data.Fuel -import Data.NumIdr - -%default total - - -public export -data Joint : Nat -> Type where - Revolute : (a, b : Double) -> Joint (2 + n) - Prismatic : (a, b : Double) -> Joint (1 + n) - - -jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double) -jointAction {n=S n} (Prismatic a b) x = - guard (a < x && x < b) - $> cast (translate $ vector $ x :: replicate n 0) -jointAction {n=S (S n)} (Revolute a b) x = - guard (a < x && x < b) - $> unsafeMkTrans (indexSetRange [EndBound 2,EndBound 2] - (rewrite rangeLenZ 2 in rotate2D x) identity) - - - --- Links are directly represented by rigid transformations, i.e. rotations --- composed with translations. The rotation component allows the link to modify --- the orientation of the next joint, but reflections are disallowed to enforce --- the right-hand rule for all joints. -Link : Nat -> Type -Link n = Rigid n Double - - -ArmElement : Nat -> Type -ArmElement n = List (Either (Link n) (Joint n)) - - -link : Vector n Double -> ArmElement n -link v = [Left $ cast (translate v)] - -linkX : {n : _} -> Double -> ArmElement (1 + n) -linkX x = link $ vector (x :: replicate n 0) - - -revolute2D : (a, b : Double) -> ArmElement 2 -revolute2D a b = [Right $ Revolute a b] - -revoluteX : (a, b : Double) -> ArmElement 3 -revoluteX a b = [Left $ cast (Rotation.rotate3DY (pi/2)), - Right $ Revolute a b, - Left $ cast (Rotation.rotate3DY (-pi/2))] - -revoluteY : (a, b : Double) -> ArmElement 3 -revoluteY a b = [Left $ cast (Rotation.rotate3DX (-pi/2)), - Right $ Revolute a b, - Left $ cast (Rotation.rotate3DX (pi/2))] - -revoluteZ : (a, b : Double) -> ArmElement 3 -revoluteZ a b = [Right $ Revolute a b] - -countJoints : ArmElement n -> Nat -countJoints [] = 0 -countJoints (Left _ :: xs) = countJoints xs -countJoints (Right _ :: xs) = S $ countJoints xs - -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 - -ArmConfig : ArmElement n -> Type -ArmConfig arm = Vector (countJoints arm) Double - - -forwardTransform : {n : _} -> (arm : ArmElement n) -> ArmConfig arm - -> Maybe (Rigid n Double) -forwardTransform arm = go arm . toVect - where - go : (arm : ArmElement n) -> Vect (countJoints arm) Double - -> Maybe (Rigid n Double) - go [] _ = Just identity - go (Left l :: xs) cs = map (l *.) (go xs cs) - go (Right j :: xs) (c :: cs) = [| jointAction j c *. go xs cs |] - - -forward : {n : _} -> (arm : ArmElement n) -> ArmConfig arm -> - Maybe (Point n Double) -forward arm cs = map (fromVector . getTranslationVector . getHMatrix) - $ forwardTransform arm cs - - -Simplex : ArmElement n -> Type -Simplex arm = Vect (S $ countJoints arm) (ArmConfig arm) - -initialSimplex : (arm : ArmElement n) -> Maybe (Simplex arm) -initialSimplex arm = - let limits = getLimits arm - orig = vector $ map (\(a,b) => (a+b)/2) limits - variance = vector $ map (\(a,b) => a*0.4+b*0.6) limits - in guard (all (uncurry (<)) limits) - $> map (\case - FZ => orig - FS i => indexSet [i] (variance !! i) orig) - Fin.range - -inverse : {n : _} -> (fuel : Fuel) -> (arm : ArmElement n) -> Point n Double - -> {auto ok : IsSucc (countJoints arm)} -> Maybe (ArmConfig arm) -inverse fuel arm p = go fuel !(initialSimplex arm) - where - sndLast : {n : _} -> {auto ok : IsSucc n} -> Vect (S n) a -> a - sndLast {n=S n,ok=ItIsSucc} v = last $ init v - - cost : ArmConfig arm -> Maybe Double - cost c = map (\p' => normSq $ p -. p') (forward arm c) - - -- The standard library currently doesn't have sorting for Vects, - -- so we have to improvise a bit. - 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 Dry _ = Nothing - go (More fuel) simplex = do - guard (all (and . zipWith (\(a,b),x => a <= x && x <= b) - (getLimits arm) . toVect) simplex) *> - 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 - else let - worst = last simplex - cworst = !(cost worst) - centroid = sum (init simplex) *. (recip $ cast {to=Double} $ countJoints arm) - costcen = !(cost centroid) - vertexr = centroid *. 2.0 - worst - cvr = !(cost vertexr) - in if cvr >= cbest && cvr < !(cost $ sndLast simplex) - then go fuel $ replaceAt last vertexr simplex - else if cvr < cbest - then let vertexe = centroid + 2.0 *. (vertexr - centroid) - in go fuel $ replaceAt last (if !(cost vertexe) < cvr then vertexe else vertexr) simplex - else let - comp = cvr < cworst - vertexc = centroid + 0.5 *. ((if comp then vertexr else worst) - centroid) - in if costcen < min cvr cworst - then go fuel $ replaceAt last vertexc simplex - else go fuel $ best :: map (\vrt => best + 0.5 *. (vrt - best)) (tail simplex) - - - -main : IO () -main = putStrLn "Hello World!" - diff --git a/robotlib.ipkg b/robotlib.ipkg index 4a3de79..89cd784 100644 --- a/robotlib.ipkg +++ b/robotlib.ipkg @@ -7,3 +7,8 @@ license = "MIT" langversion >= 0.5.1 depends = numidr >= 0.2.1 + +modules = Kinematics.Joint, + Kinematics.Arm, + Kinematics.Forward, + Kinematics.Inverse