idris2-robotlib/Kinematics/Arm.idr

60 lines
1.4 KiB
Idris
Raw Normal View History

2022-12-11 18:01:02 -05:00
module Kinematics.Arm
import Data.Vect
import Data.NumIdr
import public Kinematics.Joint
%default total
public export
ArmElement : Nat -> Type
ArmElement n = List (Either (Link n) (Joint n))
public export
countJoints : ArmElement n -> Nat
countJoints [] = 0
countJoints (Left _ :: xs) = countJoints xs
countJoints (Right _ :: xs) = S $ countJoints xs
public export
ArmConfig : ArmElement n -> Type
ArmConfig arm = Vector (countJoints arm) Double
export
getLimits : (arm : ArmElement n) -> Vect (countJoints arm) (Double, Double)
getLimits [] = []
getLimits (Left _ :: xs) = getLimits xs
2022-12-11 18:09:58 -05:00
getLimits (Right (MkJoint _ l u) :: xs) = (l,u) :: getLimits xs
2022-12-11 18:01:02 -05:00
export
link : Vector n Double -> ArmElement n
link v = [Left $ cast (translate v)]
export
linkX : {n : _} -> Double -> ArmElement (1 + n)
linkX x = link $ vector (x :: replicate n 0)
2022-12-11 18:09:58 -05:00
2022-12-11 18:01:02 -05:00
export
2022-12-11 18:09:58 -05:00
revolute2D : (l, u : Double) -> ArmElement 2
revolute2D l u = [Right $ MkJoint Revolute l u]
2022-12-11 18:01:02 -05:00
export
2022-12-11 18:09:58 -05:00
revoluteX : (l, u : Double) -> ArmElement 3
revoluteX l u = [Left $ cast (Rotation.rotate3DY (pi/2)),
Right $ MkJoint Revolute l u,
2022-12-11 18:01:02 -05:00
Left $ cast (Rotation.rotate3DY (-pi/2))]
export
2022-12-11 18:09:58 -05:00
revoluteY : (l, u : Double) -> ArmElement 3
revoluteY l u = [Left $ cast (Rotation.rotate3DX (-pi/2)),
Right $ MkJoint Revolute l u,
2022-12-11 18:01:02 -05:00
Left $ cast (Rotation.rotate3DX (pi/2))]
export
2022-12-11 18:09:58 -05:00
revoluteZ : (l, u : Double) -> ArmElement 3
revoluteZ l u = [Right $ MkJoint Revolute l u]