HighState log output example

[devemin]

Hello.

example_walk.cpp didn't have the code of checking HighState values.

So this is HighState log output example. (for exmple_walk.cpp) Best regards.

void Custom::RobotControl()
{
    motiontime += 2;
    udp.GetRecv(state);
    std::string statelog = "";

    statelog = statelog + "motiontime: (" + to_string(motiontime) + "), \n";
    statelog = statelog + "head[2]: (" + to_string(state.head[0]) + ", " + to_string(state.head[1]) + "), \n";
    statelog = statelog + "levelFlag: (" + to_string(state.levelFlag) + "), \n";
    statelog = statelog + "frameReserve: (" + to_string(state.frameReserve) + "), \n";
    statelog = statelog + "SN[2]: (" + to_string(state.SN[0]) + ", " + to_string(state.SN[1]) + "), \n";
    statelog = statelog + "version[2]: (" + to_string(state.version[0]) + ", " + to_string(state.version[1]) + "), \n";
    statelog = statelog + "bandWidth: (" + to_string(state.bandWidth) + "), \n";
    statelog = statelog + "imu.quaternion[4]: (" + to_string(state.imu.quaternion[0]) + ", " + to_string(state.imu.quaternion[1]) + ", " + to_string(state.imu.quaternion[2]) + ", " + to_string(state.imu.quaternion[3]) + "), \n";
    statelog = statelog + "imu.gyroscope[4]: (" + to_string(state.imu.gyroscope[0]) + ", " + to_string(state.imu.gyroscope[1]) + ", " + to_string(state.imu.gyroscope[2]) + ", " + to_string(state.imu.gyroscope[3]) + "), \n";
    statelog = statelog + "imu.accelerometer[3]: (" + to_string(state.imu.accelerometer[0]) + ", " + to_string(state.imu.accelerometer[1]) + ", " + to_string(state.imu.accelerometer[2]) + "), \n";
    statelog = statelog + "imu.rpy[3]: (" + to_string(state.imu.rpy[0]) + ", " + to_string(state.imu.rpy[1]) + ", " + to_string(state.imu.rpy[2]) + "), \n";
    statelog = statelog + "imu.temperature: (" + to_string(state.imu.temperature) + "), \n";

    for (int a=0; a<12; a++) {
        statelog = statelog + "motorState[" + to_string(a) + "]: (" + to_string(state.motorState[a].mode) + ", " + to_string(state.motorState[a].q) + ", " + to_string(state.motorState[a].dq) + ", " + to_string(state.motorState[a].ddq) + ", " + to_string(state.motorState[a].tauEst) + ", " + to_string(state.motorState[a].q_raw) + ", " + to_string(state.motorState[a].dq_raw) + ", " + to_string(state.motorState[a].ddq_raw) + ", " + to_string(state.motorState[a].temperature) + "), \n";
    }
    statelog = statelog + "bms: (" + to_string(state.bms.SOC) + ", " + to_string(state.bms.current) + "), \n";
    statelog = statelog + "footForce[4]: (" + to_string(state.footForce[0]) + ", " + to_string(state.footForce[1]) + ", " + to_string(state.footForce[2]) + ", " + to_string(state.footForce[3]) + "), \n";
    statelog = statelog + "footForceEst[4]: (" + to_string(state.footForceEst[0]) + ", " + to_string(state.footForceEst[1]) + ", " + to_string(state.footForceEst[2]) + ", " + to_string(state.footForceEst[3]) + "), \n";
    statelog = statelog + "mode: (" + to_string(state.mode) + "), \n";
    statelog = statelog + "progress: (" + to_string(state.progress) + "), \n";
    statelog = statelog + "gaitType: (" + to_string(state.gaitType) + "), \n";
    statelog = statelog + "footRaiseHeight: (" + to_string(state.footRaiseHeight) + "), \n";
    statelog = statelog + "position[3]: (" + to_string(state.position[0]) + ", " + to_string(state.position[1]) + ", " + to_string(state.position[2]) + "), \n";
    statelog = statelog + "bodyHeight: (" + to_string(state.bodyHeight) + "), \n";
    statelog = statelog + "velocity[3]: (" + to_string(state.velocity[0]) + ", " + to_string(state.velocity[1]) + ", " + to_string(state.velocity[2]) + "), \n";
    statelog = statelog + "yawSpeed: (" + to_string(state.yawSpeed) + "), \n";
    statelog = statelog + "rangeObstacle[4]: (" + to_string(state.rangeObstacle[0]) + ", " + to_string(state.rangeObstacle[1]) + ", " + to_string(state.rangeObstacle[2]) + ", " + to_string(state.rangeObstacle[3]) + "), \n";
    for (int a=0; a<4; a++) {
        statelog = statelog + "footPosition2Body[" + to_string(a) + "]: (" + to_string(state.footPosition2Body[a].x) + ", " + to_string(state.footPosition2Body[a].y) + ", " + to_string(state.footPosition2Body[a].z) + "), \n";
    }
    for (int a=0; a<4; a++) {
        statelog = statelog + "footSpeed2Body[" + to_string(a) + "]: (" + to_string(state.footSpeed2Body[a].x) + ", " + to_string(state.footSpeed2Body[a].y) + ", " + to_string(state.footSpeed2Body[a].z) + "), \n";
    }

    statelog = statelog + "wirelessRemote: (";
    for (int a=0; a<40; a++) {
        statelog = statelog + to_string(state.wirelessRemote[a]) + ",";
    }
    statelog = statelog + "), \n";

    printf("%s\n", statelog.c_str());

[TrivasZhang]

Thanks! Did any variety missing value at HighState?

[devemin]

Hi.

I didn't know each value is correct or not. sorry. Thanks.