diff --git a/Kinematics/Arm.idr b/Kinematics/Arm.idr index 7bbed04..6dd50b4 100644 --- a/Kinematics/Arm.idr +++ b/Kinematics/Arm.idr @@ -27,8 +27,7 @@ export getLimits : (arm : ArmElement n) -> Vect (countJoints arm) (Double, Double) getLimits [] = [] getLimits (Left _ :: xs) = getLimits xs -getLimits (Right (Revolute a b) :: xs) = (a,b) :: getLimits xs -getLimits (Right (Prismatic a b) :: xs) = (a,b) :: getLimits xs +getLimits (Right (MkJoint _ l u) :: xs) = (l,u) :: getLimits xs export link : Vector n Double -> ArmElement n @@ -38,24 +37,23 @@ export linkX : {n : _} -> Double -> ArmElement (1 + n) linkX x = link $ vector (x :: replicate n 0) -export -revolute2D : (a, b : Double) -> ArmElement 2 -revolute2D a b = [Right $ Revolute a b] export -revoluteX : (a, b : Double) -> ArmElement 3 -revoluteX a b = [Left $ cast (Rotation.rotate3DY (pi/2)), - Right $ Revolute a b, +revolute2D : (l, u : Double) -> ArmElement 2 +revolute2D l u = [Right $ MkJoint Revolute l u] + +export +revoluteX : (l, u : Double) -> ArmElement 3 +revoluteX l u = [Left $ cast (Rotation.rotate3DY (pi/2)), + Right $ MkJoint Revolute l u, Left $ cast (Rotation.rotate3DY (-pi/2))] export -revoluteY : (a, b : Double) -> ArmElement 3 -revoluteY a b = [Left $ cast (Rotation.rotate3DX (-pi/2)), - Right $ Revolute a b, +revoluteY : (l, u : Double) -> ArmElement 3 +revoluteY l u = [Left $ cast (Rotation.rotate3DX (-pi/2)), + Right $ MkJoint Revolute l u, Left $ cast (Rotation.rotate3DX (pi/2))] export -revoluteZ : (a, b : Double) -> ArmElement 3 -revoluteZ a b = [Right $ Revolute a b] - - +revoluteZ : (l, u : Double) -> ArmElement 3 +revoluteZ l u = [Right $ MkJoint Revolute l u] diff --git a/Kinematics/Joint.idr b/Kinematics/Joint.idr index 6b7b4f5..7a75d6a 100644 --- a/Kinematics/Joint.idr +++ b/Kinematics/Joint.idr @@ -7,20 +7,28 @@ import Data.NumIdr public export -data Joint : Nat -> Type where - Revolute : (a, b : Double) -> Joint (2 + n) - Prismatic : (a, b : Double) -> Joint (1 + n) +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} (Prismatic a b) x = - guard (a < x && x < b) +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)} (Revolute a b) x = - guard (a < x && x < b) +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