Skip to content

Commit

Permalink
Update Kalman Filter configuration params to be initialized from XML …
Browse files Browse the repository at this point in the history 
…files (#268)

* Update EMS + MC4PLUS + MC2PLUS
- Support for Kalman Filter Configuration params from yarprobotinterface

* Update build date and hour for ems + mc2plus + mc4plus

* Update joint.c
  • Loading branch information
@sgiraz
sgiraz authored Jun 21, 2022
1 parent 7686edb commit 65f9ed0
Show file tree
Hide file tree
Showing 7 changed files with 53 additions and 63 deletions.
14 changes: 7 additions & 7 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,21 +81,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 51
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 52

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2022
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 6
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 14
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 21
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 14
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 10
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 40
// </h>build date
// </h>Info

Expand Down Expand Up @@ -495,7 +495,7 @@ extern "C" {

// <o> max size of output datagrams <64-1500>
// <i> default: 1024
#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1416
#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1432

// </h>datagrams in socket

Expand Down Expand Up @@ -554,7 +554,7 @@ extern "C" {

// <o> capacity of the ropframe of reply rops <16-1440:8>
// <i> default: 128
#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 264
#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 280



Expand Down
14 changes: 7 additions & 7 deletions emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,20 +75,20 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 35
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 36
// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2022
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 6
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 14
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 21
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 14
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 33
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 41
// </h>build date

// </h>Info
Expand Down Expand Up @@ -489,7 +489,7 @@ extern "C" {

// <o> max size of output datagrams <64-1500>
// <i> default: 1024
#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1416
#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1432

// </h>datagrams in socket

Expand Down Expand Up @@ -548,7 +548,7 @@ extern "C" {

// <o> capacity of the ropframe of reply rops <16-1440:8>
// <i> default: 128
#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 264
#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 280



Expand Down
14 changes: 7 additions & 7 deletions emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ extern "C" {
// <o> minor <0-255>


#define EOMTHEEMSAPPLCFG_VERSION_MINOR 49
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 50


// </h>version
Expand All @@ -96,13 +96,13 @@ extern "C" {
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2022
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 6
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 14
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 21
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 14
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 11
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 42

// </h>build date

Expand Down Expand Up @@ -504,7 +504,7 @@ extern "C" {

// <o> max size of output datagrams <64-1500>
// <i> default: 1024
#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1416
#define EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF 1432

// </h>datagrams in socket

Expand Down Expand Up @@ -563,7 +563,7 @@ extern "C" {

// <o> capacity of the ropframe of reply rops <16-1440:8>
// <i> default: 128
#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 264
#define EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY 280



Expand Down
20 changes: 20 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,13 @@ void Joint_init(Joint* o)

o->not_reversible = FALSE;

// initialize Kalman Filter parameters
o->kalman_filter_enabled = false;
memset(o->x0, 0, sizeof(o->x0));
memset(o->Q, 0, sizeof(o->Q));
o->R = 0;
o->P0 = 0;

#ifdef FINGER_MK3
o->ZTau = 560.0f*1.2f; // PWM/mNm * mNm;
o->Ke = 0.05f;
Expand Down Expand Up @@ -180,6 +187,19 @@ void Joint_config(Joint* o, uint8_t ID, eOmc_joint_config_t* config)

o->dead_zone = config->deadzone;

// copy Kalman Filter parameters configuration.
o->kalman_filter_enabled = config->kalman_params.enabled;
if(o->kalman_filter_enabled)
{
memcpy(o->x0, config->kalman_params.x0, sizeof(o->x0));
memcpy(o->Q, config->kalman_params.Q, sizeof(o->Q));
o->R = config->kalman_params.R;
o->P0 = config->kalman_params.P0;

// Initialize Kalman Filter for the joint
o->kalman_filter.initialize();
}

// eOerrmanDescriptor_t errdes = {0};
// char message[150];
//
Expand Down
40 changes: 5 additions & 35 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,30 +155,6 @@ void JointSet_config //
o->Jjm = Jjm; o->Sjm = Sjm;
o->Jmj = Jmj; o->Smj = Smj;
o->Sje = Sje;

// Initialize Kalman Filter
// TODO: this will become a parameter from xml config file
o->kalman_filter_enabled = FALSE;

if(o->kalman_filter_enabled)
{
o->x0[0] = {0};
o->x0[1] = {0};
o->x0[2] = {0};

o->Q[0] = {1.0e-4};
o->Q[1] = {1.0e-2 };
o->Q[2] = {10};

o->R = 1.0e-4;
o->P0 = 9.9e-05;

for (int js=0; js < *(o->pN); ++js)
{
int j = o->joints_of_set[js];
o->joint[j].kalman_filter.initialize();
}
}
}

void JointSet_do_odometry(JointSet* o) //
Expand All @@ -205,7 +181,7 @@ void JointSet_do_odometry(JointSet* o) //

o->joint[j].pos_fbk_from_motors += Sjm[j][m] * (float)o->motor[m].pos_fbk;

if (!o->kalman_filter_enabled)
if (!o->joint[j].kalman_filter_enabled)
{
o->joint[j].vel_fbk_from_motors += Sjm[j][m] * (float)o->motor[m].vel_fbk;
}
Expand All @@ -220,7 +196,7 @@ void JointSet_do_odometry(JointSet* o) //

o->joint[j].pos_fbk_from_motors = (float)o->motor[j].pos_fbk;

if (!o->kalman_filter_enabled)
if (!o->joint[j].kalman_filter_enabled)
{
o->joint[j].vel_fbk_from_motors = (float)o->motor[j].vel_fbk;
}
Expand Down Expand Up @@ -310,15 +286,15 @@ void JointSet_do_odometry(JointSet* o) //
o->joint[j].pos_fbk += Sje[j][e] * pos[e];
o->joint[j].vel_fbk += Sje[j][e] * vel[e];

if(o->kalman_filter_enabled)
if(o->joint[j].kalman_filter_enabled)
{
kf_input[j] += Sje[j][e] * mot[e];
}
}
}
}

if (o->kalman_filter_enabled && o->USE_SPEED_FBK_FROM_MOTORS)
if (o->joint[j].kalman_filter_enabled && o->USE_SPEED_FBK_FROM_MOTORS)
{
float32_t kf_output[3] = {0, 0, 0};

Expand All @@ -327,17 +303,11 @@ void JointSet_do_odometry(JointSet* o) //
j = o->joints_of_set[js];

// estimate joint velocity and acceleration
o->joint[j].kalman_filter.step(o->kalman_filter_enabled, kf_input[j], o->x0, o->P0, o->Q, o->R, kf_output);
o->joint[j].kalman_filter.step(o->joint[j].kalman_filter_enabled, kf_input[j], o->joint[j].x0, o->joint[j].P0, o->joint[j].Q, o->joint[j].R, kf_output);

// save the estimated velocity and acceleration
o->joint[j].vel_fbk = kf_output[1];
o->joint[j].acc_fbk = kf_output[2];

// TODO: Remove after test on real robot
// static uint32_t counter;
// static char msg[64] = {0};
// snprintf(msg, sizeof(msg), "%.3f %.3f %.3f", kf_input[j], o->joint[j].vel_fbk, o->joint[j].acc_fbk);
// hal_trace_puts(msg);
}
}
else
Expand Down
7 changes: 0 additions & 7 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,13 +113,6 @@ typedef struct // JointSet
#endif

TripodCalib tripod_calib;

// Kalman Filter Parameters
BOOL kalman_filter_enabled;
float x0[3];
float Q[3];
float R;
float P0;
} JointSet;

extern JointSet* JointSet_new(uint8_t n); //
Expand Down
7 changes: 7 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Joint_hid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,13 @@ struct Joint_hid // Joint

CTRL_UNITS dead_zone;

// Kalman Filter Parameters
BOOL kalman_filter_enabled;
float x0[3];
float Q[3];
float R;
float P0;

CTRL_UNITS pos_min;
CTRL_UNITS pos_max;
CTRL_UNITS vel_max;
Expand Down

0 comments on commit 65f9ed0

Please sign in to comment.