Log joint_state (of all joints)

[nunoguedelha]

Log the position, velocity, acceleration and torque from the joints stateExt:o ports:

• ✅ [utils] Minor refactoring: move processedConfig.js functions to utils.js.
• ✅ [openmctStaticServer] Minor refactoring (comments, variable renaming).
• ✅ Integrate object variables interpolation in JSON dictionary.
• ✅ [dictionary] Generalise left leg valuesTemplates, indifferentiate left from right.
• ✅ [icubTelemetry] Extend the joint state telemetry data to vel, acc, trq.
• ✅ [iCubTelemetry] Extend the joint state telemetry data to other robot parts.
• ✅ [iCubTelemetry] Fix components---positions, velocities, etc---mapping to the logged port data nested blocks.

Implemented in #127 .

[nunoguedelha]

Mapping of joint state modalities (positions, velocities, etc) to the port data nested blocks

The mapping is defined in jointData.h and jointData.cpp which is included in the control board wrapper definition files:

Include graph for jointData.h

jointData constructor

bool jointData::read(yarp::os::idl::WireReader& reader)
{
    if (!nested_read_jointPosition(reader)) {
        return false;
    }
    if (!read_jointPosition_isValid(reader)) {
        return false;
    }
    if (!nested_read_jointVelocity(reader)) {
        return false;
    }
    if (!read_jointVelocity_isValid(reader)) {
        return false;
    }
    if (!nested_read_jointAcceleration(reader)) {
        return false;
    }
    if (!read_jointAcceleration_isValid(reader)) {
        return false;
    }
    if (!nested_read_motorPosition(reader)) {
        return false;
    }
    if (!read_motorPosition_isValid(reader)) {
        return false;
    }
    if (!nested_read_motorVelocity(reader)) {
        return false;
    }
    if (!read_motorVelocity_isValid(reader)) {
        return false;
    }
    if (!nested_read_motorAcceleration(reader)) {
        return false;
    }
    if (!read_motorAcceleration_isValid(reader)) {
        return false;
    }
    if (!nested_read_torque(reader)) {
        return false;
    }
    if (!read_torque_isValid(reader)) {
        return false;
    }
    if (!nested_read_pwmDutycycle(reader)) {
        return false;
    }
    if (!read_pwmDutycycle_isValid(reader)) {
        return false;
    }
    if (!nested_read_current(reader)) {
        return false;
    }
    if (!read_current_isValid(reader)) {
        return false;
    }
    if (!nested_read_controlMode(reader)) {
        return false;
    }
    if (!read_controlMode_isValid(reader)) {
        return false;
    }
    if (!nested_read_interactionMode(reader)) {
        return false;
    }
    if (!read_interactionMode_isValid(reader)) {
        return false;
    }
    if (reader.isError()) {
        return false;
    }
    return true;
}

So the mapping of the nested port data blocks to the joint state components is:

0  -> joint position
1  -> isValid
2  -> joint velocity
3  -> isValid
4  -> joint acceleration
5  -> isValid
6  -> motor position
7  -> isValid
8  -> motor velocity
9  -> isValid
10 -> motor acceleration
11 -> isValid
12 -> joint torque
13 -> isValid
14 -> motor pwmDutycycle
15 -> isValid
16 -> motor current
17 -> isValid
18 -> control mode
19 -> isValid
20 -> interaction mode
21 -> isValid

and we parse the port data as follows, e.g. for left leg joint state:

this.state["sens.leftLegJointState"].jointPos.hip_pitch..ankle_roll = sensorSample[0][0]..[0][5];
this.state["sens.leftLegJointState"].jointVel.hip_pitch..ankle_roll = sensorSample[2][0]..[2][5];
this.state["sens.leftLegJointState"].jointAcc.hip_pitch..ankle_roll = sensorSample[4][0]..[4][5];
this.state["sens.leftLegJointState"].jointTrq.hip_pitch..ankle_roll = sensorSample[12][0]..[12][5];