From f3c4334e8cd5136614d11f5cd9f941bbaabe95b9 Mon Sep 17 00:00:00 2001 From: Minh Nguyen Date: Fri, 3 Jul 2020 00:45:54 +0200 Subject: [PATCH] update ABAG, write log for all dimensions * ABAG now allow tuning bias/gain threshold and step params * write log files for position controllers in X, Y, Z dimensions --- generated/abag.c | 10 ++--- generated/abag.h | 2 +- include/kinova_util.h | 7 ++++ src/control_kinova.cpp | 92 ++++++++++++++++++++++-------------------- src/kinova_util.cpp | 12 ++++++ 5 files changed, 72 insertions(+), 51 deletions(-) diff --git a/generated/abag.c b/generated/abag.c index a793759..d2036b4 100644 --- a/generated/abag.c +++ b/generated/abag.c @@ -89,18 +89,14 @@ void gainAdapater_sched(double const * e_bar, double const * gain_threshold, dou /* definitions of root schedules */ -void abag_sched(abagState_t * abagState, double const * error, double * actuation, double const * alpha) { +void abag_sched(abagState_t * abagState, double const * error, double * actuation, double const * alpha, double const * biasThreshold, double const * biasStep, double const * gainThreshold, double const * gainStep) { /* data block declarations */ - const double bias_threshold_xi = 0.000407; - const double bias_step_delta = 0.000400; - const double gain_threshold_xi = 0.502492; - const double gain_step_delta = 0.002; /* fixed data flow schedule */ errSign(error, &(abagState->signedErr_access)); errSignFilter(alpha, &(abagState->signedErr_access), &(abagState->eBarDelay[abagState->delayIndex_access]), &(abagState->eBar_access)); - biasAdapter_sched(&(abagState->eBar_access), &bias_threshold_xi, &bias_step_delta, &(abagState->bias_access)); - gainAdapater_sched(&(abagState->eBar_access), &gain_threshold_xi, &gain_step_delta, &(abagState->gain_access)); + biasAdapter_sched(&(abagState->eBar_access), biasThreshold, biasStep, &(abagState->bias_access)); + gainAdapater_sched(&(abagState->eBar_access), gainThreshold, gainStep, &(abagState->gain_access)); fb_sched(&(abagState->signedErr_access), actuation, &(abagState->gain_access), &(abagState->bias_access)); delayEbar(&(abagState->eBar_access), &(abagState->delayIndex_access), abagState->eBarDelay); diff --git a/generated/abag.h b/generated/abag.h index 1647a10..55e6f4b 100644 --- a/generated/abag.h +++ b/generated/abag.h @@ -28,7 +28,7 @@ void biasAdapter_sched(double const * e_bar, double const * bias_threshold, doub void gainAdapater_sched(double const * e_bar, double const * gain_threshold, double const * gain_step, double * adapted_gain); /* declarations of root schedules */ -void abag_sched(abagState_t * abagState, double const * error, double * actuation, double const * alpha); +void abag_sched(abagState_t * abagState, double const * error, double * actuation, double const * alpha, double const * biasThreshold, double const * biasStep, double const * gainThreshold, double const * gainStep); #ifdef __cplusplus } diff --git a/include/kinova_util.h b/include/kinova_util.h index cdaa70e..25e3b11 100644 --- a/include/kinova_util.h +++ b/include/kinova_util.h @@ -3,6 +3,7 @@ #include #include +#include #include @@ -15,6 +16,8 @@ #include #include +#include "abag.h" + namespace k_api = Kinova::Api; namespace sc = std::chrono; @@ -45,4 +48,8 @@ void handleKinovaException(k_api::KDetailedException& ex); bool waitMicroSeconds(const sc::time_point &pStartTime, const sc::microseconds &pDuration); +void writeDataRow( + std::ofstream &pFileStream, const abagState_t &pState, double &pError, double &pCommand, double &pMeasured +); + #endif // _KINOVA_UTIL_H diff --git a/src/control_kinova.cpp b/src/control_kinova.cpp index cc4447e..3cf4d39 100644 --- a/src/control_kinova.cpp +++ b/src/control_kinova.cpp @@ -2,6 +2,7 @@ * Copyright (c) 2020 Minh Nguyen inc. All rights reserved. * */ +#include #include #include #include @@ -102,29 +103,25 @@ void example_cyclic_torque_control ( auto control_mode_message = k_api::ActuatorConfig::ControlModeInformation(); control_mode_message.set_control_mode(k_api::ActuatorConfig::ControlMode::TORQUE); - // int last_actuator_device_id = 7; - // actuator_config->SetControlMode(control_mode_message, last_actuator_device_id); for (int actuator_id = 1; actuator_id < ACTUATOR_COUNT + 1; actuator_id++) actuator_config->SetControlMode(control_mode_message, actuator_id); - const std::vector joint_torque_limits {39.0, 39.0, 39.0, 39.0, 9.0, 9.0, 9.0}; const std::vector cart_force_limit {5.0, 5.0, 5.0, 5.0, 5.0, 5.0}; // N - const double alpha = 0.75; - double desired_vel = 6.0, current_vel = 0.0; //deg/s - double desiredPosX = 0.0, desiredPosY = 0.0, desiredPosZ = 0.0; - double errorX = 0.0, errorY = 0.0, errorZ = 0.0; - double abagCommandX = 0.0, abagCommandY = 0.0, abagCommandZ = 0.0; - - abagState_t abagStateX; - initialize_abagState(&abagStateX); - abagState_t abagStateY; - initialize_abagState(&abagStateY); - abagState_t abagStateZ; - initialize_abagState(&abagStateZ); - - FILE * fp = fopen("simulated_data.csv", "w+"); - fprintf(fp, "error, signed, bias, gain, e_bar, e_bar_prev, actuation, currentVel\n"); + + // ABAG parameters + const double alphaPosition[3] = { 0.75 }; + const double biasThresPos[3] = {0.000407, 0.000407, 0.000407}; + const double biasStepPos[3] = {0.000400, 0.000400, 0.000400}; + const double gainThresPos[3] = {0.502492, 0.502492, 0.502492}; + const double gainStepPos[3] = {0.002}; + double desiredPosistion[3] = { 0.0 }; + double errorPos[3] = { 0.0 }; + double commandPos[3] = { 0.0 }; + abagState_t abagStatePos[3]; + for (int i = 0; i < 3; i++) { + initialize_abagState(&abagStatePos[i]); + } // Real-time loop const int SECOND_IN_MICROSECONDS = 1000000; @@ -138,6 +135,16 @@ void example_cyclic_torque_control ( sc::time_point controlStartTime = sc::steady_clock::now();; sc::time_point loopStartTime; sc::duration totalElapsedTime; + + + // open log file streams + std::ofstream logPosX, logPosY, logPosZ; + logPosX.open("ctrl_data_pos_x.csv", std::ofstream::out | std::ofstream::trunc); + logPosY.open("ctrl_data_pos_y.csv", std::ofstream::out | std::ofstream::trunc); + logPosZ.open("ctrl_data_pos_z.csv", std::ofstream::out | std::ofstream::trunc); + logPosX << "error, signed, bias, gain, e_bar, command, measured" << std::endl; + logPosY << "error, signed, bias, gain, e_bar, command, measured" << std::endl; + logPosZ << "error, signed, bias, gain, e_bar, command, measured" << std::endl; while (loopStartTime - controlStartTime < TASK_TIME_LIMIT_MICRO) { iterationCount++; @@ -150,7 +157,7 @@ void example_cyclic_torque_control ( } catch(...) { - if (fp != nullptr) fclose(fp); + logPosX.close(); logPosY.close(); logPosZ.close(); std::cerr << "error reading sensors" << std::endl; throw; } @@ -189,23 +196,27 @@ void example_cyclic_torque_control ( if (iterationCount == 1) { - desiredPosX = endEffPose.p(0) + 0.03; - desiredPosY = endEffPose.p(1) + 0.03; - desiredPosZ = endEffPose.p(2) + 0.03; - std::cout << "desired: " << desiredPosX << ", " << desiredPosY << ", " << desiredPosZ << std::endl; + desiredPosistion[0] = endEffPose.p(0) + 0.03; + desiredPosistion[1] = endEffPose.p(1) + 0.03; + desiredPosistion[2] = endEffPose.p(2) + 0.03; + std::cout << "desired position: x=" << desiredPosistion[0] + << ", y=" << desiredPosistion[1] << ", z=" << desiredPosistion[2] << std::endl; } - errorX = desiredPosX - endEffPose.p(0); - errorY = desiredPosY - endEffPose.p(1); - errorZ = desiredPosZ - endEffPose.p(2); + errorPos[0] = desiredPosistion[0] - endEffPose.p(0); + errorPos[1] = desiredPosistion[1] - endEffPose.p(1); + errorPos[2] = desiredPosistion[2] - endEffPose.p(2); - abag_sched(&abagStateX, &errorX, &abagCommandX, &alpha); - abag_sched(&abagStateY, &errorY, &abagCommandY, &alpha); - abag_sched(&abagStateZ, &errorZ, &abagCommandZ, &alpha); + abag_sched(&abagStatePos[0], &errorPos[0], &commandPos[0], &alphaPosition[0], + &biasThresPos[0], &biasStepPos[0], &gainThresPos[0], &gainStepPos[0]); + abag_sched(&abagStatePos[1], &errorPos[1], &commandPos[1], &alphaPosition[1], + &biasThresPos[1], &biasStepPos[1], &gainThresPos[1], &gainStepPos[1]); + abag_sched(&abagStatePos[2], &errorPos[2], &commandPos[2], &alphaPosition[2], + &biasThresPos[2], &biasStepPos[2], &gainThresPos[2], &gainStepPos[2]); - endEffForce(0) = abagCommandX * cart_force_limit[0]; - endEffForce(1) = abagCommandY * cart_force_limit[1]; - endEffForce(2) = abagCommandZ * cart_force_limit[2]; + endEffForce(0) = commandPos[0] * cart_force_limit[0]; + endEffForce(1) = commandPos[1] * cart_force_limit[1]; + endEffForce(2) = commandPos[2] * cart_force_limit[2]; // std::cout << end_eff_force.transpose() << std::endl; jntImpedanceTorques.data = jacobianEndEff.data.transpose() * endEffForce; @@ -222,15 +233,9 @@ void example_cyclic_torque_control ( // std::cout << jntCmdTorques.data.transpose() << std::endl; // control - // fprintf(fp, "%.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\n", - // errorX, abagStateX.signedErr_access, abagStateX.bias_access, abagStateX.gain_access, abagStateX.eBar_access, - // abagCommandX, endEffPose.p(0)); - // fprintf(fp, "%.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\n", - // errorY, abagStateY.signedErr_access, abagStateY.bias_access, abagStateY.gain_access, abagStateY.eBar_access, - // abagCommandY, endEffPose.p(1)); - fprintf(fp, "%.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\n", - errorZ, abagStateZ.signedErr_access, abagStateZ.bias_access, abagStateZ.gain_access, abagStateZ.eBar_access, - abagCommandZ, endEffPose.p(2)); + writeDataRow(logPosX, abagStatePos[0], errorPos[0], commandPos[0], endEffPose.p(0)); + writeDataRow(logPosY, abagStatePos[1], errorPos[1], commandPos[1], endEffPose.p(1)); + writeDataRow(logPosZ, abagStatePos[2], errorPos[2], commandPos[2], endEffPose.p(2)); // abag_sched(&abagState, &error, &abag_command, &alpha); // base_command.mutable_actuators(6)->set_position(base_feedback.actuators(6).position()); @@ -251,7 +256,7 @@ void example_cyclic_torque_control ( } catch(...) { - if (fp != nullptr) fclose(fp); + logPosX.close(); logPosY.close(); logPosZ.close(); std::cerr << "error sending command" << std::endl; throw; } @@ -260,7 +265,8 @@ void example_cyclic_torque_control ( if (waitMicroSeconds(loopStartTime, LOOP_DURATION) != 0) slowLoopCount++; } - if (fp != nullptr) fclose(fp); + // close log files + logPosX.close(); logPosY.close(); logPosZ.close(); std::cout << "Torque control example completed" << std::endl; std::cout << "Number of loops which took longer than specified duration: " << slowLoopCount << std::endl; diff --git a/src/kinova_util.cpp b/src/kinova_util.cpp index 09a0777..c19444c 100644 --- a/src/kinova_util.cpp +++ b/src/kinova_util.cpp @@ -168,4 +168,16 @@ bool waitMicroSeconds(const sc::time_point &pStartTime, const now = sc::steady_clock::now(); } while (now - pStartTime < pDuration); return true; +} + +void writeDataRow( + std::ofstream &pFileStream, const abagState_t &pState, double &pError, double &pCommand, double &pMeasured +) { + pFileStream << pError << "," + << pState.signedErr_access << "," + << pState.bias_access << "," + << pState.gain_access << "," + << pState.eBar_access << "," + << pCommand << "," + << pMeasured << std::endl; } \ No newline at end of file