idris2-robotlib/Kinematics/Joint.idr

41 lines
1.1 KiB
Idris
Raw Normal View History

2022-12-11 18:01:02 -05:00
module Kinematics.Joint
import Data.Vect
import Data.NumIdr
%default total
public export
2022-12-11 18:09:58 -05:00
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
2022-12-11 18:01:02 -05:00
export
jointAction : {n : _} -> Joint n -> Double -> Maybe (Rigid n Double)
2022-12-11 18:09:58 -05:00
jointAction {n=S n} (MkJoint Prismatic l u) x =
guard (l <= x && x <= u)
2022-12-11 18:01:02 -05:00
$> cast (translate $ vector $ x :: replicate n 0)
2022-12-11 18:09:58 -05:00
jointAction {n=S (S n)} (MkJoint Revolute l u) x =
guard (l <= x && x <= u)
2022-12-11 18:01:02 -05:00
$> unsafeMkTrans (indexSetRange [EndBound 2,EndBound 2]
(rewrite rangeLenZ 2 in rotate2D x) identity)
2022-12-11 18:09:58 -05:00
-- Idris isn't smart enough to recognize the above two clauses cover all inputs
jointAction _ _ = Nothing
2022-12-11 18:01:02 -05:00
-- 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