search

robotology-playground / sensors-calib-inertial

Calibration of joint offsets using inertial measurements.
9 stars 4 forks source link

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

Open nunoguedelha opened 6 years ago

nunoguedelha commented 6 years ago

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

General architecture

All the framework is designed following an Object Oriented approach:

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.

  1. Motion Sequence Profiles:

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:

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: https://github.com/robotology-playground/sensors-calib-inertial/blob/dacb506e6dc92bde46eacf6211a78a517d3cca95/src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile1.m#L40

Second step: https://github.com/robotology-playground/sensors-calib-inertial/blob/dacb506e6dc92bde46eacf6211a78a517d3cca95/src/conf/advanced/lowLevTauCtrlCalibratorSequenceProfile1.m#L41

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.

  1. Yarp and robot interface:

    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: https://github.com/robotology-playground/sensors-calib-inertial/blob/dacb506e6dc92bde46eacf6211a78a517d3cca95/src/%40MotionSequencer/seqMap2runner.m#L76-L82

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

https://github.com/robotology-playground/sensors-calib-inertial/blob/dacb506e6dc92bde46eacf6211a78a517d3cca95/src/%40MotionSequencer/run.m#L82-L110 (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: https://github.com/robotology-playground/sensors-calib-inertial/blob/dacb506e6dc92bde46eacf6211a78a517d3cca95/src/%40RemoteControlBoardRemapper/setMotorsPWM.m#L9

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 commented 6 years ago

CC @traversaro @DanielePucci

traversaro commented 6 years ago

@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 commented 6 years ago

@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 commented 6 years ago

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 commented 6 years ago

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

prashanthr05 commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

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

nunoguedelha commented 6 years ago

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(...): https://github.com/robotology-playground/sensors-calib-inertial/blob/dacb506e6dc92bde46eacf6211a78a517d3cca95/src/%40SensorDataYarpI/SensorDataYarpI.m#L123-L147

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 commented 6 years ago

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:

I don't get any problem during the compilation.

@nunoguedelha

nunoguedelha commented 6 years ago

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