Commits

bergsoe committed 5d96fc4

Functor for configuration operations for robots with circular joints.

Comments (0)

Files changed (2)

 
 (* 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))
+type my_t = t
+
+module MakeCircular (Setup : sig val setup : int * int list end) = struct
+
+  let n, circular = Setup.setup
+
+  module SO = PaplTransform.SO2
+
+  let angle_dist a b = SO.dist (SO.rotate a) (SO.rotate b)
+
+  let angle_interpolate a b =
+    let ip = SO.interpolate (SO.rotate a) (SO.rotate b) in
+      fun s -> SO.angle (ip s)
+
+  let interpolate_array =
+    let buf = Array.make n PaplInterpolate.Float.interpolate in
+    let () = List.iter
+      (fun i -> buf.(i) <- angle_interpolate)
+      circular
+    in
+      buf
+
+  let dist_array =
+    let buf = Array.make n (fun a b -> a -. b) in
+    let () = List.iter
+      (fun i -> buf.(i) <- angle_dist)
       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
+    in
+      buf
+
+  let dist i xs ys = dist_array.(i) xs.(i) ys.(i)
+
+  let interpolate a b =
+    (* 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. *)
+    let ips = Array.mapi
+      (fun i interpolate -> interpolate a.(i) b.(i))
+      interpolate_array
+    in
+      fun s -> Array.map (fun ip -> ip s) ips
+
+  module MetricOp = struct
+    type t = my_t
+    type weight_t = t
+
+    let dim_weight _ = n
+
+    let map_weight = Array.map
+
+    let combine1 f g xs =
+      let v i = f xs.(i) in
+      let sum = ref (v 0) in
+        for i = 1 to n - 1 do
+          sum := g !sum (v i)
+        done;
+        !sum
+
+    let combine2 f g xs ys =
+      let v i = f xs.(i) ys.(i) in
+      let sum = ref (v 0) in
+        for i = 1 to n - 1 do
+          sum := g !sum (v i)
+        done;
+        !sum
+
+    let combine_value = combine1
+
+    let combine_weight_x_value inner outer weight xs =
+      combine2 (fun w x -> inner (w *. x)) outer weight xs
+
+    let combine_diff inner outer xs ys =
+      let v i = inner (dist i xs ys) in
+      let sum = ref (v 0) in
+        for i = 1 to n - 1 do
+          sum := outer !sum (v i)
+        done;
+        !sum
+
+    let combine_weight_x_diff inner outer ws xs ys =
+      let v i = inner (ws.(i) *. dist i xs ys) in
+      let sum = ref (v 0) in
+        for i = 1 to n - 1 do
+          sum := outer !sum (v i)
+        done;
+        !sum
+  end
+
+  include PaplVectorMetric.Make (MetricOp)
+end
    [gram_schmidt n make] is equivalent to [gram_schmidt_by dot n make].
 *)
 
-(** {2 Interpolation for circular joints} *)
+(** {2 Circular joints} *)
 
-val interpolate_circular : int -> int list -> t PaplInterpolate.t
-(** Interpolation function for robots with circular joints.
+(** Configuration operations 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.
+    A circular joint is a revolute joint without any joint limits.
+    Configurations for robots with one or more of such joints need interpolation
+    and metric functions different from the standard implementation of [PaplQ].
 
-    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 specification for a robot with circular joints is a tuple [(n,
+    circular)] where [n] is the length of the configurations and [circular] are
+    the indices of the joints that are circular. The tuple [(7, [0, 6])] for
+    example specifies that the configurations are 7 elements long and that the
+    end-effector (joint 6) and the base joint (joint 0) are circular. The module
+    of configuration operations is constructed by [MakeCircular (struct let
+    setup = (n, circular) end)].
+*)
+module MakeCircular (M : sig val setup : int * int list end) : sig
+(** Metric functions.
 
-    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.
+    The metric always chooses the shortest route between a pair of angles when
+    measuring the distance between a pair of configurations.
 *)
+  include PaplVectorMetric.S with type t = t
+
+  val interpolate : t PaplInterpolate.t
+(** Interpolation function. Interpolation for a circular joint always chooses
+    the shortest route between the joint values. *)
+end
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.