toki/idris2-robotlib
1
0
Fork 0
Code Issues Pull requests Activity

Factor out joint limits

Browse source
This commit is contained in:
Kiana Sheibani 2022-12-11 18:09:58 -05:00
parent b88aae58f9
commit 0c59f16920
Signed by: toki
GPG key ID: 6CB106C25E86A9F7
2 changed files with 28 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

28
Kinematics/Arm.idr
View file

@ -27,8 +27,7 @@ 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 (Revolute a b) :: xs) = (a,b) :: getLimits xs getLimits (Right (MkJoint _ l u) :: xs) = (l,u) :: getLimits xs
getLimits (Right (Prismatic a b) :: xs) = (a,b) :: getLimits xs
export export
link : Vector n Double -> ArmElement n link : Vector n Double -> ArmElement n
@ -38,24 +37,23 @@ 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)
export
revolute2D : (a, b : Double) -> ArmElement 2
revolute2D a b = [Right $ Revolute a b]
export export
revoluteX : (a, b : Double) -> ArmElement 3 revolute2D : (l, u : Double) -> ArmElement 2
revoluteX a b = [Left $ cast (Rotation.rotate3DY (pi/2)), revolute2D l u = [Right $ MkJoint Revolute l u]
Right $ Revolute a b,
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))] Left $ cast (Rotation.rotate3DY (-pi/2))]
export export
revoluteY : (a, b : Double) -> ArmElement 3 revoluteY : (l, u : Double) -> ArmElement 3
revoluteY a b = [Left $ cast (Rotation.rotate3DX (-pi/2)), revoluteY l u = [Left $ cast (Rotation.rotate3DX (-pi/2)),
Right $ Revolute a b, Right $ MkJoint Revolute l u,
Left $ cast (Rotation.rotate3DX (pi/2))] Left $ cast (Rotation.rotate3DX (pi/2))]
export export
revoluteZ : (a, b : Double) -> ArmElement 3 revoluteZ : (l, u : Double) -> ArmElement 3
revoluteZ a b = [Right $ Revolute a b] revoluteZ l u = [Right $ MkJoint Revolute l u]

22
Kinematics/Joint.idr
View file

@ -7,20 +7,28 @@ import Data.NumIdr
public export public export
data Joint : Nat -> Type where data JointType : Nat -> Type where
Revolute : (a, b : Double) -> Joint (2 + n) Revolute : JointType (2 + n)
Prismatic : (a, b : Double) -> Joint (1 + n) Prismatic : JointType (1 + n)
public export
record Joint n where
constructor MkJoint
ty : JointType n
l, u : Double
export export
jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double) jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double)
jointAction {n=S n} (Prismatic a b) x = jointAction {n=S n} (MkJoint Prismatic l u) x =
guard (a < x && x < b) guard (l <= x && x <= u)
$> cast (translate $ vector $ x :: replicate n 0) $> cast (translate $ vector $ x :: replicate n 0)
jointAction {n=S (S n)} (Revolute a b) x = jointAction {n=S (S n)} (MkJoint Revolute l u) x =
guard (a < x && x < b) guard (l <= x && x <= u)
$> unsafeMkTrans (indexSetRange [EndBound 2,EndBound 2] $> unsafeMkTrans (indexSetRange [EndBound 2,EndBound 2]
(rewrite rangeLenZ 2 in rotate2D x) identity) (rewrite rangeLenZ 2 in rotate2D x) identity)
-- Idris isn't smart enough to recognize the above two clauses cover all inputs
jointAction _ _ = Nothing
-- Links are directly represented by rigid transformations, i.e. rotations -- Links are directly represented by rigid transformations, i.e. rotations