Closed nunoguedelha closed 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];
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).
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"
}
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...
After completing the development (commit 224949a53c852dbc27722e42e9001a7a17a85c66), we tested with @S-Dafarra , observed the issues listed below:
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.
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.
While reading the port data from both legs on the terminal with yarp read
, we get the hip_pitch
values for both legs...
...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...
After further analysis, we see that:
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
...
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(...))
.
[Issue 3] follow-up in issue https://github.com/ami-iit/yarp-openmct/issues/130. => Fixed_ in #131 . Cherry-picked to commit bfab110f51b308c4b7c93fb928c4bdc93ef7e1b9.
Ready for retest.
Steps to completion: rebase, squash and PR.
Done => #132 .
Log the motors
position
,velocity
,acceleration
,current
andpwmDutyCycle
from thestateExt:o
ports.