use abag to control kinova joint 6

a2s-institute · Jun 30, 2020 · 90382d7 · 90382d7
1 parent d0ecf47
commit 90382d7
Show file tree

Hide file tree

Showing 4 changed files with 163 additions and 14 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -6,9 +6,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR   ON)
 set(CMAKE_VERBOSE_MAKEFILE      ON)
 set(CMAKE_COLOR_MAKEFILE        ON)
 
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE Release)
-endif()
+set(CMAKE_BUILD_TYPE Release)
 
 # Activate C++ 11
 set (CMAKE_CXX_STANDARD 11)
@@ -35,7 +33,12 @@ include_directories(${KORTEX_DIR}/include/client_stubs)
 # user code
 include_directories(${PROJECT_SOURCE_DIR}/include)
 include_directories(${PROJECT_SOURCE_DIR}/generated)
+set_source_files_properties(generated/abag.c PROPERTIES LANGUAGE CXX )
 
 link_libraries(pthread)
 
-add_executable(control_kinova src/control_kinova.cpp src/kinova_util.cpp)
+add_executable(control_kinova
+    src/control_kinova.cpp
+    src/kinova_util.cpp
+    generated/abag.c
+)
diff --git a/generated/abag.c b/generated/abag.c
@@ -0,0 +1,94 @@
+#include "abag.h"
+
+/* definitions of contained functions and schedules */
+void fb_sched(double const * error_sign, double * actuation, double const * gain, double const * bias) {
+  /* data block declarations */
+  double gainedErrSgn_ge_sgn;
+
+  /* fixed data flow schedule */
+  gainedErrSgn_ge_sgn = (*gain) * (*error_sign);
+  *actuation = +(*bias) + (gainedErrSgn_ge_sgn);
+
+  /* update output ports */
+}
+
+void errSign(double const * input, double * output) {
+  *output = (double)( (0 < *input) - (*input < 0) );
+}
+
+void errSignFilter(double const * filterFactor, double const * signedErr, double const * lowPassErrPrev, double * lowPassErr) {
+  *lowPassErr = (*filterFactor) * (*lowPassErrPrev) + (1 - (*filterFactor)) * (*signedErr);
+}
+
+void delayEbar(double const * input, int * rearIndex, double delay[]) {
+  delay[(*rearIndex)++] = *input;
+  if (*rearIndex == 1) {
+    *rearIndex = 0;
+  }
+}
+
+void biasAdapter_sched(double const * e_bar, double const * bias_threshold, double const * bias_step, double * adapted_bias) {
+  /* data block declarations */
+  double decision_access;
+  double bias_step_access;
+
+  /* fixed data flow schedule */
+
+  /* decision map biasAdaptDecision */
+  if      (*e_bar < -*bias_threshold) decision_access = (double) -1.;
+  else if (*e_bar > *bias_threshold) decision_access = (double) 1.;
+  else decision_access = (double) 0.;
+
+  bias_step_access = (*bias_step) * (decision_access);
+  *adapted_bias = +(*adapted_bias) + (bias_step_access);
+
+  /* saturation saturateBias */
+  if      (*adapted_bias > 1.) *adapted_bias = 1.;
+  else if (*adapted_bias < -1.) *adapted_bias = -1.;
+
+
+  /* update output ports */
+}
+
+void gainAdapater_sched(double const * e_bar, double const * gain_threshold, double const * gain_step, double * adapted_gain) {
+  /* data block declarations */
+  double decision_access;
+  double gain_step_access;
+
+  /* fixed data flow schedule */
+
+  /* decision map gainDecision */
+  if      (*e_bar < -*gain_threshold) decision_access = (double) 1.;
+  else if (*e_bar > *gain_threshold) decision_access = (double) 1.;
+  else decision_access = (double) -1.;
+
+  gain_step_access = (*gain_step) * (decision_access);
+  *adapted_gain = +(*adapted_gain) + (gain_step_access);
+
+  /* saturation saturateGain */
+  if      (*adapted_gain > 1.) *adapted_gain = 1.;
+  else if (*adapted_gain < -1.) *adapted_gain = -1.;
+
+
+  /* update output ports */
+}
+
+
+/* definitions of root schedules */
+void abag_sched(abagState_t * abagState, double const * error, double * actuation, double const * alpha) {
+  /* data block declarations */
+  const double bias_threshold_xi = 0.000457;
+  const double bias_step_delta = 0.000500;
+  const double gain_threshold_xi = 0.602492;
+  const double gain_step_delta = 0.003552;
+
+  /* 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));
+  fb_sched(&(abagState->signedErr_access), actuation, &(abagState->gain_access), &(abagState->bias_access));
+  delayEbar(&(abagState->eBar_access), &(abagState->delayIndex_access), abagState->eBarDelay);
+
+  /* update output ports */
+}
diff --git a/generated/abag.h b/generated/abag.h
@@ -0,0 +1,36 @@
+#ifndef ABAG_H
+#define ABAG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* include external headers */
+
+/* struct declarations */
+typedef struct abagState_st {
+  double eBarDelay[1];
+  int delayIndex_access;
+  double gain_access;
+  double bias_access;
+  double signedErr_access;
+  double eBar_access;
+} abagState_t;
+
+
+/* declarations of contained functions and schedules */
+void fb_sched(double const * error_sign, double * actuation, double const * gain, double const * bias);
+void errSign(double const * input, double * output);
+void errSignFilter(double const * filterFactor, double const * signedErr, double const * lowPassErrPrev, double * lowPassErr);
+void delayEbar(double const * input, int * rearIndex, double delay[]);
+void biasAdapter_sched(double const * e_bar, double const * bias_threshold, double const * bias_step, double * adapted_bias);
+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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/control_kinova.cpp b/src/control_kinova.cpp
@@ -12,8 +12,7 @@
 #include <time.h>
 
 #include "kinova_util.h"
-
-#include <google/protobuf/util/json_util.h>
+#include "abag.h"
 
 #include <unistd.h>
 
@@ -28,7 +27,7 @@ const int SECOND = 1000000;
 #define RAD_TO_DEG(x) (x) * 180.0 / PI
 #define ACTUATOR_COUNT 7
 
-float TIME_DURATION = 30.0f; // Duration of the example (seconds)
+float TIME_DURATION = 5.0f; // Duration of the example (seconds)
 
 std::chrono::steady_clock::time_point loop_start_time;
 std::chrono::duration <double, std::micro> loop_interval{};
@@ -48,7 +47,11 @@ int enforce_loop_frequency(const int dt)
     else return -1; //Loop is too slow
 }
 
-bool example_cyclic_torque_control(k_api::Base::BaseClient* base, k_api::BaseCyclic::BaseCyclicClient* base_cyclic, k_api::ActuatorConfig::ActuatorConfigClient* actuator_config)
+bool example_cyclic_torque_control (
+    k_api::Base::BaseClient* base,
+    k_api::BaseCyclic::BaseCyclicClient* base_cyclic,
+    k_api::ActuatorConfig::ActuatorConfigClient* actuator_config
+)
 {
     const int RATE_HZ = 600; // Hz
     const int DT_MICRO = SECOND / RATE_HZ;
@@ -100,9 +103,13 @@ bool example_cyclic_torque_control(k_api::Base::BaseClient* base, k_api::BaseCyc
 
 
         const std::vector<double> joint_torque_limits {39.0, 39.0, 39.0, 39.0, 9.0, 9.0, 9.0};
-        double desired_vel = 4.0; //deg/s
+        const double alpha = 0.8;
+        double desired_vel = 6.0, current_vel = 0.0; //deg/s
         double error = 0.0;
         double abag_command = 0.0;
+        abagState_t abagState;
+        FILE * fp = fopen("simulated_data.csv", "w+");
+        fprintf(fp, "error, signed, bias, gain, e_bar, e_bar_prev, actuation, currentVel\n");
 
         // Real-time loop
         while (total_time_sec < task_time_limit_sec)
@@ -140,14 +147,22 @@ bool example_cyclic_torque_control(k_api::Base::BaseClient* base, k_api::BaseCyc
             //     base_command.mutable_actuators(i)->set_torque_joint(jnt_command_torque(i));
             // }
 
-            error = desired_vel - base_feedback.actuators(6).velocity();
-            printf("error: %f\n \n", error);
-            // abag_command = abag.do_stuff(error);
+            current_vel = base_feedback.actuators(6).velocity();
+            if (total_time_sec > task_time_limit_sec * 0.1 && total_time_sec < task_time_limit_sec * 0.9) {
+                error = desired_vel - current_vel;
+            } else {
+                error = 0.0 - current_vel;
+            }
+
+            fprintf(fp, "%.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\n",
+                error, abagState.signedErr_access, abagState.bias_access, abagState.gain_access,abagState.eBar_access,
+                abag_command, current_vel);
+            abag_sched(&abagState, &error, &abag_command, &alpha);
 
             base_command.mutable_actuators(6)->set_position(base_feedback.actuators(6).position());
 
-            // base_command.mutable_actuators(6)->set_torque_joint(abag_command * joint_torque_limits[6]);
-            base_command.mutable_actuators(6)->set_torque_joint(1.0);
+            base_command.mutable_actuators(6)->set_torque_joint(abag_command * joint_torque_limits[6] * 0.8);
+            // base_command.mutable_actuators(6)->set_torque_joint(1.0);
 
             // Incrementing identifier ensures actuators can reject out of time frames
             base_command.set_frame_id(base_command.frame_id() + 1);
@@ -180,6 +195,7 @@ bool example_cyclic_torque_control(k_api::Base::BaseClient* base, k_api::BaseCyc
             // Enforce the constant loop time and count how many times the loop was late
             if (enforce_loop_frequency(DT_MICRO) != 0) control_loop_delay_count++;
         }
+        fclose(fp);
 
         std::cout << "Torque control example completed" << std::endl;