Commits

bergsoe committed afb42cc

Cleanup of the package before the first public release.

- New copy_state function.
- Replace time_states with time_state_path.
- API doc.

Comments (0)

Files changed (5)

c_robwork/papl_robwork.cpp

     writeQ(vals, q);
 }
 
+RW_State* robwork_copyState(const RW_State* state)
+{
+    return new RW_State(state->state);
+}
+
 int robwork_inCollision(const RW_WorkCell* workcell, const RW_State* state)
 {
     RW_ASSERT(state);

c_robwork/papl_robwork.h

 extern void robwork_getQ(
     const RW_State* state, const RW_Device* device, double* q);
 
+extern RW_State* robwork_copyState(const RW_State* state);
+
 /*----------------------------------------------------------------------*
   Collision detection
  *----------------------------------------------------------------------*/
   = "caml_robwork_addTimedState"
 external caml_robwork_writeTimedStatePath : workcell_t -> time_state_path_t -> string -> bool
   = "caml_robwork_writeTimedStatePath"
+external caml_robwork_copyState : state_t -> state_t
+  = "caml_robwork_copyState"
 
 exception Exception of string
 
 
 let zero_q device = Array.make (get_dof device) 0.
 
-let get_bound_by device get =
+let get_bound_by get device =
   let q = zero_q device in
   let () = get device q in
     q
 
-let get_lower device = get_bound_by device caml_robwork_getLower
-let get_upper device = get_bound_by device caml_robwork_getUpper
+let get_lower device = get_bound_by caml_robwork_getLower device
+let get_upper device = get_bound_by caml_robwork_getUpper device
 
 let get_bounds device = (get_lower device, get_upper device)
 
-let set_q state device q = caml_robwork_setQ state device q
+let set_q state device q =
+  let n = get_dof device in
+  let n' = Array.length q in
+  if n = n'
+  then caml_robwork_setQ state device q
+  else
+    let msg =
+      (Printf.sprintf
+         "PaplRobwork.set_q: device and configuration (length %d and %d) do not match."
+         n n')
+    in
+      die msg
+
 let get_q state device =
   let q = zero_q device in
   let () = caml_robwork_getQ state device q in
     q
 
+let copy_state state = caml_robwork_copyState state
+
 let in_collision workcell state =
   caml_robwork_inCollision workcell state
 
-let write_time_states workcell path file =
+let write_time_state_path workcell path file =
   let buf = caml_robwork_makeTimedStatePath () in
   let () =
     let open PaplTime in
     if ok
     then ()
     else die
-      ("PaplRobwork.write_time_state_path: Error writing path to " ^ file)
+      ("PaplRobwork.write_time_state_path: error writing path to " ^ file)
+
+let update_q state device q =
+  let state = copy_state state in
+  let () = set_q state device q in
+    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
   Copyright (c) 2012 Anders Lau Olsen.
   See LICENSE file for terms and conditions.
 *)
+(**
+   Robwork interface
+*)
+
+(** {2 Types} *)
 
 type workcell_t
 (** Workcells. *)
     The exception contains an error message.
 *)
 
+(** {2 Operations} *)
+
+(** {3 Workcells} *)
+
 val load_workcell : string -> workcell_t
 (** Load a workcell from a file.
 
 *)
 
 val get_device : workcell_t -> string -> device_t
-(** [get_device workcell name] is the device of [workcell] named [name].
+(** [get_device workcell name] is the device in [workcell] named [name].
 
-    An exception is thrown if no device of this name can be found.
+    An exception is thrown if a device with this name doesn't exist.
 *)
 
+(** {3 Devices and states} *)
+
 val get_dof : device_t -> int
 (** The number of degrees of freedom of a device.
+
+    The {i dof} of a device is equal to the length of its configurations.
 *)
 
 val get_bounds : device_t -> q_t * q_t
-(** The configuration space box of the device.
+(** The configuration space box for the device.
 *)
 
 val set_q : state_t -> device_t -> q_t -> unit
 (** Set a configuration for a device.
 
     [set_q state device q] writes the configuration [q] for [device] to [state].
+
+    If the dimensions of [device] and [q] do not match, an exception is thrown.
 *)
 
 val get_q : state_t -> device_t -> q_t
 (** The configuration of a device.
 
-    [q = get_q state device] is the configuration [q] in [state] for [device].
+    [q = get_q state device] is the configuration of [device] in the state [state].
 *)
 
+val copy_state : state_t -> state_t
+(** A copy of a state. *)
+
+(** {3 Collision detection} *)
+
 val in_collision : workcell_t -> state_t -> bool
 (** Collision detection for a workcell.
 
-    [in_collision workcell state] is [true] iff the workcell is in collision in
-    the state [state].
+    [in_collision workcell state] is [true] if the workcell is in collision in
+    state [state].
 *)
 
-val write_time_states : workcell_t -> time_state_t list -> string -> unit
+(** {3 Data export} *)
+
+val write_time_state_path : workcell_t -> time_state_t list -> string -> unit
 (** Write a path of time-stamped states to a file.
+
+    The file can be loaded into RobWorkStudio by its Playback plugin.
+*)
+
+(** {2 Utilities} *)
+
+val update_q : state_t -> device_t -> q_t -> state_t
+(** [update_q state device q] returns a copy of [state] with the configuration
+    for [device] set to [q].
+*)
+
+val q_collision_constr :
+  ?state:state_t ->
+  workcell_t ->
+  device_t ->
+  q_t PaplConstraint.t
+(** Collision detection constraint for the configurations of a device.
+
+    The constraint [q_collision_constr state workcell device] rejects
+    configurations [q] for [device] for which [workcell] is in collision when
+    the device configuration of [state] is set to [q].
+
+    The constraint does not modify the [state] parameter.
+
+    If no [state] is given then [get_state workcell] is used.
+*)
+
+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
+    configurations [qts] for the device [device] and the state [state] to the
+    corresponding path of time-stamped states.
+
+    The function does not modify the [state] parameter.
 *)
     CAMLreturn(Val_unit);
 }
 
+value caml_robwork_copyState(value state_value)
+{
+    CAMLparam1(state_value);
+    const RW_State* org_state = State_val(state_value);
+
+    value state = alloc_State();
+    State_val(state) = robwork_copyState(org_state);
+    CAMLreturn(state);
+}
+
 value caml_robwork_inCollision(value workcell_value, value state_value)
 {
     CAMLparam2(workcell_value, state_value);