diff --git a/control/main_controller/clean_angles/clean.cpp b/control/main_controller/clean_angles/clean.cpp new file mode 100644 index 0000000..33868ce --- /dev/null +++ b/control/main_controller/clean_angles/clean.cpp @@ -0,0 +1,24 @@ +#include "clean.h" + +#include + +// Constants for knee and hip offsets +constexpr double KNEE_OFFSET = 0; // Example value +constexpr double HIP_OFFSET = 0; // Example value + +// Function to clean the offset in AI data readings +std::vector cleanAIOffsets(const std::vector& imuReadings) { + std::vector cleanedReadings(6); + + cleanedReadings[0] = imuReadings[0] - KNEE_OFFSET; // lk + cleanedReadings[4] = imuReadings[4] - KNEE_OFFSET; // rk + + cleanedReadings[3] = imuReadings[3] - HIP_OFFSET; // rh + cleanedReadings[5] = imuReadings[5] - HIP_OFFSET; // lh + + // Fixing Knee angles to be relative to the hip + cleanedReadings[0] = cleanedReadings[0] - cleanedReadings[5]; // left leg + cleanedReadings[4] = cleanedReadings[4] - cleanedReadings[3]; // right leg + + return cleanedReadings; +} \ No newline at end of file diff --git a/control/main_controller/clean_angles/clean.h b/control/main_controller/clean_angles/clean.h new file mode 100644 index 0000000..2e3b9e1 --- /dev/null +++ b/control/main_controller/clean_angles/clean.h @@ -0,0 +1,9 @@ +#ifndef CLEAN_H +#define CLEAN_H + +#include + +// Function to clean the offset in AI data readings +std::vector cleanAIOffsets(const std::vector& imuReadings); + +#endif // CLEAN_H diff --git a/control/main_controller/joint_estimator/jointEstimator.cpp b/control/main_controller/joint_estimator/jointEstimator.cpp new file mode 100644 index 0000000..9b0a549 --- /dev/null +++ b/control/main_controller/joint_estimator/jointEstimator.cpp @@ -0,0 +1,36 @@ +#include "jointEstimator.h" + +#include +#include + +#include "../torque_controller/torqueController.h" + +// constructor with initialized values +JointEstimator::JointEstimator() + : prev_angle(0.0), prev_velocity(0.0), velocity(0.0), acceleration(0.0) {} + + TorqueController::JointState JointEstimator::update(double current_angle, double dt) { + TorqueController::JointState angle_values; + + // Optional: Apply a low-pass filter to smooth the angle + angle_values.angle = current_angle; + + // Calculate velocity + angle_values.velocity = (current_angle - prev_angle) / dt; + velocity = angle_values.velocity; + + // Calculate acceleration + angle_values.acceleration = (velocity - prev_velocity) / dt; + + // Debugging output + std::cout << "current_angle: " << angle_values.angle + << ", velocity: " << angle_values.velocity + << ", acceleration: " << angle_values.acceleration + << std::endl; + + // Update previous values + prev_angle = current_angle; + prev_velocity = velocity; + + return angle_values; + } diff --git a/control/main_controller/joint_estimator/jointEstimator.h b/control/main_controller/joint_estimator/jointEstimator.h new file mode 100644 index 0000000..6400d9f --- /dev/null +++ b/control/main_controller/joint_estimator/jointEstimator.h @@ -0,0 +1,18 @@ +#pragma once + +#include + + +#include "../torque_controller/torqueController.h" + +class JointEstimator { + public: + JointEstimator(); + TorqueController::JointState update(double current_angle, double dt); + + private: + double prev_angle; + double prev_velocity; + double velocity; + double acceleration; +}; diff --git a/control/main_controller/main.cpp b/control/main_controller/main.cpp new file mode 100644 index 0000000..a84590e --- /dev/null +++ b/control/main_controller/main.cpp @@ -0,0 +1,177 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "../models/model.h" // header file for model +#include "../motors/motor_api.h" // header file for motor API +#include "../sensors/getSensorData.h" // header file for normzalied sensor data +#include "clean_angles/clean.h" // header file for clean angles +#include "joint_estimator/jointEstimator.h" +#include "torque_controller/torqueController.h" + +// test settings +const bool SIMULATION = true; // true when running without motors +const int MOTOR_NUMBER = 0; // number of motors 0-4 +const int SLEEP_TIME = 100; // sleep time in ms + +// to ensure motors shut down on Ctrl+C +static std::vector* g_motors_ptr = nullptr; +void signalHandler(int signum) { + std::cout << "\nCaught signal " << signum << ", disabling motors...\n"; + if (g_motors_ptr) { + for (auto& m : *g_motors_ptr) { + m.stopMotor(); + m.disconnectMotor(); + } + } + std::exit(signum); +} + +std::string get_current_timestamp() { + auto now = std::time(nullptr); + char buf[100]; + strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", localtime(&now)); + return std::string(buf); +} + +void log_message(const std::string& message) { + std::cout << get_current_timestamp() << " - " << message << std::endl; +} + +int main() { + std::vector motors; // vector of motors + double dt = 0.1; // Sampling rate for model + int chunk_index = 0; + + // Load the model + // load the models weights into memory + torch::jit::script::Module model; + try { + model = torch::jit::load("../model.pt"); + model.eval(); + log_message("Model loaded successfully."); + std::cout << std::endl; + } catch (const c10::Error& e) { + log_message("Error loading the model: " + std::string(e.what())); + return -1; + } + + // Initialize motors + if (!SIMULATION) { + motors = { + Motor("/dev/ttyUSB0", 1), + // Motor("/dev/ttyUSB1", 2), + // Motor("/dev/ttyUSB2", 3), + // Motor("/dev/ttyUSB3", 4), + }; + + g_motors_ptr = &motors; + + // register Ctrl+C handler + std::signal(SIGINT, signalHandler); + + for (auto& m : motors) { + if (!m.initializeMotor()) { + std::cerr << "Failed to init motor (slave " << m.getStatus() << ")\n"; + return -1; + } + m.setOperationMode(PT_MODE); + m.setMaxTorque(1000); + } + } + + // initialize variables + constexpr size_t WINDOW = 30; + std::vector> sensor_history(6, std::vector(WINDOW)); + TorqueController::JointState hipLeft, kneeLeft, hipRight, + kneeRight; // initializing joints + std::vector predicted_angles(6), clean_angles(6); + std::vector torque_values(4); + const std::string sensor_file = + "../filtered_imu_data_treadmill_5min_1.9mph.json"; + + // Control system configuration/initializing + JointEstimator hipRight_estimator; + JointEstimator kneeRight_estimator; + JointEstimator hipLeft_estimator; + JointEstimator kneeLeft_estimator; + + // Main loop + while (true) { + auto raw = get_sensor_data(sensor_file, chunk_index, WINDOW); + if (raw.size() < WINDOW) { + std::cerr << "Only " << raw.size() << " samples available; need " + << WINDOW << "\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + + chunk_index += 1; + + // take the last WINDOW entries and transpose into [6][30] + // for (size_t j = 0; j < 6; ++j) { + // for (size_t t = 0; t < WINDOW; ++t) { + // sensor_history[j][t] = raw[t][j]; + // } + // } + + // lk la ra rh rk lh + + // running AI model + predicted_angles = predict_joint_angles(model, raw); + + // Print predicted angles + // std::cout << "Predicted Angles:" << std::endl; + // for (size_t i = 0; i < predicted_angles.size(); ++i) { + // std::cout << " Joint " << i << ": " << predicted_angles[i] << + // std::endl; + // } + + // clean function call + clean_angles = cleanAIOffsets(predicted_angles); + std::transform(clean_angles.begin(), clean_angles.end(), clean_angles.begin(), + [](float angle) { return angle * M_PI / 180.0f; }); + + // Assign angles to the corresponding joints and log the updates + hipRight = hipRight_estimator.update(clean_angles[3], dt); + std::cout << "Updated HipRight: angle = " << hipRight.angle + << ", velocity = " << hipRight.velocity + << ", acceleration = " << hipRight.acceleration << std::endl; + + kneeRight = kneeRight_estimator.update(clean_angles[4], dt); + std::cout << "Updated KneeRight: angle = " << kneeRight.angle + << ", velocity = " << kneeRight.velocity + << ", acceleration = " << kneeRight.acceleration << std::endl; + + hipLeft = hipLeft_estimator.update(clean_angles[5], dt); + std::cout << "Updated HipLeft: angle = " << hipLeft.angle + << ", velocity = " << hipLeft.velocity + << ", acceleration = " << hipLeft.acceleration << std::endl; + + kneeLeft = kneeLeft_estimator.update(clean_angles[0], dt); + std::cout << "Updated KneeLeft: angle = " << kneeLeft.angle + << ", velocity = " << kneeLeft.velocity + << ", acceleration = " << kneeLeft.acceleration << std::endl; + + // torque calculator function call + TorqueController torqueController; + torque_values = + torqueController.computeTorque(hipRight, hipLeft, kneeRight, kneeLeft); + + if (!SIMULATION) { + for (size_t i = 0; i < motors.size(); ++i) { + motors[i].setTargetTorque(torque_values[i]); + log_message("torque for motor " + std::to_string(i) + ": " + + std::to_string(torque_values[i])); + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(SLEEP_TIME)); + } +} diff --git a/control/main_controller/torque_controller/torqueController.cpp b/control/main_controller/torque_controller/torqueController.cpp new file mode 100644 index 0000000..f7509f7 --- /dev/null +++ b/control/main_controller/torque_controller/torqueController.cpp @@ -0,0 +1,140 @@ +/* + * Computes the desired assistive torques for the hip and knee joints based on + * joint angles, velocities, accelerations, and a Mass Matrix + * + * The torque is calculated using: + * T = M(acceleration) * q̈ + C(velocity) * G(angle) + * + * Model derivation and justification are provided in the "Exoskeleton System + * Controller" document + */ + + #include "torqueController.h" + + #include + #include + #include + #include + #include + + // Torque Controller Values + const double m1 = 2.3; // Mass of thigh + const double m2 = 2.3; // Mass of shank + const double L1 = 0.5; // Length of thigh + const double L2 = 0.5; // Length of shank + const double r1 = 0.25; // Distance from hip joint to COM of thigh + const double r2 = 0.25; // Distance from knee joint to COM of shank + const double I1 = 0.01; // Moment of inertia of thigh about COM + const double I2 = 0.01; // Moment of inertia of shank about COM + const double g = 9.81; // Gravity constant + + std::vector TorqueController::computeTorque(JointState hipRight, + JointState hipLeft, + JointState kneeRight, + JointState kneeLeft) { + std::vector torque_values(4); + + // Helper lambda to compute torque for a single leg + auto computeLegTorque = [&](const JointState& hip, + const JointState& knee) -> std::vector { + std::array, 2> M, C; + std::array G; + + + + // Compute the Mass, Coriolis, and Gravity components + computeMassMatrix(hip.angle, knee.angle, M); + computeCoriolisMatrix(hip.angle, knee.angle, hip.velocity, knee.velocity, + C); + + std::cout << "hip angle: " << hip.angle << std::endl; + computeGravityVector(hip.angle, knee.angle, G); + + // Joint accelerations + std::array acc = {hip.acceleration, knee.acceleration}; + std::array vel = {hip.velocity, knee.velocity}; + + // Compute torques + double hipTorque = (M[0][0] * acc[0]) + (M[0][1] * acc[1]) + (C[0][0] * vel[0]) + + (C[0][1] * vel[1]) + G[0]; + + // debug output for each torque component for hip + std::cout << "---- Hip Torque Debug ----" << std::endl; + std::cout << "Mass Matrix (M):" << std::endl; + std::cout << " M[0][0]: " << M[0][0] << ", M[0][1]: " << M[0][1] << std::endl; + std::cout << " M[1][0]: " << M[1][0] << ", M[1][1]: " << M[1][1] << std::endl; + std::cout << "Coriolis Matrix (C):" << std::endl; + std::cout << " C[0][0]: " << C[0][0] << ", C[0][1]: " << C[0][1] << std::endl; + std::cout << " C[1][0]: " << C[1][0] << ", C[1][1]: " << C[1][1] << std::endl; + std::cout << "Gravity Vector (G):" << std::endl; + std::cout << " G[0]: " << G[0] << ", G[1]: " << G[1] << std::endl; + std::cout << "Joint Accelerations (acc):" << std::endl; + std::cout << " acc[0]: " << acc[0] << ", acc[1]: " << acc[1] << std::endl; + std::cout << "Joint Velocities (vel):" << std::endl; + std::cout << " vel[0]: " << vel[0] << ", vel[1]: " << vel[1] << std::endl; + + double kneeTorque = M[1][0] * acc[0] + M[1][1] * acc[1] + C[1][0] * vel[0] + + C[1][1] * vel[1] + G[1]; + + // Scale torques to correct units + int16_t scaledHipTorque = static_cast( + (hipTorque * ASSIST_RATE * 1000) / (GEAR_RATIO_HIP * RATED_TORQUE)); + int16_t scaledKneeTorque = static_cast( + (kneeTorque * ASSIST_RATE * 1000) / (GEAR_RATIO_KNEE * RATED_TORQUE)); + + // Debug output for computed torques + std::cout << " Hip Torque: " << hipTorque << " (scaled: " << scaledHipTorque + << "), Knee Torque: " << kneeTorque + << " (scaled: " << scaledKneeTorque << ")" << std::endl; + + return {scaledHipTorque, scaledKneeTorque}; + }; + + // Compute torques for each leg + std::cout << " Left Leg Torques: " << std::endl; + auto leftTorques = computeLegTorque(hipLeft, kneeLeft); + std::cout << " Right Leg Torques: " << std::endl; + auto rightTorques = computeLegTorque(hipRight, kneeRight); + + // Assign computed torques to the output array + torque_values[0] = rightTorques[0]; // Right hip + torque_values[1] = rightTorques[1]; // Right knee + torque_values[2] = leftTorques[0]; // Left hip + torque_values[3] = leftTorques[1]; // Left knee + + // Debug output for final torque values + std::cout << " Final Scaled Torque Values: [" << torque_values[0] << ", " + << torque_values[1] << ", " << torque_values[2] << ", " + << torque_values[3] << "]" << std::endl << std::endl; + + return torque_values; + } + void TorqueController::computeMassMatrix( + double q1, double q2, std::array, 2>& M) { + M[0][0] = + m1 * r1 * r1 + I1 + m2 * (L1 * L1 + r2 * r2 + 2 * L1 * r2 * std::cos(q2)); + M[0][1] = m2 * (L1 * r2 * std::cos(q2) + r2 * r2) + I1; + M[1][0] = M[0][1]; + M[1][1] = m2 * r2 * r2 + I2; + } + + void TorqueController::computeCoriolisMatrix( + double q1, double q2, double dq1, double dq2, + std::array, 2>& C) { + double s = std::sin(q2); + double term = m2 * L1 * r2 * s; + + C[0][0] = -term * dq2; + C[0][1] = -term * dq1 - term * dq2; + C[1][0] = term * dq1; + C[1][1] = 0; + } + + void TorqueController::computeGravityVector(double q1, double q2, + std::array& G) { + + G[0] = -m1 * g * r1 * std::sin(q1) - + m2 * g * (L1 * std::sin(q1) + r2 * std::sin(q1 + q2)); + G[1] = -m2 * g * r2 * std::sin(q1 + q2); + } + \ No newline at end of file diff --git a/control/main_controller/torque_controller/torqueController.h b/control/main_controller/torque_controller/torqueController.h new file mode 100644 index 0000000..4310062 --- /dev/null +++ b/control/main_controller/torque_controller/torqueController.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include + +class TorqueController { + public: + double GEAR_RATIO_KNEE = 31.0; // Gear ratio for torque scaling + double GEAR_RATIO_HIP = 37.0; // Gear ratio for torque scaling + double RATED_TORQUE = 0.64; // Rated torque for scaling + double ASSIST_RATE = 0.45; // Assist rate for scaling + + struct JointState { + double angle; + double velocity; + double acceleration; + }; + + std::vector computeTorque(struct JointState hipRight, + struct JointState hipLeft, + struct JointState kneeRight, + struct JointState kneeLeft); + + private: + void computeMassMatrix(double q1, double q2, + std::array, 2>& M); + void computeCoriolisMatrix(double q1, double q2, double dq1, double dq2, + std::array, 2>& C); + void computeGravityVector(double q1, double q2, std::array& G); +};