[Krobot-resources] [SCM] krobot-resources branch, master, updated. fed6bc0cba773c21980fa79904a344fb521b2879

[Pierre C. <Sup...@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-resources".

The branch, master has been updated
       via  fed6bc0cba773c21980fa79904a344fb521b2879 (commit)
      from  0d945464874b53fdc04d37e7e07ae8cf8c66f5bf (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 fed6bc0cba773c21980fa79904a344fb521b2879
Author: chambart <cha...@cr...>
Date:   Sun Apr 3 18:58:16 2011 +0200

    [bezier] create the directory, create some code to do computation for the robot

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

Changes:
diff --git a/bezier/compute_optimal_bezier.ml b/bezier/compute_optimal_bezier.ml
new file mode 100644
index 0000000..0650da1
--- /dev/null
+++ b/bezier/compute_optimal_bezier.ml
@@ -0,0 +1,177 @@
+let ( +| ) (x1,y1) (x2,y2) = x1 +. x2, y1 +. y2
+let ( -| ) (x1,y1) (x2,y2) = x1 -. x2, y1 -. y2
+let norm (x,y) = sqrt (x**2. +. y**2.)
+let normalize (x,y) =
+  let n = sqrt (x**2. +. y**2.) in
+  (x /. n, y /. n)
+let prod_vect (x1,y1) (x2,y2) = (x1 *. x2) +. (y1 *. y2)
+let prod_scal r (x1,y1) = (r *. x1), (r *. y1)
+let rotation_trigo_pi_over_2 (x,y) = (-.y,x)
+
+type bezier_f =
+    { a0 : float; a1 : float; a2 : float; a3 : float }
+
+type bezier_2d =
+    { cx : bezier_f; cy : bezier_f }
+
+type robot =
+    { r_width : float; (* distance between wheels: m *)
+      r_max_wheel_speed : float; (* m / s *)
+      r_max_a : float; } (* m / s^2 *)
+
+let epsilon = 0.00001
+
+let check_u u =
+  if ( u <= 0. -. epsilon && u >= 1. +. epsilon )
+  then failwith (Printf.sprintf "cacaaa: %f" u)
+  else ()
+
+let compute ~u b =
+  check_u u;
+  b.a0 *. ( ( 1. -. u ) ** 3. ) +. 3.*.b.a1*.u*.((1.-.u) ** 2.) +. 3. *. b.a2*.(1.-.u)*.( (u)**2. ) +. b.a3 *. ( (u) ** 3. )
+
+let df ~u b =
+  check_u u;
+  ( 3. *. b.a3 -. 9. *. b.a2 +. 9. *. b.a1 -. 3. *. b.a0 ) *. ( u ** 2. )
+  +. ( 6. *. b.a2 -. 12. *. b.a1 +. 6. *. b.a0 ) *. u +. 3. *. b.a1 -. 3. *. b.a0
+
+let ddf ~u b =
+  check_u u;
+  ( 6. *. b.a3 -. 18. *. b.a2 +. 18. *. b.a1 -. 6. *. b.a0 ) *. u
+  +. 6. *. b.a2 -. 12. *. b.a1 +. 6. *. b.a0
+
+let point ~u b =
+  compute ~u b.cx , compute ~u b.cy
+
+let dp ~u b =
+  df ~u b.cx, df ~u b.cy
+
+let ddp ~u b =
+  ddf ~u b.cx, ddf ~u b.cy
+
+let integrate n f ui uf =
+  let du = ( uf -. ui ) /. (float n) in
+  let acc = ref 0. in
+  for i = 0 to (n-1) do
+    acc := !acc +. (f ~du ~u:( ui +. ( (float i) *. du ) ) ) *. du ;
+  done;
+  !acc
+
+let wheel_rapport r ~u b =
+  let s' = norm (dp u b) in
+  let y' = df u b.cy in
+  let x' = df u b.cx in
+  let y'' = ddf u b.cy in
+  let x'' = ddf u b.cx in
+  let theta' = ( y'' *. x' -. x'' *. y' ) /. ( x' *. x' +. y' *. y' ) in
+  let rapport = ( r.r_width *. theta' *. 0.5 +. s' )
+    /. ( -. r.r_width *. theta' *. 0.5 +. s' ) in
+  rapport
+
+(* [v']: previous speed *)
+(*
+  (* probleme quand v' = 0 *)
+  let previous_speed = prod_scal v' (normalize (dp ~u b)) in
+  let dt' = du /. (fst previous_speed) in
+  let max_reachable_speed = v' +. r.r_max_a *. dt' in
+*)
+
+let max_wheel_speed ~u r b =
+  let rapport = wheel_rapport r ~u b in
+  let rapport' = abs_float rapport in
+  let rapport = ( min rapport' (1. /. rapport') )
+    *. (if rapport >= 0. then 1. else -.1.) in
+  ( 1. +. rapport ) *. 0.5 *. r.r_max_wheel_speed
+
+let trajectory n r b =
+  let du = 1. /. (float n) in
+  let rec aux i =
+    if i > n
+    then []
+    else
+      let u = du *. (float i) in
+      let v = max_wheel_speed ~u r b in
+      let p = point ~u b in
+      (p,v)::(aux (i+1))
+  in
+  aux 0
+
+let pi = 4. *. atan 1.
+
+let bezier_x ~a0 ~a3 ~theta1 ~theta2 ~d1 ~d2 =
+  { a0 = a0;
+    a1 = a0 +. d1 *. ( cos theta1 );
+    a2 = a3 +. d2 *. ( cos ( pi +. theta2 ));
+    a3 = a3 }
+
+let bezier_y ~b0 ~b3 ~theta1 ~theta2 ~d1 ~d2 =
+  { a0 = b0;
+    a1 = b0 +. d1 *. ( sin theta1 );
+    a2 = b3 +. d2 *. ( sin ( pi +. theta2 ));
+    a3 = b3 }
+
+let bezier_traj n r (a0,b0) (a3,b3) ~theta1 ~theta2 ~d1 ~d2 =
+  let b =
+    { cx = bezier_x ~a0 ~a3 ~theta1 ~theta2 ~d1 ~d2;
+      cy = bezier_y ~b0 ~b3 ~theta1 ~theta2 ~d1 ~d2 } in
+  trajectory n r b
+
+(*** TEST ***)
+
+let robot =
+  { r_width = 0.3;
+    r_max_wheel_speed = 1.;
+    r_max_a = 0.1; }
+
+let a0,b0,a3,b3 = 0.,0.,2.,0.
+let d1,d2 = 0.5,0.5
+let theta1,theta2 = 0.,0.
+let theta1,theta2 = pi/.4.,(-.(pi/.4.))
+
+let b =
+    { cx = bezier_x ~a0 ~a3 ~theta1 ~theta2 ~d1 ~d2;
+      cy = bezier_y ~b0 ~b3 ~theta1 ~theta2 ~d1 ~d2 }
+
+let _ = wheel_rapport robot ~u:1. b
+
+let test =
+  bezier_traj 10 robot (0.,0.) (2.,0.) ~theta1:(pi/.4.) ~theta2:(-.pi/.4.) ~d1:0.5 ~d2:0.5
+(*(
+let limit_acceleration v_init du r l =
+  let l =
+    match l with
+      | [] -> failure "empty list"
+      | (p,_)::q -> q
+  in
+  let limit (p_prev,v_prev,l) (p,v_p) =
+    (* TODO -> limitation en fonction de l'acceleration *)
+    
+    let v_max = min (dt *. r.r_max_a +. v_prev) (v_p) in
+    (p,v_max, (p,v_max)::l )
+  in
+  List.fold_left ...
+*)
+
+(*
+let test =
+  bezier_traj 10 robot (0.,0.) (1.,0.) ~v_init:0. ~theta1:(pi/.4.) ~theta2:(-.pi/.4.) ~d1:1. ~d2:1.
+
+let dist ~du ~u b =
+  let p1 = point u b in
+  let p2 = point (u+.du) b in
+  norm (p2 -| p1)
+
+let time n b r =
+  integrate n (fun ~du ~u ->
+    min_time ~du ~u r b ) ) 0. 1.
+
+let bezier_time n r (a0,b0) (a3,b3) ~theta1 ~theta2 ~d1 ~d2 =
+  let b =
+    { cx = bezier_x ~a0 ~a3 ~theta1 ~theta2 ~d1 ~d2;
+      cy = bezier_y ~b0 ~b3 ~theta1 ~theta2 ~d1 ~d2 } in
+  time n b r
+
+
+
+
+*)


hooks/post-receive
-- 
krobot-resources