Closed nunoguedelha closed 2 years ago
The mapping is defined in jointData.h
and jointData.cpp
which is included in the control board wrapper definition files:
jointData.h
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];
Log the
position
,velocity
,acceleration
andtorque
from the jointsstateExt:o
ports:processedConfig.js
functions toutils.js
.Implemented in #127 .