Commits

bergsoe  committed 9bca9c2

Retrieval of workcell frames.

  • Participants
  • Parent commits dae4f3a

Comments (0)

Files changed (4)

File c_robwork/papl_robwork.cpp

     RW_Device(const Device::Ptr& device) : device(device) {}
 };
 
+struct RW_Frame {
+    Frame* frame;
+
+    RW_Frame(Frame* frame) : frame(frame) {}
+};
+
 struct RW_TimedStatePath {
     TimedStatePath path;
 };
     return workcell->workcell != NULL;
 }
 
+int robwork_isValidFrame(const RW_Frame* frame)
+{
+    return frame->frame != NULL;
+}
+
 RW_TimedStatePath* robwork_makeTimedStatePath()
 {
     return new RW_TimedStatePath();
     return new RW_Device(workcell->workcell->findDevice(name));
 }
 
+RW_Frame* robwork_getFrame(const RW_WorkCell* workcell, const char* name)
+{
+    RW_ASSERT(workcell);
+
+    return new RW_Frame(workcell->workcell->findFrame(name));
+}
+
 int robwork_getDOF(const RW_Device* device)
 {
     RW_ASSERT(device);

File c_robwork/papl_robwork.h

  *----------------------------------------------------------------------*/
 
 typedef struct RW_WorkCell RW_WorkCell;
-typedef struct RW_State RW_State;
 typedef struct RW_Device RW_Device;
+typedef struct RW_State RW_State;
+typedef struct RW_Frame RW_Frame;
 typedef struct RW_TimedStatePath RW_TimedStatePath;
 
 /*----------------------------------------------------------------------*
 extern void robwork_freeWorkCell(RW_WorkCell* workcell);
 extern void robwork_freeDevice(RW_Device* device);
 extern void robwork_freeState(RW_State* state);
+extern void robwork_freeFrame(RW_Frame* frame);
 extern void robwork_freeTimedStatePath(RW_TimedStatePath* path);
 
 /*----------------------------------------------------------------------*
 /* 1 if valid; 0 otherwise. */
 extern int robwork_isValidWorkCell(const RW_WorkCell* workcell);
 
+/* 1 if valid; 0 otherwise. */
+extern int robwork_isValidFrame(const RW_Frame* frame);
+
 /*----------------------------------------------------------------------*
   Workcell operations
  *----------------------------------------------------------------------*/
 extern RW_Device* robwork_getDevice(
     const RW_WorkCell* workcell, const char* name);
 
+extern RW_Frame* robwork_getFrame(
+    const RW_WorkCell* workcell, const char* name);
+
 /*----------------------------------------------------------------------*
   Device operations
  *----------------------------------------------------------------------*/

File src/PaplRobwork.ml

 type workcell_t
 type state_t
 type device_t
+type frame_t
 type q_t = float array
 type time_state_path_t
 
   = "caml_robwork_getWorkCellError"
 external caml_robwork_isValidDevice : device_t -> bool
   = "caml_robwork_isValidDevice"
+external caml_robwork_isValidFrame : frame_t -> bool
+  = "caml_robwork_isValidFrame"
 external caml_robwork_isValidWorkCell : workcell_t -> bool
   = "caml_robwork_isValidWorkCell"
 external caml_robwork_loadWorkCell : string -> workcell_t
   = "caml_robwork_getState"
 external caml_robwork_getDevice : workcell_t -> string -> device_t
   = "caml_robwork_getDevice"
+external caml_robwork_getFrame : workcell_t -> string -> frame_t
+  = "caml_robwork_getFrame"
 external caml_robwork_getDOF : device_t -> int
   = "caml_robwork_getDOF"
 external caml_robwork_getLower : device_t -> q_t -> unit
     then device
     else die ("PaplRobwork.get_device: No device named " ^ name)
 
+let get_frame workcell name =
+  let frame = caml_robwork_getFrame workcell name in
+    if caml_robwork_isValidFrame frame
+    then frame
+    else die ("PaplRobwork.get_frame: No frame named " ^ name)
+
 let get_dof device = caml_robwork_getDOF device
 
 let zero_q device = Array.make (get_dof device) 0.
 #define WorkCell_val(v) (*((RW_WorkCell **) Data_custom_val(v)))
 #define State_val(v) (*((RW_State **) Data_custom_val(v)))
 #define Device_val(v) (*((RW_Device **) Data_custom_val(v)))
+#define Frame_val(v) (*((RW_Frame **) Data_custom_val(v)))
 #define TimedStatePath_val(v) (*((RW_TimedStatePath **) Data_custom_val(v)))
 
 static void finalize_WorkCell(value model)
     robwork_freeDevice(Device_val(device));
 }
 
+static void finalize_Frame(value frame)
+{
+    robwork_freeFrame(Frame_val(frame));
+}
+
 static void finalize_TimedStatePath(value path)
 {
     robwork_freeTimedStatePath(TimedStatePath_val(path));
   custom_deserialize_default
 };
 
+static struct custom_operations ops_Frame = {
+  "org.bergsoe.caml_robwork_frame",
+  finalize_Frame,
+  custom_compare_default,
+  custom_hash_default,
+  custom_serialize_default,
+  custom_deserialize_default
+};
+
 static struct custom_operations ops_TimedStatePath = {
   "org.bergsoe.caml_robwork_timedstatepath",
   finalize_TimedStatePath,
 #define alloc_WorkCell() alloc_custom(&ops_WorkCell, sizeof(RW_WorkCell *), 0, 1)
 #define alloc_State() alloc_custom(&ops_State, sizeof(RW_State *), 0, 1)
 #define alloc_Device() alloc_custom(&ops_Device, sizeof(RW_Device *), 0, 1)
+#define alloc_Frame() alloc_custom(&ops_Frame, sizeof(RW_Frame *), 0, 1)
 #define alloc_TimedStatePath() alloc_custom(&ops_TimedStatePath, sizeof(RW_TimedStatePath *), 0, 1)
 
 value caml_robwork_isValidDevice(value device_value)
     CAMLreturn(Val_bool(robwork_isValidDevice(device)));
 }
 
+value caml_robwork_isValidFrame(value frame_value)
+{
+    CAMLparam1(frame_value);
+
+    const RW_Frame* frame = Frame_val(frame_value);
+
+    CAMLreturn(Val_bool(robwork_isValidFrame(frame)));
+}
+
 value caml_robwork_isValidWorkCell(value workcell_value)
 {
     CAMLparam1(workcell_value);
     CAMLreturn(device);
 }
 
+value caml_robwork_getFrame(value workcell_value, value name_value)
+{
+    CAMLparam2(workcell_value, name_value);
+
+    const RW_WorkCell* workcell = WorkCell_val(workcell_value);
+    const char* name = String_val(name_value);
+
+    value frame = alloc_Frame();
+    Frame_val(frame) = robwork_getFrame(workcell, name);
+    CAMLreturn(frame);
+}
+
 value caml_robwork_getDOF(value device_value)
 {
     CAMLparam1(device_value);