Commits

bergsoe committed 5d39fc6

Interpolation for robot configurations with circular joints.

Comments (0)

Files changed (2)

   in loop 0 []
 
 let gram_schmidt n make = gram_schmidt_by dot n make
+
+(* Circular joints *)
+
+let interpolate_circular n circular =
+  let interpolators = Array.make n PaplInterpolate.Float.interpolate in
+  let () =
+    List.iter
+      (fun i -> interpolators.(i) <-
+         fun a b ->
+           let open PaplTransform.SO2 in
+           let ip = interpolate (rotate a) (rotate b) in
+             fun s -> angle (ip s))
+      circular
+  in
+  (* As all other interpolator constructors, the function assumes that the
+     interpolation function will be called repeatedly for the same pair of start
+     and goal points [a] and [b]. *)
+    fun a b ->
+      let ips = Array.mapi
+        (fun i interpolate -> interpolate a.(i) b.(i))
+        interpolators
+      in
+        fun s -> Array.map (fun ip -> ip s) ips
 
    [gram_schmidt n make] is equivalent to [gram_schmidt_by dot n make].
 *)
+
+(** {2 Interpolation for circular joints} *)
+
+val interpolate_circular : int -> int list -> t PaplInterpolate.t
+(** Interpolation function for robots with circular joints.
+
+    A circular joint is a revolute joint without any joint limits. The
+    interpolation function for the circular joint always chooses the shortest
+    rotation from one angle to the other.
+
+    Suppose a robot has [n] joints and that joint [j0, ..., jN] are circular.
+    Then [interpolate_circular n \[j0; ...; jN\]] is the interpolation for the
+    robot.
+
+    The joint numbers [j0, ..., jN] must be in the range [\[0, n)]. The joints
+    that are not listed in [j0, ..., jN] are interpolated by the standard linear
+    interpolation for floats.
+*)