new task: LowlevCurrCtrlCalibrator

[lrapetti]

Low Level Current Control Calibrator

Analogously to the LowlevTauCtrlCalibrator, the branch https://github.com/lrapetti/sensors-calib-inertial/tree/task/LowlevCurrCtrlCalibrator contains an implementation of LowlevCurrCtrlCalibratorthat aims the estimation of $K_{PWM}$, $K_{bemf}$ and $i_{offset}$ expressing the relationship between current, PWM and motor velocity.

The motor model that is calibrated is:
$$i=K_{PWM} \cdot PWM + f_{K_{bemf}}(\dot q_{m})+i_{offset}$$
where:

• $\dot q_{m}$: is the motor velocity
• $f_{K_{bemf}}(.)$: is the back electromotive force, that we can model as linear ($i_{bemf}=K_{bemf} \cdot \dot q_{m}$).

The current calibration task consists in two steps for each joint:

• The first step consists in controlling the joint in position with a fixed position ($\dot q_{m} = 0$) and the user is requested to push the joint involving changes in the PWM signal. During this step $K_{PWM}$ and $i_{offset}$ are estimated. (During this step, the real-time plot of PWM and current is presented to the user).
• The second step consists in setting $PWM=0$ and the user is requested to move the joint involving changes in $\dot q_{m}$. Given a friction model (linear) and the offset computed in the previous step, $K_{bemf}$ is estimated.

Differences from torque calibration

• Also for the torque calibration the same two procedure were involved, but in the opposite order. This because in this case an estimation of the current offset is done during the $K_{PWM}$, while no offset or static friction is considered in the "friction" step (bemf step). In the torque calibration instead no offset is considered in the $K_{PWM}$ step, while a constant (Coulomb friction) is calibrated during the friction step. (*)
• In case of current calibration, the motor is calibrated disregarding the motor-joint coupling.

Implementation

In order to implement it, it is necessary to make some changes to the framework, and some changes still as to be discussed:

1. the LowlevCurrCtrlCalibrator is implemented in https://github.com/lrapetti/sensors-calib-inertial/tree/task/LowlevCurrCtrlCalibrator/src/%40LowlevCurrCtrlCalibrator as a derived of Calibrator. A better implementation would be the definition of a parent class LowLevCtrlCalibrator for LowlevCurrCtrlCalibrator and LowlevTauCtrlCalibrator, or even a single class achieving both if the calibration procedure remain the same.
2. The reading of current was not implemented and it has been added with commit lrapetti@ea8e100. A new type of sensors option curr has been defined, and the value of current in Ampere can be read from the StateExt:o or using the 'ICurrentControl`. Adding this part doesn't require any further change in the standard architecture of the system.
3. The real-time plotting for the friction acquisition step is currently implemented in the MotorPWMController class, so it was not possible to change it accordingly to the task that is being achieved. As a momentary solution I am just using a different plotting thread that plots current instead of torque (https://github.com/lrapetti/sensors-calib-inertial/tree/task/LowlevCurrCtrlCalibrator/src/%40MotorPWMcontroller). I think the best solution would be having the real time plotter defined in the Motion Sequencer with the settings defined in the configuration file of the trajectory.
4. For the current calibrator purpose, the Motor Transfer Function is different from the one of the torque calibrator (different parameters of the system are calibrated). At the moment, the solution I have found was to define a different motor transfer function (Electrical Motor Transfer Function) that describe the motor from the electrical point of view and contains the constant that have to be calibrated in calibrateSensors. An alternative solution would be having a single model that contains all the possible motor constants, and depending on the task only some of those are calibrated.
5. Currently for the calibration I am using a simple normal equation that as been add to the Regressors.

[lrapetti]

CC @nunoguedelha @traversaro

[nunoguedelha]

(5)

Currently for the calibration I am using a simple normal equation that as been add to the Regressors

I see this is a duplicate of pwmModel1Sym, but you dropped the removal of values too close to 0 to avoid the disturbancies due to the static friction. Are you sure you don't want to keep that filtering?

(*)

In the torque calibration instead no offset is considered in the KPWM step, while a constant (Coulomb friction) is calibrated during the friction step.

Actually yes, in the torque calibration, KPWM step, the offset is considered:

sensors-calib-inertial/src/@LowlevTauCtrlCalibrator/calibrateSensors.m

Lines 142 to 156 in feee760

	case 'ktau'
	% Check that the model is symmetrical
	[KoffP, KoffN, KpwmP, KpwmN] = deal(thetaPosXvar(1),thetaNegXvar(1),thetaPosXvar(2),thetaNegXvar(2));
	if ...
	abs(KoffP+KoffN)>abs(KoffP)/100 ...
	|| abs(KpwmP-KpwmN)>abs(KpwmP)/100
	warning('calibrateSensors: The Ktau model is not symmetrical');
	end
	% Run a fitting again but matching a single Ktau
	fittedModel = Regressors.pwmModel1Sym(xVar',tauMotorG');
	if abs(fittedModel.theta(1))>1e-3 % Tau offset
	warning('calibrateSensors: There is a torque offset in the model PWM to torque !!');
	end
	calib.setKpwm(fittedModel.theta(2));

• first, an "asymmetrical" model is used, i.e. a model that considers a pair of offsets and a pair of Kpwm, and these can be different,
• then we check if the parameters are symmetrical and raise a warning if so,
• finally, we fit a symmetrical model: 1 Kpwm, 1 offset, which is exactly the normal equation.

We indeed just save the Kpwm in the calibration file, considering in most cases that the offset is negligible. But actually, if I'm not mistaken, an offset can be set in the firmware low level torque controller, so in the future we might reconsider keeping the estimated one.

[nunoguedelha]

@lrapetti I've edited your initial comment and added some temporary paragraph references (1) and (2) for locating the comments in the context.

[lrapetti]

(5)

I see this is a duplicate of pwmModel1Sym, but you dropped the removal of values too close to 0 to avoid the disturbancies due to the static friction. Are you sure you don't want to keep that filtering?

I just wanted to start by fitting the simpler model and then eventually try adding filtering and fitting different models. Regarding this, I think that a more general name can be given to pwmModel1Sym regressor (filteredNormalEquation??)

(*)

Actually yes, in the torque calibration, KPWM step, the offset is considered

Yes, I have seen it is computed, but then it is not saved in the calibration file. In may case I would start having the offset in the calibration file, and then eventually not considering it if negligible. For this reason I have decided to estimate it during the $K_{PWM}$ calibration, and I remove the computed offset when estimating the $K_{bemf}$ (offset obtained during the $K_{bemf}$ calibration is then neglected).

What I think is important is to pay attention not to compute the offset twice. For example in the case of torque control the offset is part of the Coulomb friction, and considering the offset during the $K_{PWM}$ step would result in a wrong model.

[lrapetti]

@nunoguedelha
What do you think about this point?

The real-time plotting for the friction acquisition step is currently implemented in the MotorPWMController class, so it was not possible to change it accordingly to the task that is being achieved. As a momentary solution I am just using a different plotting thread that plots current instead of torque (https://github.com/lrapetti/sensors-calib-inertial/tree/task/LowlevCurrCtrlCalibrator/src/%40MotorPWMcontroller). I think the best solution would be having the real time plotter defined in the Motion Sequencer with the settings defined in the configuration file of the trajectory.

This would imply a new label plot in the trajectory definition as follow:

left_arm_seqParams.labels = {...
    'prpt'   ,'mode'   ,'ctrl'               ,'ctrl'          ,'meas'     ,'meas'     ,'pwmctrl'    ,'plot';...
    'NA'     ,'NA'     ,'pos'                ,'vel'           ,'joint'    ,'curr'     ,'pwm'        ,'curr';...
    'NA'     ,'NA'     ,'left_arm'           ,'left_arm'      ,'left_arm' ,'left_arm' ,motor        ,motor };
left_arm_seqParams.val = {...
    NA       ,'ctrl'   ,homeCalib.val{1}     ,repmat( 4,[1 7]),false      ,false      ,0      ,false        ;...
    promptStr,'pwmctrl',homeCalib.val{1}     ,repmat( 4,[1 7]),true       ,true       ,0      ,true        };

I probably can handle it

An alternative is to keep the real time plotting in the PWM controller adding a plot label to the pwmctrl as follow:

left_arm_seqParams.labels = {...
    'prpt'   ,'mode'   ,'ctrl'               ,'ctrl'          ,'meas'     ,'meas'     ,'pwmctrl'    ,'pwmctrl';...
    'NA'     ,'NA'     ,'pos'                ,'vel'           ,'joint'    ,'curr'     ,'pwm'        ,'plot';...
    'NA'     ,'NA'     ,'left_arm'           ,'left_arm'      ,'left_arm' ,'left_arm' ,motor        ,'curr' };
left_arm_seqParams.val = {...
    NA       ,'ctrl'   ,homeCalib.val{1}     ,repmat( 4,[1 7]),false      ,false      ,0      ,false        ;...
    promptStr,'pwmctrl',homeCalib.val{1}     ,repmat( 4,[1 7]),true       ,true       ,0      ,true        };

[nunoguedelha]

@lrapetti
(3)
Yes, indeed, the real time plotting should be independent from the controller. I actually have already created a class RelatimePlotter in another tool I implemented for the angular acceleration estimation. The class can be used as follows:

1. create a realtime plotter and pass the figure to it
2. create a separate thread, setting the thread start/stop/update the respective functions defined by the class.
3. run the thread
   It should be easy to reuse that. (We can eventually move the class to the sensors-calib-inertial).
   We can discuss this f2f.

Regarding how to trigger the plot, using the trajectory profile, I would go for the first solution, but I think all of this is impacted by the choice on the class hierarchy we choose: point (1) of the implementation section.

[nunoguedelha]

the LowlevCurrCtrlCalibrator is implemented in https://github.com/lrapetti/sensors-calib-inertial/tree/task/LowlevCurrCtrlCalibrator/src/%40LowlevCurrCtrlCalibrator as a derived of Calibrator. A better implementation would be the definition of a parent class LowLevCtrlCalibrator for LowlevCurrCtrlCalibrator and LowlevTauCtrlCalibrator, or even a single class achieving both if the calibration procedure remain the same.

Single class

Initially I thought about having a single class, which would be possible with the ReltimePlotter class I mentioned earlier. This solution would be faster to implement (maybe) but less flexible than having separate classes.

Separate classes

I fully agree with you, we should definitely go for a parent class LowLevCtrlCalibrator and two derived classes LowlevCurrCtrlCalibrator and LowlevTauCtrlCalibrator.

[nunoguedelha]

I just wanted to start by fitting the simpler model and then eventually try adding filtering and fitting different models. Regarding this, I think that a more general name can be given to pwmModel1Sym regressor (filteredNormalEquation??)

Yes, and make it call normalEquation(). Regarding the function names, the idea was to directly see a match between the calibration purpose ("pwm model ...") and the fitting method used, without having to dig into the calibrator code.
So we can define functions with general names, and handles to those functions as class properties, e.g.:

classdef Regressors < handle
    %UNTITLED8 Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
         pwmModel1Sym = @Regressors.filteredNormalEquation;
    end
    
    methods (Static=true)
        [thetaPos,thetaNeg] = normalEquationAsym(x,y);
        
        model = filteredNormalEquation(x,y);
        ... 
    end
    
end