27 lines
743 B
Idris
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
|