Init gym

cpp/ball_predictor/Makefile

@@ -0,0 +1,14 @@
+src = $(wildcard *.cpp)
+obj = $(src:.c=.o)
+
+CFLAGS = -O3 -shared -std=c++11 -fPIC -Wall $(PYBIND_INCLUDES)
+
+all: $(obj)
+	g++ $(CFLAGS) -o ball_predictor.so $^
+
+debug: $(filter-out lib_main.cpp,$(obj))
+	g++ -O0 -std=c++14 -Wall -g -o debug.bin debug_main.cc $^
+
+.PHONY: clean
+clean:
+	rm -f $(obj) all

cpp/ball_predictor/ball_predictor.cpp

@@ -0,0 +1,104 @@
+#include <cmath>
+#include "ball_predictor.h"
+
+
+float ball_pos_pred[600]; // ball position   (x,y) prediction for 300*0.02s = 6s 
+float ball_vel_pred[600]; // ball velocity   (x,y) prediction for 300*0.02s = 6s 
+float ball_spd_pred[300]; // ball linear speed (s) prediction for 300*0.02s = 6s 
+int pos_pred_len=0;
+
+/**
+ * @brief Get intersection with moving ball (intersection point and distance)
+ * @param x robot position (x)
+ * @param y robot position (y)
+ * @param max_robot_sp_per_step maximum speed per step
+ * @param ball_pos imported ball positions (possibly modified version of ball_pos_pred)
+ * @param ball_pos_len length of ball_pos
+ * @param ret_x returned position (x) of intersection point
+ * @param ret_y returned position (y) of intersection point
+ * @param ret_d returned distance between robot and intersection point
+ */
+void get_intersection_with_ball(float x, float y, float max_robot_sp_per_step, float ball_pos[], float ball_pos_len,
+                                float &ret_x, float &ret_y, float &ret_d){
+
+    float robot_max_displacement = 0.2; // robot has an immediate reach radius of 0.2m
+    int j=0;
+    
+    while(1){
+        float vec_x = ball_pos[j++] - x;
+        float vec_y = ball_pos[j++] - y;
+        float b_dist_sq = vec_x*vec_x + vec_y*vec_y; // squared ball distance
+        
+        // If robot has reached the ball, or the ball has stopped but the robot is still not there
+        if (b_dist_sq <= robot_max_displacement*robot_max_displacement or j>=ball_pos_len){
+            float d = sqrtf(b_dist_sq);
+            ret_d = d; 
+            ret_x = ball_pos[j-2];
+            ret_y = ball_pos[j-1];
+            break;
+        }
+        robot_max_displacement += max_robot_sp_per_step;
+    }
+}
+
+
+/**
+ * @brief Predict ball position/velocity until the ball stops or gets out of bounds (up to 6s)
+ * Adequate when the ball is rolling on the ground
+ * @param bx ball position (x)
+ * @param by ball position (y)
+ * @param vx ball velocity (x)
+ * @param vy ball velocity (y)
+ */
+void predict_rolling_ball_pos_vel_spd(double bx, double by, double vx, double vy){
+
+    // acceleration = Rolling Drag Force * mass (constant = 0.026 kg)
+    // acceleration = k1 * velocity^2 + k2 * velocity
+    const double k1 = -0.01;
+    const double k2 = -1;
+
+    const double k1_x = (vx < 0) ? -k1 : k1; // invert k1 if vx is negative, because vx^2 absorbs the sign
+    const double k1_y = (vy < 0) ? -k1 : k1; // invert k1 if vy is negative, because vy^2 absorbs the sign
+    
+    ball_pos_pred[0] = bx; // current ball position
+    ball_pos_pred[1] = by;
+    ball_vel_pred[0] = vx; // current ball velocity
+    ball_vel_pred[1] = vy;
+    ball_spd_pred[0] = sqrt(vx*vx+vy*vy);
+
+    int counter = 2;
+
+    while(counter < 600){
+
+        // acceleration
+        double acc_x = vx*vx*k1_x + vx*k2;
+        double acc_y = vy*vy*k1_y + vy*k2;
+
+        // second equation of motion: displacement = v0*t + 0.5*a*t^2
+        double dx = vx*0.02 + acc_x*0.0002; // 0.5*0.02^2 = 0.0002
+        double dy = vy*0.02 + acc_y*0.0002; // 0.5*0.02^2 = 0.0002
+
+        // position
+        bx += dx;
+        by += dy;
+
+        // abort when displacement is low or ball is out of bounds
+        if ((fabs(dx) < 0.0005 and fabs(dy) < 0.0005) or fabs(bx) > 15 or fabs(by) > 10){
+            break;
+        }
+
+        // velocity
+        vx += acc_x*0.02;
+        vy += acc_y*0.02;
+
+        // store as 32b
+        ball_spd_pred[counter/2] = sqrt(vx*vx+vy*vy);
+        ball_vel_pred[counter] = vx;
+        ball_pos_pred[counter++] = bx;
+        ball_vel_pred[counter] = vy;
+        ball_pos_pred[counter++] = by;
+        
+    }
+
+    pos_pred_len = counter;
+}

cpp/ball_predictor/ball_predictor.h

@@ -0,0 +1,10 @@
+#pragma once
+
+extern float ball_pos_pred[600]; // ball position   (x,y) prediction for 300*0.02s = 6s 
+extern float ball_vel_pred[600]; // ball velocoty   (x,y) prediction for 300*0.02s = 6s 
+extern float ball_spd_pred[300]; // ball linear speed (s) prediction for 300*0.02s = 6s 
+extern int pos_pred_len;
+
+extern void get_intersection_with_ball(float x, float y, float max_robot_sp_per_step, float ball_pos[], float ball_pos_len,
+                                       float &ret_x, float &ret_y, float &ret_d);
+extern void predict_rolling_ball_pos_vel_spd(double bx, double by, double vx, double vy);

cpp/ball_predictor/debug_main.cc

@@ -0,0 +1,54 @@
+#include "ball_predictor.h"
+#include <chrono>
+#include <iostream>
+#include <iomanip>
+
+using std::cout;
+using std::chrono::high_resolution_clock;
+using std::chrono::duration_cast;
+using std::chrono::microseconds;
+
+std::chrono::_V2::system_clock::time_point t1,t2;
+
+int main(){
+
+    // ================================================= 1. Generate data
+
+    float px = 3;
+    float py = 4;
+    float vx = -5;
+    float vy = -1;
+
+    // ================================================= 2. Compute prediction
+
+    t1 = high_resolution_clock::now();
+    predict_rolling_ball_pos_vel_spd(px, py, vx, vy);
+    t2 = high_resolution_clock::now();
+
+    cout << std::fixed << std::setprecision(8);
+
+    for(int i=0; i<pos_pred_len; i+=2){
+        cout << i/2 << " pos:" << ball_pos_pred[i] << "," << ball_pos_pred[i+1] <<
+                       " vel:" << ball_vel_pred[i] << "," << ball_vel_pred[i+1] <<
+                       " spd:" << ball_spd_pred[i/2] << "\n";
+    }    
+
+    cout << "\n\n" << duration_cast<microseconds>(t2 - t1).count() << "us for prediction\n";
+
+    // ================================================= 3. Generate data
+
+    float robot_x = -1;
+    float robot_y = 1;
+    float max_speed_per_step = 0.7*0.02;
+    float ret_x, ret_y, ret_d;
+
+    // ================================================= 4. Compute intersection
+
+    t1 = high_resolution_clock::now();
+    get_intersection_with_ball(robot_x, robot_y, max_speed_per_step, ball_pos_pred, pos_pred_len, ret_x, ret_y, ret_d);
+    t2 = high_resolution_clock::now();
+
+    cout << duration_cast<microseconds>(t2 - t1).count() << "us for intersection\n\n";
+    cout << "Intersection: " << ret_x << "," << ret_y << " dist: " << ret_d << "\n\n";
+
+}

cpp/ball_predictor/lib_main.cpp

@@ -0,0 +1,100 @@
+#include "ball_predictor.h"
+#include <pybind11/pybind11.h>
+#include <pybind11/numpy.h>
+
+namespace py = pybind11;
+using namespace std;
+
+
+/**
+ * @brief Predict rolling ball position, velocity, linear speed
+ * 
+ * @param parameters 
+ *        ball_x, ball_y, ball_vel_x, ball_vel_y
+ * @return ball_pos_pred, ball_vel_pred, ball_spd_pred
+ */
+py::array_t<float> predict_rolling_ball( py::array_t<float> parameters ){
+
+    // ================================================= 1. Parse data
+    
+    py::buffer_info parameters_buf = parameters.request();
+    float* parameters_ptr = (float*)parameters_buf.ptr;
+
+    float px = parameters_ptr[0];
+    float py = parameters_ptr[1];
+    float vx = parameters_ptr[2];
+    float vy = parameters_ptr[3];
+
+    // ================================================= 2. Compute path
+
+    predict_rolling_ball_pos_vel_spd(px, py, vx, vy);
+    
+    // ================================================= 3. Prepare data to return
+    
+    py::array_t<float> retval = py::array_t<float>(pos_pred_len+pos_pred_len+pos_pred_len/2); //allocate
+    py::buffer_info buff = retval.request();
+    float *ptr = (float *) buff.ptr;
+
+    for(int i=0; i<pos_pred_len; i++){
+        ptr[i] = ball_pos_pred[i];
+    }
+    ptr+=pos_pred_len;
+    for(int i=0; i<pos_pred_len; i++){
+        ptr[i] = ball_vel_pred[i];
+    }
+    ptr+=pos_pred_len;
+    for(int i=0; i<pos_pred_len/2; i++){
+        ptr[i] = ball_spd_pred[i];
+    }
+    return retval;
+}
+
+
+/**
+ * @brief Get point of intersection with moving ball
+ * 
+ * @param parameters 
+ *        robot_x, robot_y, robot_max_speed_per_step
+ * @return intersection_x, intersection_y, intersection_distance
+ */
+py::array_t<float> get_intersection( py::array_t<float> parameters ){
+
+    // ================================================= 1. Parse data
+    
+    py::buffer_info parameters_buf = parameters.request();
+    float* parameters_ptr = (float*)parameters_buf.ptr;
+    int params_len = parameters_buf.shape[0];
+
+    float x = parameters_ptr[0];
+    float y = parameters_ptr[1];
+    float max_sp = parameters_ptr[2];
+    float* ball_pos = parameters_ptr + 3;
+    float ret_x, ret_y, ret_d;
+
+    // ================================================= 2. Compute path
+
+    get_intersection_with_ball(x, y, max_sp, ball_pos, params_len-3, ret_x, ret_y, ret_d);
+    
+    // ================================================= 3. Prepare data to return
+    
+    py::array_t<float> retval = py::array_t<float>(3); //allocate
+    py::buffer_info buff = retval.request();
+    float *ptr = (float *) buff.ptr;
+
+    ptr[0] = ret_x;
+    ptr[1] = ret_y;
+    ptr[2] = ret_d;
+    return retval;
+}
+
+
+using namespace pybind11::literals; // to add informative argument names as -> "argname"_a
+
+PYBIND11_MODULE(ball_predictor, m) {  // the python module name, m is the interface to create bindings
+    m.doc() = "Ball predictor"; // optional module docstring
+
+    // optional arguments names
+    m.def("predict_rolling_ball", &predict_rolling_ball, "Predict rolling ball", "parameters"_a); 
+    m.def("get_intersection", &get_intersection, "Get point of intersection with moving ball", "parameters"_a); 
+}
+