Hints on the framework design and entry points for integrating the motor parameters estimation using the current control

[nunoguedelha]

@lrapetti, @prashanthr05, I will depict here the link between the blocks in the framework design diagram below and the code entry points:

General architecture

All the framework is designed following an Object Oriented approach:

• all the features and interactions are implemented in class methods
• there is one main script src/app/sensorSelfCalibrator.m taking the input parameters from one main init configuration file src/conf/sensorSelfCalibratorInit.m and running all the class methods.

Init configuration files

1. Init config file: src/conf/sensorSelfCalibratorInit.m is the main user interface. The script defines:

• the robot modelName and modelPath
• the dataPath where all the logs will be saved
• the path to the saved calibration file (estimation results) calibrationMapFile. All the paths here are relative, starting from the application folder <repo_ROOT_path>/src/app
• the task checklist. e.g. for estimating the joint friction and motor parameters, set calibrateLowLevTauCtrl = true; and the other entries to false
• the "Common output parameters" are meant for saving the calibration and figures:

saveCalibration = true;
defaultSavePlot = true;
defaultExportPlot = true;

• the remaining file is separated in blocks, each block addressing a predefined task. Go to the block relative to the task you've selected. There are only 3 parameters there to set: calibedParts = {'right_leg'[,'left_leg','right_arm','left_arm']} selects the parts to calibrate; calibedJointsIdxes fine selects the joints for each active part (only the lines relative to selected parts are relevant); sensorDataAcq value triggers for each estimation procedure, a new acquisition, the last acquired data, or a specific data log ID. The two later features haven't been tested for the friction/motor parameters estimation. Refer to the comments for more details.

Changes for motor current control: you need an additional entry in the task checklist, and the respective block with the parameters calibedParts, calibedJointsIdxes and sensorDataAcq.

2. Motion Sequence Profiles:

• src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile1.m
• src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile2.m

Each of these define the target joint positions sequences to execute while acquiring the desired data, along with the list of sensors to get the data from. The profile1 defines the sequences for the friction parameters estimation, while profile2 targets the motor parameters estimation.
Each structure defining the joints position sequence has two fields: labels (the headers) and val (the values). There are two types of structures:
- one defining the home position: homeCalib
- another defining a sequence for each part
We will focus on the second type. We can take as an example the left arm sequence:

sensors-calib-inertial/src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile1.m

Lines 35 to 42 in dacb506

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

Each column defines a task (motion control, sensor data acquisition, control mode switch, ...), and each line in the val field section defines a time step in the sequence. Most often, each column header (for instance {'ctrl','pos','left_arm'}') defines respectively: an action, an action parameter, a target part. NA stands for Not Applicable and results in no action. Let's describe each column in this example over the two time steps.
First step:

sensors-calib-inertial/src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile1.m

Line 40 in dacb506

NA ,'ctrl' ,homeCalib.val{1} ,repmat( 4,[1 7]),false ,false ,0 ;...

• column 1: no action
• column 2: perform a mode switch (keyword mode) to position control mode (keyword ctrl)
• column 3,4: position control mode ctrl, position pos relative to the left_arm is set to homeCalib.val{1}, velocity vel is set to repmat( 4,[1 7]) => go to position that position (vector for the left arm 7 DoFs) with maximum velocity 4 degrees/s
• column 5,6: acquire measurements meas from joint encoders joint and joint torques jtorq on the left_arm are set to false => this step is just for positioning the arm
  column 7: irrelevant.

Second step:

sensors-calib-inertial/src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile1.m

Line 41 in dacb506

promptStr,'pwmctrl',homeCalib.val{1} ,repmat( 4,[1 7]),true ,true ,0 };

• column 1: display prompt string promptStr to the user
• column 2: perform a mode switch (keyword mode) to PWM control mode (keyword pwmctrl)
• column 3,4: left arm position is unchanged
• column 5,6: do acquire measurements meas from joint encoders joint and joint torques jtorq on the left_arm ('true')
• column 7: relative to the PWM control mode (pwmctrl), set the PWM (pwm) parameter value to 0 for the motor named motor (before this script is run, 'motor' is set to the joints/motors group being calibrated).

Changes for motor current control: replace the pwmctrl mode by a new mode currctrl, and the parameter pwm by a new parameter curr. These parameters are processed in the MotionSequencer class methods. Refer to the next section for more details.

3. Yarp and robot interface:

• src/conf/yarpPortNameRules.m defines the Yarp port naming rules. The file content is self-explanatory.
• src/conf/hardwareMechanicalsConfig.m defines some robot hardware parameters (usually found in robots-configuration/hardware/mechanicals) that cannot be properly retrieved from the robot interface yet: fullscalePWM, Gearbox_M2J (torque gearbox ratio from motor to joint), matrixM2J (motor to joint coupling matrix). The Gearbox_M2J is used here:

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

  Line 90 in dacb506

  tauMotorG = jointMotorCoupling.Tm2j(:,motorIdx)' * tauJoints;

The Yarp port naming rules and the hardware transmission parameters are already defined for iCub and don't require any change except if you're using another robot. The current information should already be logged in the stateExt port.

Main app, calibrator and motion sequencer classes

LowlevTauCtrlCalibrator is the main class driving the friction and motor parameters estimation procedures. MotionSequencer class triggers the ports open/close commands ('run' method) and processes the motor control parameters (run and seqMap2runner methods) extracted from the sequences defined in the sequence profiles mentioned in the previous section.

Changes for motor current control: add the new control mode currctrl handling. The existing mode pwmctrl is handled in the below code sections:

sensors-calib-inertial/src/@MotionSequencer/seqMap2runner.m

Lines 76 to 82 in dacb506

	if ismember('pwmctrl',fieldnames(runSeq))
	% list of join/motor groups
	runSeq.pwmctrl.motor = cell2mat(fieldnames(runSeq.pwmctrl.pwm));
	% reshape 'pwm'
	runSeq.pwmctrl.pwm = struct2cellConcat(runSeq.pwmctrl.pwm);
	end

(A similar section should be added for the current control mode currctrl)

sensors-calib-inertial/src/@MotionSequencer/run.m

Lines 82 to 110 in dacb506

	case 'pwmctrl' % PWM control
	% Only setting 1 motor or 1 set of coupled motors in PWM
	% control mode is supported.
	% Get name of motor to set in PWM control mode, as well as
	% the PWM value.
	motorName = sequence.pwmctrl.motor;
	pwm = sequence.pwmctrl.pwm{stepIdx};
	% Set the motor in PWM control mode and handle the coupled
	% motors keeping their control mode and state unchanged. If
	% this is not supported by the YARP remoteControlBoardRemapper,
	% emulate it. We can only emulate position control.
	pwmController = MotorPWMcontroller(motorName,obj.ctrlBoardRemap,Const.ThreadON);
	% Set the desired PWM level (0-100%) for the named motor
	ok = pwmController.setMotorPWM(pwm);
	% Prompt the user to proceed
	promptString = sequence.prpt{stepIdx}();
	if ~isempty(promptString)
	fprintf(promptString);
	pause;
	end
	% Stop the controller. This also restores the previous
	% control mode for the named motor and eventual coupled
	% motors.
	ok = pwmController.stop();

(A similar section should be added for the current control mode currctrl, replacing the MotorPWMcontroller by a new current controller).

Motor control interface

The class RemoteControlBoardRemapper wraps the bindings for creating the remote control board remapper device and get the motor interface for changing the control mode or setting the PWM (or the same way, the current). setMotorsPWM(...) is a good example on how to get an interface like IPWMControl to set desired motor parameters:

sensors-calib-inertial/src/@RemoteControlBoardRemapper/setMotorsPWM.m

Line 9 in dacb506

ipwm = obj.driver.viewIPWMControl();

Changes for motor current control: A similar interface should be created for the current control.

Note on the motor velocity measurement

@prashanthr05, in case the motor velocity measurement is not provided by the hardware, you should stub the respective method getMotorEncoderSpeeds in the remapper, with a code that estimates the velocity from other measurements (also considering the gearbox ratio).

[nunoguedelha]

CC @traversaro @DanielePucci

[traversaro]

@prashanthr05, in case the motor velocity measurement is not provided by the hardware, you should stub the respective method getMotorEncoderSpeeds in the remapper, with a code that estimates the velocity from other measurements (also considering the gearbox ratio).

I am not sure about this. If no low-level speed estimates are available, the proper replacement is a non-causal velocity estimation, that cannot be done in realtime without delay that could affect the estimation.

[nunoguedelha]

@traversaro , yes indeed. Sorry for the confusion, I was focusing on the interface for getting the velocity. The idea would be to have a stub that gets the last results from a velocity estimator. That estimator could eventually even be implemented in C++ and running under a different thread.

[traversaro]

Perhaps we need to discuss this f2f, but what I meant is that for estimation we should use a non-causal velocity estimation, that could be implemented in matlab.

[nunoguedelha]

ok, I get it. Anyway we can discuss further after lunch.

[prashanthr05]

I am not sure about this. If no low-level speed estimates are available, the proper replacement is a non-causal velocity estimation, that cannot be done in realtime without delay that could affect the estimation.

@traversaro To have clarity, in our case (if I'm not wrong), the joint velocities are obtained as a result of the velocity estimator, however we do not have a measure or an estimate for the motor velocities. But this could be obtained by mapping the joint velocities through the transmission. But, it would be better to not rely on these velocity estimates due to the inherent delay involved in the controller.

Hence we would rely on offline non-causal estimation of the velocities and accelerations by applying a savitzy-golay filtering over the joint positions. Please correct me, if I'm wrong ?

[prashanthr05]

ok, I get it. Anyway we can discuss further after lunch.

I'm sorry, I'm not in IIT. I could join you through skype, if you're going to have a meeting/discussion on this.

[lrapetti]

Before using current control, I require to do current control calibration similarly to LowlevTauCtrlCalibrator, and I am going to implement a LowlevCurrCtrlCalibrator task (to compute PWM-current constant and BEMF friction constant).

In order to do that I need to record current measurement that is currently not available in sensor-calib-intertial.

I have some questions regarding this implementation. @nunoguedelha

• The current, as the other quantities, is written on the StateExt port. However it is not parsed in readStateExt.m, and the getCurrents method is not defined in the sensor-calib-intertial ControlBoardRemapper.
  I think I can work on adding current measurement, but I am probably missing some passages of the sensors measurement process. It would help if you could give me some extra info on sensors measurement framework in sensor-calib-intertial?

• When a profile is defined, clearly the option ['meas' 'joint'] enable the measurement of qs, dqs, d2qs (joint position, velocity and acceleration) while ['meas' 'jtorq'] enables the measurement of taus (torques). I noticed there is no specific option for dqMs (motor velocity) and pwms, that anyway seems to be enabled by the joints measurement option ['meas' 'joint'].
  Did I get it right?
  Considering that I want to measure the current, does it need to be enabled by a different option ['meas' 'curr'], or should it be included in ['meas' 'joint'] option?

[nunoguedelha]

Enabling measurements for specific sensors

"It would help if you could give me some extra info on sensors measurement framework in sensor-calib-intertial?"

I forgot to mention in the initial notes... The compomemt SensorDataYarpI (snapshot on the left) handles the ports required for acquiring data from specific sensors. This is defined jointly by the config file https://github.com/robotology-playground/sensors-calib-inertial/blob/master/src/conf/yarpPortNameRules.m and the method buildSensType2portNgetter(...):

sensors-calib-inertial/src/@SensorDataYarpI/SensorDataYarpI.m

Lines 123 to 147 in dacb506

	function buildSensType2portNgetter(obj)
	% init parameters from config file
	run yarpPortNameRules;
	sensorType = {...
	'joint_from','acc_from','imu_from','fts_from','jtorq_from'...
	'joint_to','acc_to','imu_to','fts_to','jtorq_to',...
	'joint_path','acc_path','imu_path','fts_path','jtorq_path'};
	portNamingRule = {...
	eval(['@(robotname,part)' joints_port_rule_icub]),...
	eval(['@(robotname,part)' accSensors_port_rule_icub]),...
	eval(['@(robotname,part)' imuSensors_port_rule_icub]),...
	eval(['@(robotname,part)' FTSensors_port_rule_icub]),...
	eval(['@(robotname,part)' joints_port_rule_icub]),...
	eval(['@(robotname,part)' joints_port_rule_dumper]),...
	eval(['@(robotname,part)' accSensors_port_rule_dumper]),...
	eval(['@(robotname,part)' imuSensors_port_rule_dumper]),...
	eval(['@(robotname,part)' FTSensors_port_rule_dumper]),...
	eval(['@(robotname,part)' joints_port_rule_dumper]),...
	eval(['@(datapath,part)' joints_folder_rule_dumper]),...
	eval(['@(datapath,part)' accSensors_folder_rule_dumper]),...
	eval(['@(datapath,part)' imuSensors_folder_rule_dumper]),...
	eval(['@(datapath,part)' FTSensors_folder_rule_dumper]),...
	eval(['@(datapath,part)' joints_folder_rule_dumper])};
	obj.sensorType2portNameGetter = containers.Map(sensorType,portNamingRule);
	end

This function maps the sensor types to the Yarp ports where the respective data can be retrieved. The entries to that map are the sensor types you find in the Motion Sequence Profile files like https://github.com/robotology-playground/sensors-calib-inertial/blob/master/src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile1.m. This way ['joint' , 'from'] will map to the /icub/<part>/stateExt:o port.

The initial idea was to have an interface of the profile files independent w.r.t. how the ports group the sensor data on iCub, so although there are two labels 'joint' (for joint encoders) and 'jtorq', they both map to the same port on iCub. For some reason I forgot (probably lack of time), I didn't define a label for 'pwm' ad 'mot', but just reused 'joint'. So the option {'meas','joint'} is the one used.
For that to work, in the stateExt:o port, we parse all the available measurements joint q, dq, d2q, motor dq, and pwm.

Considering that I want to measure the current, does it need to be enabled by a different option ['meas' 'curr'], or should it be included in ['meas' 'joint'] option?

As you like, I suggest you use ['meas' 'curr'] and on the go, correct the case for 'pwm' and 'mot' (motor q,dq,d2q).

Retrieving the data offline from the port readStateExt.m

I let you take a look at the script and we can re-discuss on this tomorrow:

src/utils/yarp/readStateExt.m:function [q, dq, d2q, dqM, tau, pwm, time] = readStateExt(n, filename)

[lrapetti]

I am trying to run the sensor-calib-inertial (master branch)but at runtime I get the following error in MATLAB:

Error using yarpMEX
Invalid default value for property 'ctrlModeVocabDef' in class 'RemoteControlBoardRemapper':
Invalid default value for property 'aVocab' in class 'y':
function VOCAB requires at least 4 arguments

The problem may be related to the compilation of the bindings, and doesn't depend specifically on the ctrlModeVocabDef property since I got the same error with pidTypeVocabDef if it is moved before ctrlModeVocabDef.

I am currently at:

• yarp-matlab-bindings: bindings/regenerate
• yarp 8c421d66b57d767f7385a56fe0b7611c8c39f975

I don't get any problem during the compilation.

@nunoguedelha

[nunoguedelha]

@lrapetti , could you move this to a new issue please? in order to leave in this one only the tutorial discussion.