Commits

bergsoe committed 49edc8d

- New to_state_path utility.

- New get_end and get_world functions.

- New Metric submodule.

Comments (0)

Files changed (5)

c_robwork/papl_robwork.cpp

     writeQ(vals, device->device->getBounds().second);
 }
 
+RW_Frame* robwork_getEnd(const RW_Device* device)
+{
+    RW_ASSERT(device);
+
+    return new RW_Frame(device->device->getEnd());
+}
+
 /*----------------------------------------------------------------------*
   State operations
  *----------------------------------------------------------------------*/

c_robwork/papl_robwork.h

 extern void robwork_getUpper(const RW_Device* device, double* q);
 extern void robwork_getVelocityLimits(
     const RW_Device* device, double* q);
+extern RW_Frame* robwork_getEnd(const RW_Device* device);
 
 /*----------------------------------------------------------------------*
   State operations

src/PaplRobwork.ml

   = "caml_robwork_getFrame"
 external caml_robwork_getDOF : device_t -> int
   = "caml_robwork_getDOF"
+external caml_robwork_getEnd : device_t -> frame_t
+  = "caml_robwork_getEnd"
 external caml_robwork_getLower : device_t -> q_t -> unit
   = "caml_robwork_getLower"
 external caml_robwork_getUpper : device_t -> q_t -> unit
     then frame
     else die ("PaplRobwork.get_frame: No frame named " ^ name)
 
+let get_world_frame workcell = get_frame workcell "WORLD"
+
 let get_dof device = caml_robwork_getDOF device
 
+let get_end device = caml_robwork_getEnd device
+
 let zero_q device = Array.make (get_dof device) 0.
 
 let get_q_by get device =
     (fun qt -> PaplTime.map_q (fun q -> update_q state device q) qt)
     qts
 
+let to_state_path state device qs =
+  List.map (fun q -> update_q state device q) qs
+
 let set_device_trajectory_qs state device_traj_pairs t =
   List.iter
     (fun (device, trajectory) ->
     let constr = state_in_collision workcell in
       q_time state constr obstacles device
 end
+
+module Metric = struct
+  let q_time_dist device =
+    PaplQ.dist_inf_size_normalized (get_velocity_limits device)
+
+  let state_time_dist device =
+    let dist = q_time_dist device in
+      fun sa sb ->
+        let qa = get_q sa device in
+        let qb = get_q sb device in
+          dist qa qb
+end

src/PaplRobwork.mli

     An exception is thrown if a frame with this name doesn't exist.
 *)
 
+val get_world_frame : workcell_t -> frame_t
+(** The world frame of the workcell. *)
+
 (** {3 Devices and states} *)
 
 val get_dof : device_t -> int
     The {i dof} of a device is equal to the length of its configurations.
 *)
 
+val get_end : device_t -> frame_t
+(** The end-effector of the device. *)
+
 val get_bounds : device_t -> q_t * q_t
 (** The configuration space box for the device.
 *)
 *)
 end
 
+(** {2 Metrics} *)
+
+module Metric : sig
+  val q_time_dist : device_t -> q_t PaplMetric.t
+(** Time distance metric for the configurations of a device.
+
+    The metric [dist = q_time_dist device] is a scaled infinity norm. The metric
+    assumes that the joints of [device] are capable of moving with the maximum
+    velocities given by {! get_velocity_limits}.
+*)
+
+  val state_time_dist : device_t -> state_t PaplMetric.t
+(** Time distance metric for the configurations of a device.
+
+    The device configurations are extracted from states of the workcell.
+
+    The time distance is computed as explained for {! q_time_dist}.
+*)
+end
+
 (** {2 Utilities} *)
 
+val to_state_path :
+  state_t -> device_t -> q_t list -> state_t list
+(** [to_state_path state device qs] converts a path of configurations [qs] for
+    the device [device] and the state [state] to the corresponding path of
+    states.
+
+    The function does not modify the [state] parameter.
+*)
+
 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_getEnd(value device_value)
+{
+    CAMLparam1(device_value);
+    const RW_Device* device = Device_val(device_value);
+
+    value frame = alloc_Frame();
+    Frame_val(frame) = robwork_getEnd(device);
+    CAMLreturn(frame);
+}
+
 /*----------------------------------------------------------------------*
   State operations
  *----------------------------------------------------------------------*/
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.