Commits

bergsoe committed 429bce8

New [Trajectory.of_device_trajectories].

This function maps independent device trajectories to a single
state trajectory.

Comments (0)

Files changed (3)

src/PaplRobwork.ml

   let () = set_q state device q in
     state
 
-(*
-let state_collision_constr workcell =
-  PaplConstraint.make_reject
-    (fun state -> in_collision workcell state)
-
-let q_collision_constr ?state workcell device =
-  let state =
-    match state with
-        None -> get_state workcell
-      | Some state -> copy_state state
-  in
-    PaplConstraint.make_reject
-      (fun q ->
-         let () = set_q state device q in
-           in_collision workcell state)
-*)
-
 let to_time_state_path state device qts =
   List.map
     (fun qt -> PaplTime.map_q (fun q -> update_q state device q) qt)
     qts
 
+let set_device_trajectory_qs state device_traj_pairs t =
+  List.iter
+    (fun (device, trajectory) ->
+       let q = PaplTrajectory.get trajectory t in
+         set_q state device q)
+    device_traj_pairs
+
+module Trajectory = struct
+  let of_device_trajectories state device_traj_pairs =
+    let get_bounds_by get =
+      BatList.map (fun (_, traj) -> get traj) device_traj_pairs
+    in
+    let t0 = BatList.max (get_bounds_by PaplTrajectory.t0) in
+    let t1 = BatList.min (get_bounds_by PaplTrajectory.t1) in
+      PaplTrajectory.make
+        (t0, t1)
+        (fun t ->
+           let state = copy_state state in
+           let () = set_device_trajectory_qs state device_traj_pairs t in
+             state)
+end
+
 module Constraint = struct
 
   let state_in_collision workcell =
     let reject qt =
       let open PaplTime in
       let () = set_q state device qt.q in
-      let t = qt.time in
-      let () =
-        List.iter
-          (fun (device, traj) ->
-             let q = PaplTrajectory.get traj t in
-               set_q state device q)
-          obstacles
-      in
+      let () = set_device_trajectory_qs state obstacles qt.time in
         PaplConstraint.reject constr state
     in
       PaplConstraint.make_reject reject

src/PaplRobwork.mli

     The file can be loaded into RobWorkStudio by its PlayBack plugin.
 *)
 
+(** {2 Trajectories} *)
+
+module Trajectory : sig
+  val of_device_trajectories :
+    state_t ->
+    (device_t * q_t PaplTrajectory.t) list ->
+    state_t PaplTrajectory.t
+end
+
 (** {2 Constraints} *)
 
 module Constraint : sig
     for [device] set to [q].
 *)
 
-(*
-val state_collision_constr :
-  workcell_t -> state_t PaplConstraint.t
-
-val q_collision_constr :
-  ?state:state_t ->
-  workcell_t ->
-  device_t ->
-  q_t PaplConstraint.t
-
-val q_time_constr :
-  state_t ->
-  state_t PaplConstraint.t ->
-  (device_t * q_t PaplTrajectory.t) ->
-  q_t PaplTime.t PaplConstraint.t
-
-val q_time_collision_constr :
-  ?state:state_t ->
-  workcell_t ->
-  (device_t * q_t PaplTrajectory.t) ->
-  q_t PaplTime.t PaplConstraint.t
-*)
-
 val to_time_state_path :
   state_t -> device_t -> q_t PaplTime.t list -> state_t PaplTime.t list
 (** [to_time_state_path state device qts] converts a path of time-stamped
     CAMLreturn(Val_unit);
 }
 
+value caml_robwork_getVelocityLimits(value device_value, value q_value)
+{
+    CAMLparam2(device_value, q_value);
+
+    const RW_Device* device = Device_val(device_value);
+    double* q = (double*)q_value;
+    robwork_getVelocityLimits(device, q);
+
+    CAMLreturn(Val_unit);
+}
+
 value caml_robwork_setQ(
     value state_value,
     value device_value,