[Krobot-commits] [SCM] krobot branch, master, updated. 9e9d1491f3d038ec47349e046c9234fedde6ac64

[Xavier L. <Ba...@us...>]

This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".

The branch, master has been updated
       via  9e9d1491f3d038ec47349e046c9234fedde6ac64 (commit)
       via  405ee0953efe7043668a7173e869d6f71963e12d (commit)
      from  1380caeb212200f4423cd6ce13dbfa7ed9b6c8fa (commit)

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- Log -----------------------------------------------------------------
commit 9e9d1491f3d038ec47349e046c9234fedde6ac64
Author: Xavier Lagorce <Xav...@cr...>
Date:   Sun May 5 23:12:47 2013 +0200

    [krobot-simulator] Added velocity profiles along Bezier's curves

commit 405ee0953efe7043668a7173e869d6f71963e12d
Author: Xavier Lagorce <Xav...@cr...>
Date:   Sun May 5 23:12:11 2013 +0200

    [Krobot_geom] Added computation of velocity profile along Bezier curves

-----------------------------------------------------------------------

Changes:
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index cb4f683..ddcd72e 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -223,11 +223,67 @@ module Bezier = struct
   let ddt b t =
     (b.a *| (6. *. t)) +| (b.b *| 2.)
 
+  let cr b t =
+    let db = dt b t in
+    let ddb = ddt b t in
+    ((db.vx*.db.vx +. db.vy*.db.vy) ** 1.5) /. (db.vx*.ddb.vy -. db.vy*.ddb.vx)
+
   type robot_info =
     { r_width : float; (* distance between wheels: m *)
       r_max_wheel_speed : float; (* m / s *)
       r_max_a : float; } (* m / s^2 *)
 
+  exception Exit_for_vel
+
+  let velocity_profile b v_max omega_max at_max ar_max v_ini v_end du =
+    Printf.printf "inside velocity_profile\n%!";
+    let n_pts = int_of_float ( 1. /. du +. 1.) in
+    let us = Array.init n_pts (fun i -> (float_of_int i) *. du) in
+    let v_tab = Array.map (fun _ -> v_max) us in
+    let mins = ref [(0,v_ini); ((n_pts-1),v_end)] in
+    let cr_pp = ref (abs_float (cr b 0.)) in
+    let cr_p = ref (abs_float (cr b du)) in
+    Array.iteri (fun i u -> if (i > 1) then begin
+      let cr = abs_float (cr b u) in
+      if (cr >= !cr_p && !cr_p < !cr_pp) then begin
+        mins := (i, sqrt (ar_max*.cr)) :: !mins;
+      end;
+      cr_pp := !cr_p;
+      cr_p := cr;
+      end
+    ) us;
+    mins := List.rev !mins;
+    List.iter (fun (m, vm) ->
+      if (vm < v_tab.(m)) then begin
+        v_tab.(m) <- vm;
+        (try
+           for i = m-1 downto 0 do
+             let db = dt b (us.(i+1)) in
+             let dsu = sqrt (db.vx*.db.vx +. db.vy*.db.vy) in
+             let dt = (-.v_tab.(i+1)+.sqrt(v_tab.(i+1)*.v_tab.(i+1)+.2.*.at_max*.du*.dsu))/.at_max in
+             let nv = v_tab.(i+1)+.at_max*.dt in
+             if (nv < v_tab.(i)) then
+               v_tab.(i) <- nv
+             else
+               raise Exit_for_vel
+           done
+         with Exit_for_vel -> ());
+        (try
+           for i = m+1 to (n_pts-1) do
+             let db = dt b (us.(i-1)) in
+             let dsu = sqrt (db.vx*.db.vx +. db.vy*.db.vy) in
+             let dt = (-.v_tab.(i-1)+.sqrt(v_tab.(i-1)*.v_tab.(i-1)+.2.*.at_max*.du*.dsu))/.at_max in
+             let nv = v_tab.(i-1)+.at_max*.dt in
+             if (nv < v_tab.(i)) then
+               v_tab.(i) <- nv
+             else
+               raise Exit_for_vel
+           done
+         with Exit_for_vel -> ());
+      end
+    ) !mins;
+    v_tab
+
   let wheel_speed_rapport r b t =
     let s' = norm (dt b t) in
     let { vx = x'; vy = y' } = dt b t in
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index 7e2355d..6a58739 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -113,15 +113,20 @@ module Bezier : sig
   val mul_d2 : curve -> float -> curve
 
   val dt : curve -> float -> vector
-  (** [dt curve t] is the value of the dérivée de curve en t *)
+  (** [dt curve t] is the value of the derivative of curve in t *)
   val ddt : curve -> float -> vector
-  (** [ddt curve t] is the value of the dérivée seconde de curve en t *)
+  (** [ddt curve t] is the value of the second derivative of curve in t *)
+  val cr : curve -> float -> float
+  (** [cr curve t] is the value of the curvature radius of curve in t *)
 
   type robot_info =
     { r_width : float; (* distance between wheels: m *)
       r_max_wheel_speed : float; (* m / s *)
       r_max_a : float; } (* m / s^2 *)
 
+  val velocity_profile : curve -> float -> float -> float -> float -> float -> float -> float -> float array
+  (** [velocity_profile curve v_max omega_max at_max ar_max v_ini v_end du] *)
+
   val wheel_speed_rapport : robot_info -> curve -> float -> float
   (** [wheel_speed_rapport width curve t] is the rapport between the
       speed of both wheels at the point t of the curve if the 2 wheels
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 2857a59..7d5df77 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -71,7 +71,7 @@ type command =
       (* [Turn(t_acc, velocity)] *)
   | Move of float * float
       (* [Move(t_acc, velocity)] *)
-  | Bezier_cmd of (float * float * float * float) * bool
+  | Bezier_cmd of (float array) * (float * float * float * float) * bool
       (** [Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max)] *)
 
 (* Type of simulators. *)
@@ -90,9 +90,9 @@ type simulator = {
   (* The state of the ghost. *)
   mutable bezier_u : float;
   (* position on the Bezier's curve*)
-  mutable bezier_curve : Bezier.curve option;
+  mutable bezier_curve : (Bezier.curve * float) option;
   (* Bezier's curve currently being followed if existing *)
-  mutable bezier_next : (Bezier.curve * bool) option;
+  mutable bezier_next : (Bezier.curve * float * bool) option;
   (* Next Bezier's curve to follow *)
   mutable time : float;
   (* The current time. *)
@@ -140,14 +140,23 @@ velocities:
 let velocities sim dt =
   (* Put the robot into idle if the last command is terminated. *)
   (match sim.command with
-    | Bezier_cmd (_,cur_dir) ->
+    | Bezier_cmd (_,_,cur_dir) ->
       if sim.bezier_u >= 1. then begin
+        let v_ini = match sim.bezier_curve with
+          | Some (curve, v_end) -> v_end
+          | None -> assert false
+        in
         match sim.bezier_next with
         | None ->
           sim.command <- Idle
-        | Some (curve,dir) ->
-          sim.command <- Bezier_cmd (sim.bezier_limits, cur_dir);
-          sim.bezier_curve <- Some curve;
+        | Some (curve,v_end,dir) ->
+          let v_max, omega_max, at_max, ar_max = sim.bezier_limits in
+          sim.command <-
+            Bezier_cmd (Bezier.velocity_profile
+                          curve v_max omega_max at_max ar_max v_ini v_end 0.01,
+                        sim.bezier_limits,
+                        cur_dir);
+          sim.bezier_curve <- Some (curve, v_end);
           sim.bezier_next <- None;
           sim.bezier_u <- 0.
       end
@@ -171,13 +180,22 @@ let velocities sim dt =
         (vel, 0.)
       else
         (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
-    | Bezier_cmd (limits, dir) ->
+    | Bezier_cmd (v_tab, limits, dir) ->
       (match sim.bezier_curve with
          | None ->
            sim.command <- Idle;
            (0., 0.)
-         | Some curve ->
+         | Some (curve,_) ->
            let (v_max,omega_max,_,a_r_max) = limits in
+           let ui = int_of_float (sim.bezier_u *. 100.) in
+           let v_max =
+             if ui >= (Array.length v_tab)-1 then
+               v_tab.((Array.length v_tab)-1)
+             else if ui < 0 then
+               v_tab.(0)
+             else
+               v_tab.(ui) +. (v_tab.(ui+1)-.v_tab.(ui)) *. (sim.bezier_u -. 0.01*.(float_of_int ui))/.0.01;
+           in
            let s' = norm (Bezier.dt curve sim.bezier_u) in
            let { vx = x'; vy = y' } = Bezier.dt curve sim.bezier_u in
            let { vx = x'';vy = y''} = Bezier.ddt curve sim.bezier_u in
@@ -204,7 +222,7 @@ let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
     | None ->
       {Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
       sim.state.theta
-    | Some curve ->
+    | Some (curve,_) ->
       let _,_,r,s = Bezier.pqrs curve in
       s,
       (angle (vector r s))
@@ -213,13 +231,19 @@ let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
   let q = translate p (vector_of_polar d1 theta_start) in
   let r = translate s (vector_of_polar (-.d2) theta_end) in
   let dir = d1 >= 0. in
+  let curve = Bezier.of_vertices p q r s in
   match sim.bezier_curve with
     | None ->
-      sim.command <- Bezier_cmd (sim.bezier_limits,dir);
+      let v_max, omega_max, at_max, ar_max = sim.bezier_limits in
+      sim.command <-
+        Bezier_cmd (Bezier.velocity_profile
+                      curve v_max omega_max at_max ar_max 0.01 v_end 0.01,
+                    sim.bezier_limits,
+                    dir);
       sim.bezier_u <- 0.;
-      sim.bezier_curve <- Some (Bezier.of_vertices p q r s);
+      sim.bezier_curve <- Some (curve, v_end);
     | Some _ ->
-      sim.bezier_next <- Some ((Bezier.of_vertices p q r s),dir)
+      sim.bezier_next <- Some (curve,v_end,dir)
 
 let move sim distance velocity acceleration =
   if distance <> 0. && velocity > 0. && acceleration > 0. then begin


hooks/post-receive
-- 
krobot