Factor out joint limits

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

View file

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

View file

@ -7,20 +7,28 @@ import Data.NumIdr
public export
data Joint : Nat -> Type where
Revolute : (a, b : Double) -> Joint (2 + n)
Prismatic : (a, b : Double) -> Joint (1 + n)
data JointType : Nat -> Type where
Revolute : JointType (2 + n)
Prismatic : JointType (1 + n)
public export
record Joint n where
constructor MkJoint
ty : JointType n
l, u : Double
export
jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double)
jointAction {n=S n} (Prismatic a b) x =
guard (a < x && x < b)
jointAction {n=S n} (MkJoint Prismatic l u) x =
guard (l <= x && x <= u)
$> cast (translate $ vector $ x :: replicate n 0)
jointAction {n=S (S n)} (Revolute a b) x =
guard (a < x && x < b)
jointAction {n=S (S n)} (MkJoint Revolute l u) x =
guard (l <= x && x <= u)
$> unsafeMkTrans (indexSetRange [EndBound 2,EndBound 2]
(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