Python bindings: add collision_avoidance

bindings/cffirmware.i

@@ -9,12 +9,14 @@
 #include "pptraj.h"
 #include "planner.h"
 #include "stabilizer_types.h"
+#include "collision_avoidance.h"
 %}
 
 %include "math3d.h"
 %include "pptraj.h"
 %include "planner.h"
 %include "stabilizer_types.h"
+%include "collision_avoidance.h"
 
 %inline %{
 struct poly4d* piecewise_get(struct piecewise_traj *pp, int i)
@@ -37,6 +39,41 @@ void poly4d_free(struct poly4d *p)
 {
     free(p);
 }
+
+struct vec vec2svec(struct vec3_s v)
+{
+    return mkvec(v.x, v.y, v.z);
+}
+
+struct vec3_s svec2vec(struct vec v)
+{
+    struct vec3_s vv = {
+        .x = v.x,
+        .y = v.y,
+        .z = v.z,
+    };
+    return vv;
+}
+
+void collisionAvoidanceUpdateSetpointWrap(
+    collision_avoidance_params_t const *params,
+    collision_avoidance_state_t *collisionState,
+    int nOthers,
+    float const *otherPositions,
+    setpoint_t *setpoint, sensorData_t const *sensorData, state_t const *state)
+{
+    nOthers /= 3;
+    float *workspace = malloc(sizeof(float) * 7 * (nOthers + 6));
+    collisionAvoidanceUpdateSetpointCore(
+        params,
+        collisionState,
+        nOthers,
+        otherPositions,
+        workspace,
+        setpoint, sensorData, state);
+    free(workspace);
+}
+
 %}
 
 %pythoncode %{
@@ -94,3 +131,7 @@ structname(struct structname const *x) { \
             return _cffirmware.vsub(self, other)
     %}
 };
+
+%extend traj_eval {
+    COPY_CTOR(traj_eval)
+};

bindings/setup.py

@@ -14,7 +14,8 @@
 modules = [
     "pptraj.c",
     "pptraj_compressed.c",
-    "planner.c"
+    "planner.c",
+    "collision_avoidance.c"
 ]
 fw_sources = [os.path.join(fw_dir, "src/modules/src", mod) for mod in modules]