From 5d39fc65b62b69d741dad1053f08f96e4d28d8a9 Mon Sep 17 00:00:00 2001
From: Anders Lau Olsen
Date: Sun, 27 May 2012 07:44:49 +0200
Subject: [PATCH] Interpolation for robot configurations with circular joints.
---
src/PaplQ.ml | 23 +++++++++++++++++++++++
src/PaplQ.mli | 18 ++++++++++++++++++
2 files changed, 41 insertions(+)
diff --git a/src/PaplQ.ml b/src/PaplQ.ml
index d8ab743..c9f397d 100644
--- a/src/PaplQ.ml
+++ b/src/PaplQ.ml
@@ -91,3 +91,26 @@ let gram_schmidt_by dot n make =
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
diff --git a/src/PaplQ.mli b/src/PaplQ.mli
index 7254fa4..c840d84 100644
--- a/src/PaplQ.mli
+++ b/src/PaplQ.mli
@@ -113,3 +113,21 @@ val gram_schmidt : int -> (unit -> t) -> t list
[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.
+*)
--
2.1.1