Factor out joint limits
This commit is contained in:
parent
b88aae58f9
commit
0c59f16920
|
@ -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]
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in a new issue