idris2-robotlib/Kinematics/Forward.idr

27 lines
743 B
Idris

module Kinematics.Forward
import Data.Vect
import Data.NumIdr
import public Kinematics.Arm
%default total
export
forwardTransform : {n : _} -> (arm : ArmElement n) -> ArmConfig arm
-> Maybe (Rigid n Double)
forwardTransform arm = go arm . toVect
where
go : (arm : ArmElement n) -> Vect (countJoints arm) Double
-> Maybe (Rigid n Double)
go [] _ = Just identity
go (Left l :: xs) cs = map (l *.) (go xs cs)
go (Right j :: xs) (c :: cs) = [| jointAction j c *. go xs cs |]
export
forward : {n : _} -> (arm : ArmElement n) -> ArmConfig arm ->
Maybe (Point n Double)
forward arm cs = map (fromVector . getTranslationVector . getHMatrix)
$ forwardTransform arm cs