diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 37aeabc..bd07dab 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -5,6 +5,7 @@ "label": "Clear Terminal", "type": "shell", "command": "clear", + "windows": { "command": "cls" }, "presentation": { "reveal": "never", "clear": true diff --git a/Swerve-Standard/inc/robot.h b/Swerve-Standard/inc/robot.h index 4c6de26..31e52f2 100644 --- a/Swerve-Standard/inc/robot.h +++ b/Swerve-Standard/inc/robot.h @@ -2,6 +2,7 @@ #define ROBOT_H #include "user_math.h" +#include "swerve_odometry.h" #include typedef enum Robot_State_e { @@ -18,6 +19,11 @@ typedef struct { float omega; uint8_t IS_SPINTOP_ENABLED; + float yaw; + + // Estimated pose according to wheel odometry + pose_2d pose; + // power management uint16_t power_index; uint16_t power_count; diff --git a/Swerve-Standard/src/chassis_task.c b/Swerve-Standard/src/chassis_task.c index 4d1be42..7f23dc6 100644 --- a/Swerve-Standard/src/chassis_task.c +++ b/Swerve-Standard/src/chassis_task.c @@ -3,14 +3,20 @@ #include "robot.h" #include "remote.h" #include "dji_motor.h" +#include "imu_task.h" #include "motor.h" #include "referee_system.h" #include "swerve_locomotion.h" +#include "swerve_odometry.h" #include "rate_limiter.h" +#include "FreeRTOS.h" +#include "task.h" + extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern Referee_System_t Referee_System; +extern IMU_t g_imu; DJI_Motor_Handle_t *g_azimuth_motors[NUMBER_OF_MODULES]; DJI_Motor_Handle_t *g_drive_motors[NUMBER_OF_MODULES]; @@ -25,8 +31,10 @@ float chassis_rad = WHEEL_BASE * 1.414f; //TODO init? float g_spintop_omega = SPIN_TOP_OMEGA; -void Chassis_Task_Init() -{ +float prev_yaw; +unsigned int last_odom_update; + +void Chassis_Task_Init() { // init common PID configuration for azimuth motors Motor_Config_t azimuth_motor_config = { .control_mode = POSITION_VELOCITY_SERIES, @@ -94,16 +102,21 @@ void Chassis_Task_Init() g_drive_motors[i] = DJI_Motor_Init(&drive_motor_config, M3508); } - // Initialize the swerve locomotion constants + // // Initialize the swerve locomotion constants g_swerve_constants = swerve_init(TRACK_WIDTH, WHEEL_BASE, WHEEL_DIAMETER, SWERVE_MAX_SPEED, SWERVE_MAX_ANGLUAR_SPEED); - // Initialize the rate limiters + // // Initialize the rate limiters for (int i = 0; i < NUMBER_OF_MODULES; i++) { rate_limiter_init(&chassis_vel_limiters[i], SWERVE_MAX_WHEEL_ACCEL); } #define SWERVE_MAX_OMEGA_ACCEL (5.0f) rate_limiter_init(&chassis_omega_limiter, SWERVE_MAX_OMEGA_ACCEL); + + g_robot_state.chassis.pose = (pose_2d){0, 0, 0}; + + prev_yaw = g_imu.rad.yaw; + last_odom_update = xTaskGetTickCount(); } void Chassis_Ctrl_Loop() @@ -157,6 +170,13 @@ void Chassis_Ctrl_Loop() swerve_calculate_kinematics(&g_chassis_state, &g_swerve_constants); swerve_optimize_module_angles(&g_chassis_state, measured_angles); //swerve_desaturate_wheel_speeds(&g_chassis_state, &g_swerve_constants); + + // Update odometry + unsigned int curr_time = xTaskGetTickCount(); + float dt = (curr_time - last_odom_update) / (float)configTICK_RATE_HZ; // convert to seconds + swerve_odometry_update(&g_robot_state.chassis.pose, g_chassis_state.states, &g_swerve_constants, g_robot_state.chassis.yaw, prev_yaw, dt); + last_odom_update = curr_time; + prev_yaw = g_imu.rad.yaw; // rate limit the module speeds for (int i = 0; i < NUMBER_OF_MODULES; i++) { diff --git a/Swerve-Standard/src/robot.c b/Swerve-Standard/src/robot.c index 5231d74..ddb19e9 100644 --- a/Swerve-Standard/src/robot.c +++ b/Swerve-Standard/src/robot.c @@ -2,6 +2,7 @@ #include "robot_tasks.h" #include "chassis_task.h" +#include "dji_motor.h" #include "gimbal_task.h" #include "launch_task.h" #include "gimbal_task.h" @@ -12,11 +13,14 @@ #include "user_math.h" #include "math.h" #include "rate_limiter.h" +#include "imu_task.h" Robot_State_t g_robot_state = {0}; extern Remote_t g_remote; extern Supercap_t g_supercap; +extern IMU_t g_imu; + extern DJI_Motor_Handle_t *g_yaw; Input_State_t g_input_state = {0}; @@ -115,9 +119,13 @@ void Process_Remote_Input() g_robot_state.input.vx = temp_x; g_robot_state.input.vy = temp_y; + // Update chassis yaw angle based on gimble IMU + // TODO: Place an IMU directly on the chassis, this is kind of jank + // UNTESTED!!! If odometry isn't working, it's probably this + float theta = DJI_Motor_Get_Absolute_Angle(g_yaw); + g_robot_state.chassis.yaw = g_imu.rad.yaw - theta; // Calculate Gimbal Oriented Control - float theta = DJI_Motor_Get_Absolute_Angle(g_yaw); g_robot_state.chassis.x_speed = -g_robot_state.input.vy * sin(theta) + g_robot_state.input.vx * cos(theta); g_robot_state.chassis.y_speed = g_robot_state.input.vy * cos(theta) + g_robot_state.input.vx * sin(theta); @@ -157,12 +165,12 @@ void Process_Remote_Input() } if (g_remote.controller.left_switch == UP) { // Left switch high to enable spintop - //g_robot_state.chassis.IS_SPINTOP_ENABLED = 1; - //g_robtot_state.launch.IS_FIRING_ENABLED = 1; + // g_robot_state.chassis.IS_SPINTOP_ENABLED = 1; + // g_robot_state.launch.IS_FIRING_ENABLED = 1; g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 1; } else { - //g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; - //g_robot_state.launch.IS_FIRING_ENABLED = 0; + // g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; + // g_robot_state.launch.IS_FIRING_ENABLED = 0; g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 0; } diff --git a/control-base/algo/inc/swerve_locomotion.h b/control-base/algo/inc/swerve_locomotion.h index 526cf80..2c6c177 100644 --- a/control-base/algo/inc/swerve_locomotion.h +++ b/control-base/algo/inc/swerve_locomotion.h @@ -1,6 +1,8 @@ #ifndef SWERVE_LOCOMOTION_H #define SWERVE_LOCOMOTION_H +#include "vec_utils.h" + #define NUMBER_OF_MODULES 4 typedef struct @@ -25,7 +27,8 @@ typedef struct float wheel_diameter; float max_speed; float max_angular_speed; - float kinematics_matrix[8][3]; + Mat* kinematics_matrix; + Mat* forward_kinematics_matrix; } swerve_constants_t; swerve_constants_t swerve_init(float track_width, float wheel_base, float wheel_diameter, float max_speed, float max_angular_speed); diff --git a/control-base/algo/inc/swerve_odometry.h b/control-base/algo/inc/swerve_odometry.h new file mode 100644 index 0000000..b0eaaaa --- /dev/null +++ b/control-base/algo/inc/swerve_odometry.h @@ -0,0 +1,15 @@ +#ifndef SWERVE_ODOMETRY_H +#define SWERVE_ODOMETRY_H + +#include "swerve_locomotion.h" + +typedef struct { + float x; // x position in meters + float y; // y position in meters + float theta; // orientation in radians +} pose_2d; + +void swerve_odometry_update(pose_2d *pose, module_state_t wheel_states[NUMBER_OF_MODULES], + swerve_constants_t *swerve_constants, float yaw, float previous_yaw, float dr); + +#endif // SWERVE_ODOMETRY_H \ No newline at end of file diff --git a/control-base/algo/inc/vec_utils.h b/control-base/algo/inc/vec_utils.h new file mode 100644 index 0000000..469d72b --- /dev/null +++ b/control-base/algo/inc/vec_utils.h @@ -0,0 +1,112 @@ +#ifndef VEC_UTILS_H +#define VEC_UTILS_H + +#include + + +// MACROS +#define MAT_IDX(m, i, j) ((m)->data[(i) * (m)->cols + (j)]) +#define VEC_IDX(m, i) ((m)->data[(i)]) + + +/* +------------------------------------------------------------- +SECTION: MATRICES +------------------------------------------------------------- +*/ + +// NxN Matrix struct +typedef struct Mat { + int rows; + int cols; + float* data; +} Mat; + +// matrix creation functions +Mat* new_mat(int rows, int cols); +Mat* new_eye(int size); +Mat* new_mat_buffer(int rows, int cols, float* buffer); +void free_mat(Mat* m); +Mat* mat_copy(Mat* m); +Mat* create_temp_mat(Mat* m); +Mat* mat_execute_and_free(Mat* (*func)(void *, ...), ...); + +// matrix helpers +char* mat_to_string(Mat* m); +void print_mat(Mat* m); +bool mat_equal(Mat* m1, Mat* m2, float tol); + +// general matrix operations +Mat* mat_mult(Mat* m1, Mat* m2); +Mat* mat_mult_buffer(Mat* m1, Mat* m2, Mat* product); +Mat* mat_scalar_mult(Mat* m, float scalar); +Mat* mat_scalar_mult_buffer(Mat* m, float scalar, Mat* product); +Mat* mat_add(Mat* m1, Mat* m2); +Mat* mat_add_buffer(Mat* m1, Mat* m2, Mat* sum); +Mat* mat_sub(Mat* m1, Mat* m2); +Mat* mat_sub_buffer(Mat* m1, Mat* m2, Mat* diff); + +// advanced matrix operations +float mat_determinant(Mat* m); +Mat* mat_adjoint(Mat* m); +Mat* mat_adjoint_buffer(Mat* m, Mat* buffer); +float mat_cofactor(Mat *m, int i, int j); +Mat* mat_cofactor_matrix(Mat* m); +Mat* mat_cofactor_matrix_buffer(Mat* m, Mat* buffer); +Mat* mat_inverse(Mat* m); +Mat* mat_inverse_buffer(Mat* m, Mat* buffer); +Mat* mat_transpose(Mat *m); +Mat* mat_transpose_buffer(Mat *m, Mat* buffer); +Mat* mat_transpose_overwrite(Mat *m); +Mat* mat_pseudo_inverse(Mat *m); +Mat* mat_damped_pseudo_inverse(Mat* m, float rho); + +// TODO: ADD PSEUDO INVERSE + +// TODO: If I feel like it: LU DECOMPOSITION, QR DECOMPOSITION, SVD DECOMPOSITION, EIGEN DECOMPOSITION, SOLVE LINEAR SYSTEM, SOLVE EIGENVALUE PROBLEM, SOLVE EIGENVECTOR PROBLEM, SOLVE SVD PROBLEM + +/* +------------------------------------------------------------- +SECTION: VECTORS +------------------------------------------------------------- +*/ + +// To maintain compatibility we use 1D matrix for vectors. You can simply use the matrix functions for vectors. +#define Vec Mat + +// vector creation functions +Vec* new_vec(int size); +Vec* new_vec_buffer(int size, float* buffer); + +// vector helpers +bool assert_vec(Vec* v); + +// general vector operations +float vec_dot(Vec* v1, Vec* v2); +Vec* vec_cross(Vec* v1, Vec* v2); +Vec* vec_cross_buffer(Vec* v1, Vec* v2, Vec* buffer); +float vec_magnitude(Vec* v); +Vec* vec_normalize(Vec* v); +Vec* vec_normalize_overwrite(Vec* v); + + +/* +------------------------------------------------------------- +SECTION: DH TRANSFORMATIONS +------------------------------------------------------------- +*/ + +typedef struct DH_Params { + float a; + float alpha; + float d; + float theta; +} DH_Params; + +// Denavit-Hartenberg matrix calculations +DH_Params* new_dh_params(float a, float alpha, float d, float theta); +void free_dh_params(DH_Params* dh); +Mat* dh_transform(DH_Params dh); +Mat* dh_transform_buffer(DH_Params dh, Mat* buffer); + +#endif // VEC_UTILS_H \ No newline at end of file diff --git a/control-base/algo/src/swerve_locomotion.c b/control-base/algo/src/swerve_locomotion.c index c2fa8e0..9502a56 100644 --- a/control-base/algo/src/swerve_locomotion.c +++ b/control-base/algo/src/swerve_locomotion.c @@ -1,7 +1,15 @@ #include "swerve_locomotion.h" #include +#include #include "user_math.h" +#include "vec_utils.h" + +float chassis_speeds_data[3]; +float module_states_data[NUMBER_OF_MODULES * 2]; + +Mat chassis_speeds_mat = {3, 1, chassis_speeds_data}; +Mat module_states_matrix = {NUMBER_OF_MODULES * 2, 1, module_states_data}; /** * @brief Initializes the swerve drive constants. @@ -20,18 +28,24 @@ swerve_constants_t swerve_init(float track_width, float wheel_base, float wheel_ .wheel_diameter = wheel_diameter, .max_speed = max_speed, .max_angular_speed = max_angular_speed, - .kinematics_matrix = { - {0, 1, -(wheel_base / 2)}, // front left - {1, 0, -(track_width / 2)}, - {0, 1, -(wheel_base / 2)}, // rear left - {1, 0, +(track_width / 2)}, - {0, 1, +(wheel_base / 2)}, // rear right - {1, 0, +(track_width / 2)}, - {0, 1, +(wheel_base / 2)}, // front right - {1, 0, -(track_width / 2)} - } + .kinematics_matrix = new_mat(8, 3), }; + float kinematics_data[8 * 3] = { + 1, 0, -(track_width / 2), + 0, 1, -(wheel_base / 2), // front left + 1, 0, +(track_width / 2), + 0, 1, -(wheel_base / 2), // rear left + 1, 0, +(track_width / 2), + 0, 1, +(wheel_base / 2), // rear right + 1, 0, -(track_width / 2), + 0, 1, +(wheel_base / 2) // front right + }; + + memcpy(swerve_constants.kinematics_matrix->data, kinematics_data, sizeof(kinematics_data)); + + swerve_constants.forward_kinematics_matrix = mat_pseudo_inverse(swerve_constants.kinematics_matrix); + return swerve_constants; } @@ -42,12 +56,12 @@ swerve_constants_t swerve_init(float track_width, float wheel_base, float wheel_ * @param swerve_constants Pointer to the swerve constants structure. */ void swerve_calculate_kinematics(swerve_chassis_state_t *chassis_state, swerve_constants_t *swerve_constants) { - float v_x = chassis_state->v_x; - float v_y = chassis_state->v_y; - float omega = chassis_state->omega; - float(*k_mat)[3] = swerve_constants->kinematics_matrix; - if (v_x == 0 && v_y == 0 && omega == 0) + chassis_speeds_mat.data[0] = chassis_state->v_x; + chassis_speeds_mat.data[1] = chassis_state->v_y; + chassis_speeds_mat.data[2] = chassis_state->omega; + + if (chassis_state->v_x == 0 && chassis_state->v_y == 0 && chassis_state->omega == 0) { for (int i = 0; i < NUMBER_OF_MODULES; i++) { @@ -58,34 +72,21 @@ void swerve_calculate_kinematics(swerve_chassis_state_t *chassis_state, swerve_c } // Multiply the inverse kinematics matrix by the chassis speeds vector - float module_states_matrix[8] = { - k_mat[0][0] * v_x + k_mat[0][1] * v_y + k_mat[0][2] * omega, - k_mat[1][0] * v_x + k_mat[1][1] * v_y + k_mat[1][2] * omega, - k_mat[2][0] * v_x + k_mat[2][1] * v_y + k_mat[2][2] * omega, - k_mat[3][0] * v_x + k_mat[3][1] * v_y + k_mat[3][2] * omega, - k_mat[4][0] * v_x + k_mat[4][1] * v_y + k_mat[4][2] * omega, - k_mat[5][0] * v_x + k_mat[5][1] * v_y + k_mat[5][2] * omega, - k_mat[6][0] * v_x + k_mat[6][1] * v_y + k_mat[6][2] * omega, - k_mat[7][0] * v_x + k_mat[7][1] * v_y + k_mat[7][2] * omega, - }; - + mat_mult_buffer(swerve_constants->kinematics_matrix, &chassis_speeds_mat, &module_states_matrix); + // Convert module x,y matrix to wheel speed and angle - for (int i = 0; i < NUMBER_OF_MODULES; i++) - { - float x = module_states_matrix[i * 2 + 1]; - float y = module_states_matrix[i * 2]; + for (int i = 0; i < NUMBER_OF_MODULES; i++) { + float x = MAT_IDX(&module_states_matrix, i * 2, 0); + float y = MAT_IDX(&module_states_matrix, i * 2 + 1, 0); float speed = hypotf(x, y); chassis_state->states[i].speed = speed; - if (speed > 1e-6f) - { + if (speed > 1e-6f) { float angle = atan2f(y, x); chassis_state->states[i].angle = angle; chassis_state->states[i].last_angle = angle; - } - else - { + } else { chassis_state->states[i].angle = chassis_state->states[i].last_angle; } } diff --git a/control-base/algo/src/swerve_odometry.c b/control-base/algo/src/swerve_odometry.c new file mode 100644 index 0000000..20892d0 --- /dev/null +++ b/control-base/algo/src/swerve_odometry.c @@ -0,0 +1,58 @@ +#include "swerve_odometry.h" + +#include "swerve_locomotion.h" +#include "vec_utils.h" + +#include + +float module_delta_data[NUMBER_OF_MODULES * 2]; +float chassis_delta_data[3]; + +Mat module_deltas = {NUMBER_OF_MODULES * 2, 1, module_delta_data}; +Mat chassis_delta = {3, 1, chassis_delta_data}; + +/** + * @brief Calculates the forward kinematics of the swerve drive. + * + * @param pose Pointer to the pose structure. + * @param wheel_states Array of module states. + * @param swerve_constants Pointer to the swerve constants structure. + * @param yaw Current yaw angle (rad) according to the IMU + * @param previous_yaw Previous yaw angle (rad) according to the IMU (When the last update was called) + * @param dt Time since the last update (s) + */ +void swerve_odometry_update(pose_2d *pose, module_state_t wheel_states[NUMBER_OF_MODULES], swerve_constants_t *swerve_constants, + float yaw, float previous_yaw, float dt) { + + // Populate module deltas matrix + for (int i = 0; i < NUMBER_OF_MODULES; i++) { + module_deltas.data[i * 2] = wheel_states[i].speed * dt * cosf(wheel_states[i].angle); + module_deltas.data[i * 2 + 1] = wheel_states[i].speed * dt * sinf(wheel_states[i].angle); + } + + // Calculate chassis delta + mat_mult_buffer(swerve_constants->forward_kinematics_matrix, &module_deltas, &chassis_delta); + + // Trust the IMU for yaw more than odometry + chassis_delta.data[2] = yaw - previous_yaw; + + // Calculate effect of chassis delta on pose + // Derivation can be found in section 10.4: https://file.tavsys.net/control/controls-engineering-in-frc.pdf + float sin_theta = sinf(chassis_delta.data[2]); + float cos_theta = cosf(chassis_delta.data[2]); + + float s; + float c; + if (fabsf(chassis_delta.data[2] < 1E-9)) { + s = 1.0 - chassis_delta.data[2] * chassis_delta.data[2] / 6.0; + c = 0.5 * chassis_delta.data[2]; + } else { + s = sin_theta / chassis_delta.data[2]; + c = (1.0 - cos_theta) / chassis_delta.data[2]; + } + + // Apply delta to the pose + pose->x += chassis_delta.data[0] * s - chassis_delta.data[1] * c; + pose->y += chassis_delta.data[0] * c + chassis_delta.data[1] * s; + pose->theta = yaw; // Pull directly from IMU +} \ No newline at end of file diff --git a/control-base/algo/src/vec_utils.c b/control-base/algo/src/vec_utils.c new file mode 100644 index 0000000..240e284 --- /dev/null +++ b/control-base/algo/src/vec_utils.c @@ -0,0 +1,827 @@ +#include "vec_utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +Mat *new_mat(int rows, int cols) +{ + Mat *mat = (Mat *)malloc(sizeof(Mat)); + + if (!mat) + { + perror("Failed to allocate memory for matrix"); + exit(EXIT_FAILURE); + } + + mat->rows = rows; + mat->cols = cols; + mat->data = (float *)malloc(rows * cols * sizeof(float)); + + if (!mat->data) + { + perror("Failed to allocate memory for matrix data"); + free(mat); + exit(EXIT_FAILURE); + } + + for (int i = 0; i < rows; i++) + { + for (int j = 0; j < cols; j++) + { + MAT_IDX(mat, i, j) = 0; + } + } + return mat; +} + +Mat *new_eye(int size) +{ + Mat *mat = new_mat(size, size); + for (int i = 0; i < size; i++) + { + MAT_IDX(mat, i, i) = 1; + } + return mat; +} + +Mat *new_mat_buffer(int rows, int cols, float *buffer) +{ + Mat *mat = (Mat *)malloc(sizeof(Mat)); + mat->rows = rows; + mat->cols = cols; + mat->data = (float *)malloc(rows * cols * sizeof(float)); + for (int i = 0; i < rows; i++) + { + for (int j = 0; j < cols; j++) + { + MAT_IDX(mat, i, j) = buffer[i * cols + j]; + } + } + return mat; +} + +void free_mat(Mat *m) +{ + if (m) + { + free(m->data); + free(m); + } +} + +Mat *mat_copy(Mat *m) +{ + Mat *copy = new_mat(m->rows, m->cols); + for (int i = 0; i < m->rows; i++) + { + for (int j = 0; j < m->cols; j++) + { + MAT_IDX(copy, i, j) = MAT_IDX(m, i, j); + } + } + return copy; +} + +Mat *create_temp_mat(Mat *m) +{ + return mat_copy(m); +} + +/** + * @brief DO NOT USE RN, IT IS BROKEN + * + * Execute a 5rfunction that returns a Mat* and free the arguments + * + * @param func The function to execute + * @param ... The arguments to pass to the function + * + * @attention The function must take Mat* arguments and NULL-terminate the list. + * Also any input matrices will be freed after the function is executed. If you want + * to keep the input matrices, pass a copy using mat_copy(). Alternatively, use the + * MAT_OP_FREE_INPUT() macro to automatically copy the input. + * + * @return The result of the function + * + * WARNING: Be very careful with this function. It is very easy to introduce memory leaks. + */ +Mat *mat_execute_and_free(Mat *(*func)(void *, ...), ...) { + va_list args; + va_start(args, func); + + Mat* arg_array[5]; + int arg_count = 0; + void* current_arg; + + while ((current_arg = va_arg(args, void*)) != NULL) { + if (arg_count >= 5) { + fprintf(stderr, "Too many arguments to mat_execute_and_free\n"); + va_end(args); + return NULL; + } + arg_array[arg_count++] = (Mat*)current_arg; + } + + va_end(args); + + Mat* result = NULL; + + switch (arg_count) { + case 1: result = func(arg_array[0]); break; + case 2: result = func(arg_array[0], arg_array[1]); break; + case 3: result = func(arg_array[0], arg_array[1], arg_array[2]); break; + case 4: result = func(arg_array[0], arg_array[1], arg_array[2], arg_array[3]); break; + case 5: result = func(arg_array[0], arg_array[1], arg_array[2], arg_array[3], arg_array[4]); break; + default: + fprintf(stderr, "Unsupported number of arguments in mat_execute_and_free\n"); + return NULL; + } + + for (int i = 0; i < arg_count; i++) { + free_mat(arg_array[i]); + } + + return result; +} + +/* +------------------------------------------------------------- +SECTION: Matrix Helpers +------------------------------------------------------------- +*/ + +char *mat_to_string(Mat *m) +{ + // esdtimate size + int buffer_size = 1000; + char *str = (char *)malloc(buffer_size * sizeof(char)); + if (!str) + return NULL; + + char temp[50]; // Temporary buffer for formatting each number + snprintf(str, buffer_size, "Matrix %dx%d\n", m->rows, m->cols); + + // check largest and smallest -> see if need sci notation decision + float max_val = 0.0, min_val = FLT_MAX; // __FLT_MAX__; + for (int i = 0; i < m->rows * m->cols; i++) + { + if (fabsf(m->data[i]) > max_val) + max_val = fabsf(m->data[i]); + if (fabsf(m->data[i]) < min_val && m->data[i] != 0) + min_val = fabsf(m->data[i]); + } + + int use_sci = (max_val > 1e4 || min_val < 1e-3); // if large diff, use sci notation + + for (int i = 0; i < m->rows; i++) + { + strcat(str, "|"); + for (int j = 0; j < m->cols; j++) + { + float val = MAT_IDX(m, i, j); + if (use_sci) + snprintf(temp, sizeof(temp), " % .2e", val); // scientific notation + else + snprintf(temp, sizeof(temp), " %6.2f", val); // default + + strcat(str, temp); + } + strcat(str, " |\n"); + } + + return str; +} + +void print_mat(Mat *m) +{ + char *str = mat_to_string(m); + printf("\n%s\n", str); + free(str); +} + +bool mat_equal(Mat *m1, Mat *m2, float tol) +{ + if (m1->rows != m2->rows || m1->cols != m2->cols) + { + return false; + } + for (int i = 0; i < m1->rows * m1->cols; i++) + { + if (fabsf(m1->data[i] - m2->data[i]) > tol) + { + return false; + } + } + return true; +} + +/* +------------------------------------------------------------- +SECTION: General Matrix Operations +------------------------------------------------------------- +*/ + +Mat *mat_mult(Mat *m1, Mat *m2) +{ + assert(m1->cols == m2->rows); + Mat *product = new_mat(m1->rows, m2->cols); + product->rows = m1->rows; + product->cols = m2->cols; + for (int i = 0; i < m1->rows; i++) + { + for (int j = 0; j < m2->cols; j++) + { + MAT_IDX(product, i, j) = 0; + for (int k = 0; k < m1->cols; k++) + { + MAT_IDX(product, i, j) += MAT_IDX(m1, i, k) * MAT_IDX(m2, k, j); + } + } + } + return product; +} + +Mat* mat_mult_buffer(Mat *m1, Mat *m2, Mat *product) +{ + // assert(m1->cols == m2->rows && + // product->rows == m1->rows && + // product->cols == m2->cols); + for (int i = 0; i < m1->rows; i++) + { + for (int j = 0; j < m2->cols; j++) + { + MAT_IDX(product, i, j) = 0; + for (int k = 0; k < m1->cols; k++) + { + MAT_IDX(product, i, j) += MAT_IDX(m1, i, k) * MAT_IDX(m2, k, j); + } + } + } + + return product; +} + +Mat *mat_scalar_mult(Mat *m, float scalar) +{ + Mat *product = new_mat(m->rows, m->cols); + product->rows = m->rows; + product->cols = m->cols; + for (int i = 0; i < m->rows; i++) + { + for (int j = 0; j < m->cols; j++) + { + MAT_IDX(product, i, j) = MAT_IDX(m, i, j) * scalar; + } + } + return product; +} + +Mat *mat_scalar_mult_buffer(Mat *m, float scalar, Mat *product) +{ + assert(product->rows == m->rows && product->cols == m->cols); + for (int i = 0; i < m->rows; i++) + { + for (int j = 0; j < m->cols; j++) + { + MAT_IDX(product, i, j) = MAT_IDX(m, i, j) * scalar; + } + } + + return product; +} + +Mat *mat_add(Mat *m1, Mat *m2) +{ + assert(m1->rows == m2->rows && m1->cols == m2->cols); + Mat *sum = new_mat(m1->rows, m1->cols); + sum->rows = m1->rows; + sum->cols = m1->cols; + for (int i = 0; i < m1->rows; i++) + { + for (int j = 0; j < m1->cols; j++) + { + MAT_IDX(sum, i, j) = MAT_IDX(m1, i, j) + MAT_IDX(m2, i, j); + } + } + return sum; +} + +Mat *mat_add_buffer(Mat *m1, Mat *m2, Mat *sum) +{ + assert(m1->rows == m2->rows && m1->cols == m2->cols && sum->rows == m1->rows && sum->cols == m1->cols); + for (int i = 0; i < m1->rows; i++) + { + for (int j = 0; j < m1->cols; j++) + { + MAT_IDX(sum, i, j) = MAT_IDX(m1, i, j) + MAT_IDX(m2, i, j); + } + } + return sum; +} + +Mat *mat_sub(Mat *m1, Mat *m2) +{ + assert(m1->rows == m2->rows && m1->cols == m2->cols); + Mat *diff = new_mat(m1->rows, m1->cols); + diff->rows = m1->rows; + diff->cols = m1->cols; + for (int i = 0; i < m1->rows; i++) + { + for (int j = 0; j < m1->cols; j++) + { + MAT_IDX(diff, i, j) = MAT_IDX(m1, i, j) - MAT_IDX(m2, i, j); + } + } + return diff; +} + +Mat *mat_sub_buffer(Mat *m1, Mat *m2, Mat *diff) +{ + assert(m1->rows == m2->rows && m1->cols == m2->cols && diff->rows == m1->rows && diff->cols == m1->cols); + for (int i = 0; i < m1->rows; i++) + { + for (int j = 0; j < m1->cols; j++) + { + MAT_IDX(diff, i, j) = MAT_IDX(m1, i, j) - MAT_IDX(m2, i, j); + } + } + return diff; +} + +/* +------------------------------------------------------------- +SECTION: Advanced Matrix Operations +------------------------------------------------------------- +*/ + +Mat *mat_transpose(Mat *m) +{ + Mat *transposed = new_mat(m->cols, m->rows); + for (int i = 0; i < m->rows; i++) + { + for (int j = 0; j < m->cols; j++) + { + MAT_IDX(transposed, j, i) = MAT_IDX(m, i, j); + } + } + return transposed; +} + +Mat *mat_transpose_buffer(Mat *m, Mat *buffer) +{ + assert(m->rows == buffer->cols && m->cols == buffer->rows); + for (int i = 0; i < m->rows; i++) + { + for (int j = 0; j < m->cols; j++) + { + MAT_IDX(buffer, j, i) = MAT_IDX(m, i, j); + } + } + return buffer; +} + +/** + * Transpose the matrix in place. + * NOTE: THIS FUNCTION ONLY WORKS FOR SQUARE MATRICES. + * + * @param m The matrix to transpose (will be overwritten). + * + * @return The transposed matrix. + */ +Mat *mat_transpose_overwrite(Mat *m) +{ + float temp; + for (int i = 0; i < m->rows; i++) + { + for (int j = i + 1; j < m->cols; j++) + { + temp = MAT_IDX(m, i, j); + MAT_IDX(m, i, j) = MAT_IDX(m, j, i); + MAT_IDX(m, j, i) = temp; + } + } + return m; +} + +/** + * Calculate the determinant of the matrix. + * + * @param m The matrix to calculate the determinant of. + * + * @return The determinant of the matrix. + * + * Example: + * + * if m = |1 2 3| + * |4 5 6| + * |7 8 9| + * + * You would call: mat_determinant(m) + */ +float mat_determinant(Mat *m) +{ + assert(m->rows == m->cols); + float det = 0.0f; + Mat *temp = NULL; + switch (m->rows) + { + case 1: + return m->data[0]; + case 2: + return m->data[0] * m->data[3] - m->data[1] * m->data[2]; + case 3: + for (int i = 0; i < 3; i++) + { + det += MAT_IDX(m, 0, i) * MAT_IDX(m, 1, (i + 1) % 3) * MAT_IDX(m, 2, (i + 2) % 3); + det -= MAT_IDX(m, 0, i) * MAT_IDX(m, 1, (i + 2) % 3) * MAT_IDX(m, 2, (i + 1) % 3); + } + return det; + default: + // submatrix for cofactor + temp = new_mat(m->rows - 1, m->cols - 1); + for (int j = 0; j < m->cols; j++) + { + float sign = (j % 2 == 0) ? 1.0f : -1.0f; + + int subi = 0; + for (int i = 1; i < 4; i++) + { + int subj = 0; + for (int k = 0; k < 4; k++) + { + if (k == j) + continue; + MAT_IDX(temp, subi, subj) = MAT_IDX(m, i, k); + subj++; + } + subi++; + } + det += sign * MAT_IDX(m, 0, j) * mat_determinant(temp); + } + free_mat(temp); + return det; + } +} + +float mat_cofactor(Mat *m, int i, int j) +{ + assert(m->rows == m->cols); + + Mat *submat = new_mat(m->rows - 1, m->cols - 1); + int sub_i = 0, sub_j = 0; + + for (int row = 0; row < m->rows; row++) + { + if (row == i) + continue; + sub_j = 0; + for (int col = 0; col < m->cols; col++) + { + if (col == j) + continue; + MAT_IDX(submat, sub_i, sub_j) = MAT_IDX(m, row, col); + sub_j++; + } + sub_i++; + } + + float cofactor = (1 - 2 * ((i + j) % 2)) * mat_determinant(submat); + free_mat(submat); + return cofactor; +} + +Mat *mat_cofactor_matrix(Mat *m) +{ + Mat *cofactor_mat = new_mat(m->rows, m->cols); + for (int i = 0; i < m->rows; i++) + { + for (int j = 0; j < m->cols; j++) + { + MAT_IDX(cofactor_mat, i, j) = mat_cofactor(m, i, j); + } + } + return cofactor_mat; +} + +Mat *mat_cofactor_matrix_buffer(Mat *m, Mat *buffer) +{ + assert(m->rows == buffer->rows && m->cols == buffer->cols); + for (int i = 0; i < m->rows; i++) + { + for (int j = 0; j < m->cols; j++) + { + MAT_IDX(buffer, i, j) = mat_cofactor(m, i, j); + } + } + return buffer; +} + +Mat *mat_adjoint(Mat *m) +{ + Mat *cofactor_mat = mat_cofactor_matrix(m); + Mat *adjoint_mat = mat_transpose_overwrite(cofactor_mat); + return adjoint_mat; +} + +Mat *mat_adjoint_buffer(Mat *m, Mat *buffer) +{ + mat_cofactor_matrix_buffer(m, buffer); + return mat_transpose_overwrite(buffer); +} + +Mat *mat_inverse(Mat *m) +{ + assert(m->rows == m->cols); + + Mat *cofactor_mat = new_mat(m->rows, m->cols); + + mat_cofactor_matrix_buffer(m, cofactor_mat); + float det = 0.0f; + for (int j = 0; j < m->cols; j++) + { + det += MAT_IDX(m, 0, j) * MAT_IDX(cofactor_mat, 0, j); + } + + assert(det != 0); // make sure matrix is invertible + + mat_transpose_overwrite(cofactor_mat); + mat_scalar_mult_buffer(cofactor_mat, 1.0f / det, cofactor_mat); + return cofactor_mat; +} + +Mat *mat_inverse_buffer(Mat *m, Mat *buffer) +{ + assert(m->rows == m->cols); + assert(buffer->rows == m->rows && buffer->cols == m->cols); + + mat_cofactor_matrix_buffer(m, buffer); + + float det = 0.0f; + for (int j = 0; j < m->cols; j++) + { + det += MAT_IDX(m, 0, j) * MAT_IDX(buffer, 0, j); + } + + assert(det != 0); // make sure matrix is invertible + + mat_transpose_overwrite(buffer); + mat_scalar_mult_buffer(buffer, 1.0f / det, buffer); + return buffer; +} + +Mat *mat_pseudo_inverse(Mat *m) +{ + Mat *m_t = mat_transpose(m); + Mat *m_tm, *m_tm_inv, *m_p; + + if (m->rows >= m->cols) + { + // tall matrix: (A^T A)^-1 A^T + m_tm = mat_mult(m_t, m); + m_tm_inv = mat_inverse(m_tm); + m_p = mat_mult(m_tm_inv, m_t); + } + else + { + // wide matrix: A^T (A A^T)^-1 + m_tm = mat_mult(m, m_t); + m_tm_inv = mat_inverse(m_tm); + m_p = mat_mult(m_t, m_tm_inv); + } + + free_mat(m_t); + free_mat(m_tm); + free_mat(m_tm_inv); + + return m_p; +} + +Mat *mat_damped_pseudo_inverse(Mat *m, float rho) +{ + Mat *m_t = mat_transpose(m); + Mat *m_tm, *m_tm_inv, *m_p, *m_eye; + + if (m->rows >= m->cols) + { + // tall matrix: (A^T A + ρ²I)^-1 A^T + m_tm = mat_mult(m_t, m); + m_eye = new_eye(m_tm->rows); + } + else + { + // wide matrix: A^T (A A^T + ρ²I)^-1 + m_tm = mat_mult(m, m_t); + m_eye = new_eye(m_tm->rows); + } + + mat_scalar_mult_buffer(m_eye, rho * rho, m_eye); + mat_add_buffer(m_tm, m_eye, m_tm); + m_tm_inv = mat_inverse(m_tm); + + if (m->rows >= m->cols) + { + m_p = mat_mult(m_tm_inv, m_t); + } + else + { + m_p = mat_mult(m_t, m_tm_inv); + } + + free_mat(m_t); + free_mat(m_tm); + free_mat(m_tm_inv); + free_mat(m_eye); + + return m_p; +} + +/* +------------------------------------------------------------- +SECTION: VECTOR CREATION FUNCTIONS +------------------------------------------------------------- +*/ + +Vec *new_vec(int size) +{ + Vec *vec = new_mat(size, 1); + return vec; +} + +Vec *new_vec_buffer(int size, float *buffer) +{ + Vec *vec = new_mat_buffer(size, 1, buffer); + return vec; +} + +/* +------------------------------------------------------------- +SECTION: VECTOR HELPERS +------------------------------------------------------------- +*/ + +bool assert_vec(Vec *v) +{ + bool cond = (v->cols == 1 && v->rows > 0); + assert(cond); + return cond; +} + +/* +------------------------------------------------------------- +SECTION: VECTOR OPERATIONS +------------------------------------------------------------- +*/ + +float vec_dot(Vec *v1, Vec *v2) +{ + assert(v1->rows == v2->rows); + assert_vec(v1); + assert_vec(v2); + + float dot = 0.0f; + for (int i = 0; i < v1->rows; i++) + { + dot += v1->data[i] * v2->data[i]; + } + return dot; +} + +Vec *vec_cross(Vec *v1, Vec *v2) +{ + assert(v1->rows == 3 && v2->rows == 3); + assert_vec(v1); + assert_vec(v2); + + Vec *cross = new_vec(3); + cross->data[0] = v1->data[1] * v2->data[2] - v1->data[2] * v2->data[1]; + cross->data[1] = v1->data[2] * v2->data[0] - v1->data[0] * v2->data[2]; + cross->data[2] = v1->data[0] * v2->data[1] - v1->data[1] * v2->data[0]; + return cross; +} + +Vec *vec_cross_buffer(Vec *v1, Vec *v2, Vec *buffer) +{ + assert(v1->rows == 3 && v2->rows == 3 && buffer->rows == 3); + assert_vec(v1); + assert_vec(v2); + assert_vec(buffer); + + buffer->data[0] = v1->data[1] * v2->data[2] - v1->data[2] * v2->data[1]; + buffer->data[1] = v1->data[2] * v2->data[0] - v1->data[0] * v2->data[2]; + buffer->data[2] = v1->data[0] * v2->data[1] - v1->data[1] * v2->data[0]; + return buffer; +} + +float vec_magnitude(Vec *v) +{ + assert_vec(v); + float mag = 0.0f; + for (int i = 0; i < v->rows; i++) + { + mag += v->data[i] * v->data[i]; + } + return sqrtf(mag); +} + +Vec *vec_normalize(Vec *v) +{ + assert_vec(v); + Vec *normalized = new_vec(v->rows); + float mag = vec_magnitude(v); + for (int i = 0; i < v->rows; i++) + { + normalized->data[i] = v->data[i] / mag; + } + return normalized; +} + +Vec *vec_normalize_overwrite(Vec *v) +{ + assert_vec(v); + float mag = vec_magnitude(v); + for (int i = 0; i < v->rows; i++) + { + v->data[i] /= mag; + } + return v; +} + +/* +------------------------------------------------------------- +SECTION: DH TRANSFORMATIONS +------------------------------------------------------------- +*/ + +DH_Params *new_dh_params(float a, float alpha, float d, float theta) +{ + DH_Params *dh = (DH_Params *)malloc(sizeof(DH_Params)); + dh->a = a; + dh->alpha = alpha; + dh->d = d; + dh->theta = theta; + return dh; +} + +void free_dh_params(DH_Params *dh) +{ + free(dh); +} + +Mat *dh_transform(DH_Params dh) +{ + Mat *transform = new_mat(4, 4); + // first row + MAT_IDX(transform, 0, 1) = cosf(dh.theta); + MAT_IDX(transform, 0, 2) = -1 * sinf(dh.theta) * cosf(dh.alpha); + MAT_IDX(transform, 0, 3) = sinf(dh.theta) * sinf(dh.alpha); + MAT_IDX(transform, 0, 4) = dh.a * cosf(dh.theta); + // second row + MAT_IDX(transform, 1, 1) = sinf(dh.theta); + MAT_IDX(transform, 1, 2) = cosf(dh.theta) * cosf(dh.alpha); + MAT_IDX(transform, 1, 3) = cosf(dh.theta) * sinf(dh.alpha); + MAT_IDX(transform, 1, 4) = dh.a * sinf(dh.theta); + // third row + MAT_IDX(transform, 2, 1) = 0; + MAT_IDX(transform, 2, 2) = sinf(dh.alpha); + MAT_IDX(transform, 2, 3) = cosf(dh.alpha); + MAT_IDX(transform, 2, 4) = dh.d; + // fourth row + MAT_IDX(transform, 3, 1) = 0; + MAT_IDX(transform, 3, 2) = 0; + MAT_IDX(transform, 3, 3) = 0; + MAT_IDX(transform, 3, 4) = 1; + + return transform; +} + +Mat *dh_transform_buffer(DH_Params dh, Mat *buffer) +{ + assert(buffer->rows == 4 && buffer->cols == 4); + // first row + MAT_IDX(buffer, 0, 1) = cosf(dh.theta); + MAT_IDX(buffer, 0, 2) = -1 * sinf(dh.theta) * cosf(dh.alpha); + MAT_IDX(buffer, 0, 3) = sinf(dh.theta) * sinf(dh.alpha); + MAT_IDX(buffer, 0, 4) = dh.a * cosf(dh.theta); + // second row + MAT_IDX(buffer, 1, 1) = sinf(dh.theta); + MAT_IDX(buffer, 1, 2) = cosf(dh.theta) * cosf(dh.alpha); + MAT_IDX(buffer, 1, 3) = cosf(dh.theta) * sinf(dh.alpha); + MAT_IDX(buffer, 1, 4) = dh.a * sinf(dh.theta); + // third row + MAT_IDX(buffer, 2, 1) = 0; + MAT_IDX(buffer, 2, 2) = sinf(dh.alpha); + MAT_IDX(buffer, 2, 3) = cosf(dh.alpha); + MAT_IDX(buffer, 2, 4) = dh.d; + // fourth row + MAT_IDX(buffer, 3, 1) = 0; + MAT_IDX(buffer, 3, 2) = 0; + MAT_IDX(buffer, 3, 3) = 0; + MAT_IDX(buffer, 3, 4) = 1; + + return buffer; +} \ No newline at end of file