Skip to content

Commit

Permalink
update ABAG, write log for all dimensions
Browse files Browse the repository at this point in the history 
* ABAG now allow tuning bias/gain threshold and step params
* write log files for position controllers in X, Y, Z dimensions
  • Loading branch information
@minhnh
minhnh committed Jul 2, 2020
1 parent 64764d0 commit f3c4334
Show file tree
Hide file tree
Showing 5 changed files with 72 additions and 51 deletions.
10 changes: 3 additions & 7 deletions generated/abag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
2 changes: 1 addition & 1 deletion generated/abag.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down
7 changes: 7 additions & 0 deletions include/kinova_util.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@

#include <string>
#include <memory>
#include <fstream>

#include <kdl_parser/kdl_parser.hpp>

Expand All @@ -15,6 +16,8 @@
#include <TransportClientUdp.h>
#include <TransportClientTcp.h>

#include "abag.h"

namespace k_api = Kinova::Api;
namespace sc = std::chrono;

Expand Down Expand Up @@ -45,4 +48,8 @@ void handleKinovaException(k_api::KDetailedException& ex);

bool waitMicroSeconds(const sc::time_point<sc::steady_clock> &pStartTime, const sc::microseconds &pDuration);

void writeDataRow(
std::ofstream &pFileStream, const abagState_t &pState, double &pError, double &pCommand, double &pMeasured
);

#endif // _KINOVA_UTIL_H
92 changes: 49 additions & 43 deletions src/control_kinova.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
* Copyright (c) 2020 Minh Nguyen inc. All rights reserved.
*
*/
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
Expand Down Expand Up @@ -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<double> joint_torque_limits {39.0, 39.0, 39.0, 39.0, 9.0, 9.0, 9.0};
const std::vector<double> 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;
Expand All @@ -138,6 +135,16 @@ void example_cyclic_torque_control (
sc::time_point<sc::steady_clock> controlStartTime = sc::steady_clock::now();;
sc::time_point<sc::steady_clock> loopStartTime;
sc::duration<int64_t, nano> 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++;
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
Expand All @@ -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());
Expand All @@ -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;
}
Expand All @@ -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;
Expand Down
12 changes: 12 additions & 0 deletions src/kinova_util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,4 +168,16 @@ bool waitMicroSeconds(const sc::time_point<sc::steady_clock> &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;
}

0 comments on commit f3c4334

Please sign in to comment.