Commits

bergsoe committed ea92f22

Forward kinematics tables.

Comments (0)

Files changed (5)

c_robwork/papl_robwork.cpp

 */
 
 #include "papl_robwork.h"
+#include <rw/math/Transform3D.hpp>
+#include <rw/math/Rotation3D.hpp>
+#include <rw/math/Vector3D.hpp>
 #include <rw/common/macros.hpp>
 #include <rw/common/Exception.hpp>
 #include <rw/models/WorkCell.hpp>
 #include <rw/models/Device.hpp>
+#include <rw/kinematics/FKTable.hpp>
 #include <rw/kinematics/State.hpp>
 #include <rw/kinematics/Kinematics.hpp>
 #include <rw/math/Q.hpp>
 using namespace rw::trajectory;
 using namespace rwlibs::proximitystrategies;
 
+/*----------------------------------------------------------------------*
+  Data types
+ *----------------------------------------------------------------------*/
+
 struct RW_WorkCell
 {
     WorkCell::Ptr workcell;
     RW_Frame(Frame* frame) : frame(frame) {}
 };
 
+struct RW_FKTable {
+    Ptr<FKTable> table;
+
+    RW_FKTable(Ptr<FKTable> table) : table(table) {}
+};
+
 struct RW_TimedStatePath {
     TimedStatePath path;
 };
 
+/*----------------------------------------------------------------------*
+  Destructors
+ *----------------------------------------------------------------------*/
+
+void robwork_freeWorkCell(RW_WorkCell* workcell)
+{
+    delete workcell;
+}
+
+void robwork_freeState(RW_State* state)
+{
+    delete state;
+}
+
+void robwork_freeDevice(RW_Device* device)
+{
+    delete device;
+}
+
+void robwork_freeFrame(RW_Frame* frame)
+{
+    delete frame;
+}
+
+void robwork_freeFKTable(RW_FKTable* table)
+{
+    delete table;
+}
+
+void robwork_freeTimedStatePath(RW_TimedStatePath* path)
+{
+    delete path;
+}
+
+/*----------------------------------------------------------------------*
+  Data verification.
+ *----------------------------------------------------------------------*/
+
 const char* robwork_getWorkCellError(const RW_WorkCell* workcell)
 {
     RW_ASSERT(workcell);
     return frame->frame != NULL;
 }
 
-RW_TimedStatePath* robwork_makeTimedStatePath()
-{
-    return new RW_TimedStatePath();
-}
+/*----------------------------------------------------------------------*
+  Workcell operations
+ *----------------------------------------------------------------------*/
 
 RW_WorkCell* robwork_loadWorkCell(const char* file)
 {
     }
 }
 
-void robwork_freeWorkCell(RW_WorkCell* workcell)
-{
-    delete workcell;
-}
-
-void robwork_freeState(RW_State* state)
-{
-    delete state;
-}
-
-void robwork_freeDevice(RW_Device* device)
-{
-    delete device;
-}
-
-void robwork_freeTimedStatePath(RW_TimedStatePath* path)
-{
-    delete path;
-}
-
 RW_State* robwork_getState(const RW_WorkCell* workcell)
 {
     RW_ASSERT(workcell);
     return new RW_Frame(workcell->workcell->findFrame(name));
 }
 
+/*----------------------------------------------------------------------*
+  Device operations
+ *----------------------------------------------------------------------*/
+
 int robwork_getDOF(const RW_Device* device)
 {
     RW_ASSERT(device);
     writeQ(vals, device->device->getBounds().second);
 }
 
+/*----------------------------------------------------------------------*
+  State operations
+ *----------------------------------------------------------------------*/
+
+RW_State* robwork_copyState(const RW_State* state)
+{
+    RW_ASSERT(state);
+
+    return new RW_State(state->state);
+}
+
+RW_FKTable* robwork_getFKTable(const RW_State* state)
+{
+    RW_ASSERT(state);
+
+    return new RW_FKTable(ownedPtr(new FKTable(state->state)));
+}
+
+/*----------------------------------------------------------------------*
+  FKTable operations
+ *----------------------------------------------------------------------*/
+
+void robwork_getTransform(
+    const RW_FKTable* table,
+    const RW_Frame* frame,
+    double* out)
+{
+    RW_ASSERT(table);
+    RW_ASSERT(frame);
+    RW_ASSERT(out);
+
+    const Transform3D<>& t = table->table->get(frame->frame);
+    const Vector3D<>& p = t.P();
+    const Rotation3D<>& r = t.R();
+
+    out[0] = r(0, 0); out[1] = r(0, 1); out[2]  = r(0, 2); out[3]  = p(0);
+    out[4] = r(1, 0); out[5] = r(1, 1); out[6]  = r(1, 2); out[7]  = p(1);
+    out[8] = r(2, 0); out[9] = r(2, 1); out[10] = r(2, 2); out[11] = p(2);
+}
+
+/*----------------------------------------------------------------------*
+  Device and state operations
+ *----------------------------------------------------------------------*/
+
 void robwork_setQ(
     RW_State* state,
     const RW_Device* device,
     writeQ(vals, q);
 }
 
-RW_State* robwork_copyState(const RW_State* state)
+/*----------------------------------------------------------------------*
+  Frame operations
+ *----------------------------------------------------------------------*/
+
+int robwork_gripFrame(
+    RW_Frame* item, RW_Frame* gripper, RW_State* state)
 {
+    RW_ASSERT(item);
+    RW_ASSERT(gripper);
     RW_ASSERT(state);
 
-    return new RW_State(state->state);
+    try {
+        Kinematics::gripFrame(item->frame, gripper->frame, state->state);
+        return 1;
+    } catch (const Exception& e) {
+        // There is a useful error message in [e] but it is too much work
+        // passing it on.
+        return 0;
+    }
 }
 
+/*----------------------------------------------------------------------*
+  Collision detection
+ *----------------------------------------------------------------------*/
+
 int robwork_inCollision(const RW_WorkCell* workcell, const RW_State* state)
 {
     RW_ASSERT(state);
         state->state, 0, true);
 }
 
+/*----------------------------------------------------------------------*
+  (Time, State) path operations
+ *----------------------------------------------------------------------*/
+
+RW_TimedStatePath* robwork_makeTimedStatePath()
+{
+    return new RW_TimedStatePath();
+}
+
 void robwork_addTimedState(
     RW_TimedStatePath* path, double time, const RW_State* state)
 {
     path->path.push_back(Timed<State>(time, state->state));
 }
 
-int robwork_gripFrame(
-    RW_Frame* item, RW_Frame* gripper, RW_State* state)
-{
-    RW_ASSERT(item);
-    RW_ASSERT(gripper);
-    RW_ASSERT(state);
-
-    try {
-        Kinematics::gripFrame(item->frame, gripper->frame, state->state);
-        return 1;
-    } catch (const Exception& e) {
-        // There is a useful error message in [e] but it is too much work
-        // passing it on.
-        return 0;
-    }
-}
-
 int robwork_writeTimedStatePath(
     const RW_WorkCell* workcell,
     const RW_TimedStatePath* path,

c_robwork/papl_robwork.h

 typedef struct RW_Device RW_Device;
 typedef struct RW_State RW_State;
 typedef struct RW_Frame RW_Frame;
+typedef struct RW_FKTable RW_FKTable;
 typedef struct RW_TimedStatePath RW_TimedStatePath;
 
 /*----------------------------------------------------------------------*
 extern void robwork_freeDevice(RW_Device* device);
 extern void robwork_freeState(RW_State* state);
 extern void robwork_freeFrame(RW_Frame* frame);
+extern void robwork_freeFKTable(RW_FKTable* table);
 extern void robwork_freeTimedStatePath(RW_TimedStatePath* path);
 
 /*----------------------------------------------------------------------*
  *----------------------------------------------------------------------*/
 
 extern RW_State* robwork_copyState(const RW_State* state);
+extern RW_FKTable* robwork_getFKTable(const RW_State* state);
+
+/*----------------------------------------------------------------------*
+  FKTable operations
+ *----------------------------------------------------------------------*/
+
+extern void robwork_getTransform(
+    const RW_FKTable* table,
+    const RW_Frame* frame,
+    double* result);
 
 /*----------------------------------------------------------------------*
   Device and state operations

src/PaplRobwork.ml

 type state_t
 type device_t
 type frame_t
+type fk_t
+type transform_t = float array
 type q_t = float array
 type time_state_path_t
 
   = "caml_robwork_writeTimedStatePath"
 external caml_robwork_copyState : state_t -> state_t
   = "caml_robwork_copyState"
+external caml_robwork_getFKTable : state_t -> fk_t
+  = "caml_robwork_getFKTable"
+external caml_robwork_getTransform : fk_t -> frame_t -> transform_t -> unit
+  = "caml_robwork_getTransform"
 
 exception Exception of string
 
 
 let copy_state state = caml_robwork_copyState state
 
+module FK = struct
+  let make state = caml_robwork_getFKTable state
+
+  let get fk frame =
+    let buf = Array.make 12 0. in
+    let () = caml_robwork_getTransform fk frame buf in
+      buf
+end
+
 let grip_frame item frame state =
   if caml_robwork_gripFrame item frame state then ()
   else die "PaplRobwork.grip_frame: item must be a movable DAF."

src/PaplRobwork.mli

 type frame_t
 (** Frames. *)
 
+type fk_t
+(** Forward kinematics lookup table. *)
+
+type transform_t = float array
+(** 3D rigid body transformations.
+
+    The array is 12 elements long and contains the rows of a 3x4 transformation
+    matrix. In other words, if the rotation matrix is [R = (rij)] and the
+    translation vector is [p = (px, py, pz)], then the array equals
+    \[ [| r00; r01; r02; px;
+          r10; r11; r12; py;
+          r21; r22; r23; pz \] \].
+*)
+
 type q_t = float array
 (** Configurations. *)
 
     The file can be loaded into RobWorkStudio by its PlayBack plugin.
 *)
 
+(** {3 Forward kinematics} *)
+
+module FK : sig
+  val make : state_t -> fk_t
+  (** Forward kinematics table for a state.
+  *)
+
+  val get : fk_t -> frame_t -> transform_t
+  (** [get fk frame] is the transformation of [frame] in the world
+      frame given the state [fk].
+  *)
+end
+
 (** {2 Trajectories} *)
 
 module Trajectory : sig
 #include <papl_robwork.h>
 #include <stdio.h>
 
+/*----------------------------------------------------------------------*
+  Value accessors
+ *----------------------------------------------------------------------*/
+
 #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 FKTable_val(v) (*((RW_FKTable **) Data_custom_val(v)))
 #define TimedStatePath_val(v) (*((RW_TimedStatePath **) Data_custom_val(v)))
 
+/*----------------------------------------------------------------------*
+  Finalizers
+ *----------------------------------------------------------------------*/
+
 static void finalize_WorkCell(value model)
 {
     robwork_freeWorkCell(WorkCell_val(model));
     robwork_freeFrame(Frame_val(frame));
 }
 
+static void finalize_FKTable(value table)
+{
+    robwork_freeFKTable(FKTable_val(table));
+}
+
 static void finalize_TimedStatePath(value path)
 {
     robwork_freeTimedStatePath(TimedStatePath_val(path));
 }
 
+/*----------------------------------------------------------------------*
+  Value setup
+ *----------------------------------------------------------------------*/
+
 static struct custom_operations ops_WorkCell = {
   "org.bergsoe.caml_robwork_workcell",
   finalize_WorkCell,
   custom_deserialize_default
 };
 
+static struct custom_operations ops_FKTable = {
+  "org.bergsoe.caml_robwork_fktable",
+  finalize_FKTable,
+  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,
   custom_deserialize_default
 };
 
+/*----------------------------------------------------------------------*
+  Allocators
+ *----------------------------------------------------------------------*/
+
 #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_FKTable() alloc_custom(&ops_FKTable, sizeof(RW_FKTable *), 0, 1)
 #define alloc_TimedStatePath() alloc_custom(&ops_TimedStatePath, sizeof(RW_TimedStatePath *), 0, 1)
 
+/*----------------------------------------------------------------------*
+  Data verification.
+ *----------------------------------------------------------------------*/
+
 value caml_robwork_isValidDevice(value device_value)
 {
     CAMLparam1(device_value);
     CAMLreturn(Val_unit);
 }
 
+/*----------------------------------------------------------------------*
+  Workcell operations
+ *----------------------------------------------------------------------*/
+
 value caml_robwork_loadWorkCell(value name_value)
 {
     CAMLparam1(name_value);
     CAMLreturn(frame);
 }
 
+/*----------------------------------------------------------------------*
+  Device operations
+ *----------------------------------------------------------------------*/
+
 value caml_robwork_getDOF(value device_value)
 {
     CAMLparam1(device_value);
     CAMLreturn(Val_unit);
 }
 
+/*----------------------------------------------------------------------*
+  State operations
+ *----------------------------------------------------------------------*/
+
+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_getFKTable(value state_value)
+{
+    CAMLparam1(state_value);
+    const RW_State* state = State_val(state_value);
+
+    value table = alloc_FKTable();
+    FKTable_val(table) = robwork_getFKTable(state);
+    CAMLreturn(table);
+}
+
+/*----------------------------------------------------------------------*
+  FKTable operations
+ *----------------------------------------------------------------------*/
+
+value caml_robwork_getTransform(
+    value table_value,
+    value frame_value,
+    value result_value)
+{
+    CAMLparam3(table_value, frame_value, result_value);
+    const RW_FKTable* table = FKTable_val(table_value);
+    const RW_Frame* frame = Frame_val(frame_value);
+    double* result = (double*)result_value;
+
+    robwork_getTransform(table, frame, result);
+    CAMLreturn(Val_unit);
+}
+
+/*----------------------------------------------------------------------*
+  Device and state operations
+ *----------------------------------------------------------------------*/
+
 value caml_robwork_setQ(
     value state_value,
     value device_value,
     CAMLreturn(Val_unit);
 }
 
-value caml_robwork_copyState(value state_value)
+/*----------------------------------------------------------------------*
+  Frame operations
+ *----------------------------------------------------------------------*/
+
+value caml_robwork_gripFrame(
+    value item_value,
+    value gripper_value,
+    value state_value)
 {
-    CAMLparam1(state_value);
-    const RW_State* org_state = State_val(state_value);
+    CAMLparam3(item_value, gripper_value, state_value);
 
-    value state = alloc_State();
-    State_val(state) = robwork_copyState(org_state);
-    CAMLreturn(state);
+    RW_Frame* item = Frame_val(item_value);
+    RW_Frame* gripper = Frame_val(gripper_value);
+    RW_State* state = State_val(state_value);
+
+    CAMLreturn(Val_bool(robwork_gripFrame(item, gripper, state)));
 }
 
+/*----------------------------------------------------------------------*
+  Collision detection
+ *----------------------------------------------------------------------*/
+
 value caml_robwork_inCollision(value workcell_value, value state_value)
 {
     CAMLparam2(workcell_value, state_value);
     CAMLreturn(Val_bool(robwork_inCollision(workcell, state)));    
 }
 
+/*----------------------------------------------------------------------*
+  (Time, State) path operations
+ *----------------------------------------------------------------------*/
+
 value caml_robwork_makeTimedStatePath()
 {
     CAMLparam0();
 
     CAMLreturn(Val_bool(robwork_writeTimedStatePath(workcell, path, file)));
 }
-
-value caml_robwork_gripFrame(
-    value item_value,
-    value gripper_value,
-    value state_value)
-{
-    CAMLparam3(item_value, gripper_value, state_value);
-
-    RW_Frame* item = Frame_val(item_value);
-    RW_Frame* gripper = Frame_val(gripper_value);
-    RW_State* state = State_val(state_value);
-
-    CAMLreturn(Val_bool(robwork_gripFrame(item, gripper, state)));
-}