`motor_state` (of all motors)

nunoguedelha commented 2 years ago

Log the motors position, velocity, acceleration, current and pwmDutyCycle from the stateExt:o ports.

[x] Refactor the telemetry values templates which are to be generated from a more generic and complete template facilitating the dictionary definition for the joint state port. the outcome of this step should be an iCub Telemetry dictionary identical to the one prior the change.
[x] Extend the dictionary with motor position, velocity, acceleration, Pwm and current sub-entries (modalities).

nunoguedelha commented 2 years ago

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

... 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
...

Originally posted by @nunoguedelha in https://github.com/ami-iit/yarp-openmct/issues/122#issuecomment-1198755410

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

this.state["sens.leftLegJointState"].motorPos.hip_pitch..ankle_roll = sensorSample[6][0]..[6][5];
this.state["sens.leftLegJointState"].motorVel.hip_pitch..ankle_roll = sensorSample[8][0]..[8][5];
this.state["sens.leftLegJointState"].motorAcc.hip_pitch..ankle_roll = sensorSample[10][0]..[10][5];
this.state["sens.leftLegJointState"].motorPwm.hip_pitch..ankle_roll = sensorSample[14][0]..[14][5];
this.state["sens.leftLegJointState"].motorCurr.hip_pitch..ankle_roll = sensorSample[16][0]..[16][5];

nunoguedelha commented 2 years ago

Refactor the telemetry values templates which are to be generated from a more compact template

This facilitates the dictionary definition for the joint state port. the outcome of this step should be an iCub Telemetry dictionary identical to the one prior the change.

The template, valuesTemplateGeneratorParams, which allows to define the joint state telemetry values in a more compact way, is defined as follows:

  "valuesTemplateGeneratorParams": {
    "jointState": {
      "jointKeys": {
        "leg": [
          "hip_pitch",
          "hip_roll",
          "hip_yaw",
          "knee",
          "ankle_pitch",
          "ankle_roll"
        ],
        "arm": [
          "shoulder_pitch",
          "shoulder_roll",
          "shoulder_yaw",
          "knee",
          "wrist_prosup",
          "wrist_pitch",
          "wrist_yaw"
        ],
        "torso": [
          "torso_pitch",
          "torso_roll",
          "torso_yaw"
        ],
        "head": [
          "head_pitch",
          "head_roll",
          "head_yaw"
        ]
      },
      "mods": {
        "jointPos": {
          "namePrefix": "Pos: ",
          "unit": "deg"
        },
        "jointVel": {
          "namePrefix": "Vel: ",
          "unit": "deg/s"
        },
        "jointAcc": {
          "namePrefix": "Acc: ",
          "unit": "deg/s^2"
        },
        "jointTrq": {
          "namePrefix": "Trq: ",
          "unit": "Nm"
        }
      },
      "valuesTemplate": {
        "key": "value.${modKey}.${jointKey}",
        "name": "${modValue.namePrefix+jointName}",
        "unit": "${modValue.unit}",
        "format": "float",
        "hints": {
          "range": "${rangeIndex}"
        }
      }
    }
  }

The former valuesTemplate structure, now renamed generatedValuesBase, is generated from the above template by a function which generates the telemetry value blocks for all the relevant valuesTemplateGeneratorParams.jointState.jointKeys and valuesTemplateGeneratorParams.jointState.mods combinations ("mods" stand for modalities).

nunoguedelha commented 2 years ago

Extend the dictionary with motor position, velocity, acceleration, Pwm and current sub-entries (modalities)

This step consists in extending the structure "mods" with the following additional <key:value> pairs:

        "motorPos": "${JSON.stringify(this.valuesTemplateGeneratorParams.jointState.mods.jointPos)}",
        "motorVel": "${JSON.stringify(this.valuesTemplateGeneratorParams.jointState.mods.jointVel)}",
        "motorAcc": "${JSON.stringify(this.valuesTemplateGeneratorParams.jointState.mods.jointAcc)}",
        "motorPwr": {
          "namePrefix": "Pwr: ",
          "unit": "%"
        },
        "motorCur": {
          "namePrefix": "Cur: ",
          "unit": "A"
        }

nunoguedelha commented 2 years ago

Unit Testing

Refactor the telemetry values templates which are to be generated from a more compact template

Finishing the fix of a few bugs in this step before proceeding...

nunoguedelha commented 2 years ago

Testing and issues

After completing the development (commit 224949a53c852dbc27722e42e9001a7a17a85c66), we tested with @S-Dafarra , observed the issues listed below:

Motor telemetry data was not displayed in the visualiser plots
The motor PWM and Current data were switched in the plots.
The displayed joint positions are always the same between left and right parts, e.g. left and right legs or left and right arms, and values are oscillating between the two expected values.

nunoguedelha commented 2 years ago

Issue 1: Motor telemetry data was not displayed in the visualiser plots

I had just forgotten to update the motor telemetry data parsing in this step https://github.com/ami-iit/yarp-openmct/issues/123#issuecomment-1226939682.

Fixed in commit aee41e2c83ed70f76d58cccba95a66179cf38a18.

nunoguedelha commented 2 years ago

Issue 2: The motor PWM and Current data were switched in the plots

Motor PWM and Current positions in the parsing where properly identified in https://github.com/ami-iit/yarp-openmct/issues/123#issuecomment-1198790667 but switched in the code.

Fixed in commit 4af9060c1117f17622b97e0a8edef21564b9b0cb.

nunoguedelha commented 2 years ago

Issue 3: The displayed joint positions are always the same between left and right parts, e.g. left and right legs or left and right arms, and values are oscillating between the two expected values

While reading the port data from both legs on the terminal with yarp read, we get the hip_pitch values for both legs...

left hip_pitch ~ -3 deg
right hip_pitch ~ 14 deg

...we can see in the example screenshots below (obtained with Gazebo simulation) the visualiser plot values being always the same between the two legs, and oscillating between the two expected values.

We can conclude that the visualiser is displaying the same values for both telemetry entries and somehow mixing both ports.

Analysing...

nunoguedelha commented 2 years ago

After further analysis, we see that:

The issue is already present in https://github.com/ami-iit/yarp-openmct/pull/129, so before the changes introduced by this issue.
The parser ICubTelemetry.prototype.parseFromId is called with the same Yarp port sample for both telemetry ids sens.rightArmJointState and sens.leftArmJointState in the case of the arms, and similar thing happens for the legs.

It could be a problem with the definition of the function pointer icubtelemetry.forwardYarpDataToNotifier...

nunoguedelha commented 2 years ago

After checking again, actually the parser ICubTelemetry.prototype.parseFromId is called with the proper samples, different between the two ids sens.leftLegJointState and sens.rightLegJointState.

The problem was with the initialisation of the variable ICubTelemetry.state storing the parsed samples.

https://github.com/ami-iit/yarp-openmct/blob/ff74f1e351fe1ddb7c1139002a57acc9dbcc5eb9/iCubTelemVizServer/icubtelemetry.js#L22 https://github.com/ami-iit/yarp-openmct/blob/ff74f1e351fe1ddb7c1139002a57acc9dbcc5eb9/iCubTelemVizServer/icubtelemetry.js#L29-L35 https://github.com/ami-iit/yarp-openmct/blob/ff74f1e351fe1ddb7c1139002a57acc9dbcc5eb9/iCubTelemVizServer/icubtelemetry.js#L70

When initialising the sens.rightLegJointState nested structure, we had used the = operator to copy sens.leftLegJointState, which just sets a pointer to the later. For that reason we were always filling the same data for both legs.

This is easily fixed by using a deep copy through JSON.parse(JSON.stringify(...)).

nunoguedelha commented 2 years ago

[Issue 3] follow-up in issue https://github.com/ami-iit/yarp-openmct/issues/130. => Fixed_ in #131 . Cherry-picked to commit bfab110f51b308c4b7c93fb928c4bdc93ef7e1b9.

nunoguedelha commented 2 years ago

Ready for retest.

Steps to completion: rebase, squash and PR.

nunoguedelha commented 2 years ago

Done => #132 .

ami-iit / yarp-openmct