41 lines
1.1 KiB
Idris
41 lines
1.1 KiB
Idris
module Kinematics.Joint
|
|
|
|
import Data.Vect
|
|
import Data.NumIdr
|
|
|
|
%default total
|
|
|
|
|
|
public export
|
|
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} (MkJoint Prismatic l u) x =
|
|
guard (l <= x && x <= u)
|
|
$> cast (translate $ vector $ x :: replicate n 0)
|
|
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
|
|
-- 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
|