Log `joint_state` (of all joints)
Closed this issue · 1 comments
nunoguedelha commented
Log the position
, velocity
, acceleration
and torque
from the joints stateExt:o
ports:
- ✅ [utils] Minor refactoring: move
processedConfig.js
functions toutils.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 commented
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];