void SideConfidenceProvider::update(SideConfidence& sideConfidence)
{
// Debug stuff
DECLARE_PLOT("module:SideConfidenceProvider:originalWeighting");
DECLARE_PLOT("module:SideConfidenceProvider:mirroredWeighting");
// Setting the time when the robot has a good stand
if(theFallDownState.state != theFallDownState.upright &&
theFallDownState.state != theFallDownState.undefined &&
theFallDownState.state != theFallDownState.staggering &&
theFrameInfo.getTimeSince(timeOfLastFall) > 8000)
timeOfLastFall = theFrameInfo.time;
// Setting the time when the robot not has arm contact
if(!theArmContactModel.contactLeft && !theArmContactModel.contactRight)
lastTimeWithoutArmContact = theFrameInfo.time;
// Remove old entries from confidence buffer
while(!confidenceBuffer.empty())
{
if(theFrameInfo.getTimeSince(confidenceBuffer.back().timeStamp) > maxBufferAge)
confidenceBuffer.pop_back();
else
break;
}
updateBallConfidences(sideConfidence);
updateSideConfidenceFromOthers(sideConfidence);
updateSideConfidenceFromOwn(sideConfidence);
updateConfidenceState(sideConfidence);
// MOOOOH!
if(sideConfidence.mirror)
SystemCall::playSound("theMirrorCow.wav");
}
void GroundTruthEvaluator::update(GroundTruthResult& groundTruthResult)
{
DECLARE_PLOT("module:GroundTruthEvaluator:translationalError");
DECLARE_PLOT("module:GroundTruthEvaluator:rotationalError");
DECLARE_PLOT("module:GroundTruthEvaluator:ballPositionError");
DECLARE_PLOT("module:GroundTruthEvaluator:evaluationDelay");
robotPoses.add(RepresentationWithTimestamp<RobotPose>(theRobotPose,
theFrameInfo.time));
if(groundTruthRobotPoses.getNumberOfEntries() == 0 ||
groundTruthRobotPoses.top().timestamp != theGroundTruthRobotPose.timestamp)
{
groundTruthRobotPoses.add(RepresentationWithTimestamp<RobotPose>(
theGroundTruthRobotPose, theGroundTruthRobotPose.timestamp));
}
RobotPose groundTruthRobotPose = currentGroundTruthRobotPose();
RobotPose estimatedRobotPose = averageEstimatedRobotPose();
float translationalError = (groundTruthRobotPose.translation
- estimatedRobotPose.translation).abs();
PLOT("module:GroundTruthEvaluator:translationalError", translationalError);
float rotationalError = fabs(groundTruthRobotPose.rotation
- estimatedRobotPose.rotation);
if(rotationalError > pi) // Occures around +/-pi.
rotationalError = pi2 - rotationalError;
PLOT("module:GroundTruthEvaluator:rotationalError", rotationalError);
ballModels.add(RepresentationWithTimestamp<BallModel>(theBallModel,
theFrameInfo.time));
if(groundTruthBallModels.getNumberOfEntries() == 0 ||
groundTruthBallModels.top().timestamp != theGroundTruthBallModel.timeWhenLastSeen)
{
groundTruthBallModels.add(RepresentationWithTimestamp<BallModel>(
theGroundTruthBallModel, theGroundTruthBallModel.timeWhenLastSeen));
}
BallModel groundTruthBallModel = currentGroundTruthBallModel();
BallModel estimatedBallModel = averageEstimatedBallModel();
float ballPositionError = (groundTruthBallModel.estimate.position
- estimatedBallModel.estimate.position).abs();
PLOT("module:GroundTruthEvaluator:ballPositionError", ballPositionError);
// TODO ball model velocity
// TODO RobotsModel
int evaluationDelay = 0;
if(groundTruthRobotPoses.getNumberOfEntries() >= 2)
{
evaluationDelay = (int) theFrameInfo.time - (int) groundTruthRobotPoses[1].timestamp;
}
PLOT("module:GroundTruthEvaluator:evaluationDelay", evaluationDelay);
}
void TeamDataProvider::update(TeamMateDataBH& teamMateData)
{
DECLARE_PLOT("module:TeamDataProvider:ntpOffset1"); // 1-5: players
DECLARE_PLOT("module:TeamDataProvider:ntpOffset2");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset3");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset4");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset5");
PLOT("module:TeamDataProvider:ntpOffset1", ntp.timeSyncBuffers[1].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset2", ntp.timeSyncBuffers[2].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset3", ntp.timeSyncBuffers[3].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset4", ntp.timeSyncBuffers[4].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset5", ntp.timeSyncBuffers[5].bestOffset);
teamMateData = theTeamMateDataBH;
teamMateData.currentTimestamp = theFrameInfoBH.time;
teamMateData.numOfConnectedTeamMates = 0;
teamMateData.firstTeamMate = TeamMateDataBH::numOfPlayers;
for(int i = TeamMateDataBH::firstPlayer; i < TeamMateDataBH::numOfPlayers; ++i)
{
teamMateData.isActive[i] = false;
teamMateData.isFullyActive[i] = false;
// Check, if network connection is working (-> connected):
if(teamMateData.timeStamps[i] && theFrameInfoBH.getTimeSince(teamMateData.timeStamps[i]) < static_cast<int>(teamMateData.networkTimeout))
{
teamMateData.numOfConnectedTeamMates++;
// Check, if team mate is not penalized (-> active):
if(!teamMateData.isPenalized[i] && theOwnTeamInfoBH.players[i - 1].penalty == PENALTY_NONE)
{
teamMateData.numOfActiveTeamMates++;
teamMateData.isActive[i] = true;
if(teamMateData.numOfActiveTeamMates == 1)
teamMateData.firstTeamMate = i;
// Check, if team mate has not been fallen down or lost ground contact (-> fully active):
if(teamMateData.hasGroundContact[i] && teamMateData.isUpright[i])
{
teamMateData.numOfFullyActiveTeamMates++;
teamMateData.isFullyActive[i] = true;
}
}
}
}
if(teamMateData.numOfConnectedTeamMates)
teamMateData.wasConnected = theTeamMateDataBH.wasConnected = true;
teamMateData.sendThisFrame = (ntp.doSynchronization(theFrameInfoBH.time, Global::getTeamOut()) ||
theFrameInfoBH.getTimeSince(lastSentTimeStamp) >= 200) // TODO config file?
#ifdef TARGET_ROBOT
&& !(theMotionRequestBH.motion == MotionRequestBH::specialAction && theMotionRequestBH.specialActionRequest.specialAction == SpecialActionRequest::playDead)
&& !(theMotionInfoBH.motion == MotionRequestBH::specialAction && theMotionInfoBH.specialActionRequest.specialAction == SpecialActionRequest::playDead)
#endif
;
if(teamMateData.sendThisFrame)
lastSentTimeStamp = theFrameInfoBH.time;
}
void TeamDataProvider::update(TeamMateData& teamMateData)
{
DECLARE_PLOT("module:TeamDataProvider:sslVisionOffset");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset1"); // 1-4: players
DECLARE_PLOT("module:TeamDataProvider:ntpOffset2");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset3");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset4");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset12"); // SSL vision data
PLOT("module:TeamDataProvider:ntpOffset1", ntp.timeSyncBuffers[1].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset2", ntp.timeSyncBuffers[2].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset3", ntp.timeSyncBuffers[3].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset4", ntp.timeSyncBuffers[4].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset12", ntp.timeSyncBuffers[12].bestOffset);
teamMateData = theTeamMateData;
teamMateData.numOfConnectedPlayers = 0;
teamMateData.firstTeamMate = teamMateData.secondTeamMate = teamMateData.thirdTeamMate = TeamMateData::numOfPlayers;
for(int i = TeamMateData::firstPlayer; i < TeamMateData::numOfPlayers; ++i)
if(teamMateData.timeStamps[i] && theFrameInfo.getTimeSince(teamMateData.timeStamps[i]) < networkTimeout)
{
if(!teamMateData.isPenalized[i] && teamMateData.hasGroundContact[i])
{
++teamMateData.numOfConnectedPlayers;
if(teamMateData.numOfConnectedPlayers == 1) teamMateData.firstTeamMate = i;
else if(teamMateData.numOfConnectedPlayers == 2) teamMateData.secondTeamMate = i;
else if(teamMateData.numOfConnectedPlayers == 3) teamMateData.thirdTeamMate = i;
}
else
teamMateData.timeStamps[i] = 0;
}
if(teamMateData.numOfConnectedPlayers > 0)
teamMateData.wasConnected = theTeamMateData.wasConnected = true;
teamMateData.sendThisFrame = (ntp.doSynchronization(theFrameInfo.time, Global::getTeamOut()) ||
theFrameInfo.getTimeSince(lastSentTimeStamp) >= 100) // TODO config file?
#ifdef TARGET_ROBOT
&& !(theMotionRequest.motion == MotionRequest::specialAction && theMotionRequest.specialActionRequest.specialAction == SpecialActionRequest::playDead)
&& !(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
#endif
;
if(teamMateData.sendThisFrame)
{
lastSentTimeStamp = theFrameInfo.time;
TEAM_OUTPUT(idTeamMateIsPenalized, bin, (theRobotInfo.penalty != PENALTY_NONE));
TEAM_OUTPUT(idTeamMateHasGroundContact, bin, (theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection));
TEAM_OUTPUT(idTeamMateIsUpright, bin, (theFallDownState.state == theFallDownState.upright));
if(theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection)
TEAM_OUTPUT(idTeamMateTimeSinceLastGroundContact, bin, lastSentTimeStamp);
}
}
void ArmContactModelProvider::update(ArmContactModel& model)
{
MODIFY("module:ArmContactModelProvider:parameters", p);
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
DECLARE_PLOT("module:ArmContactModelProvider:errorDurationLeft");
DECLARE_PLOT("module:ArmContactModelProvider:errorDurationRight");
DECLARE_PLOT("module:ArmContactModelProvider:errorYThreshold");
DECLARE_PLOT("module:ArmContactModelProvider:errorXThreshold");
DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
DECLARE_PLOT("module:ArmContactModelProvider:contactRight");
DECLARE_DEBUG_DRAWING("module:ArmContactModelProvider:armContact", "drawingOnField");
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 400, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 600, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 800, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1000, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
CIRCLE("module:ArmContactModelProvider:armContact", 0, 0, 1200, 30, Drawings::ps_solid, ColorClasses::blue, Drawings::ps_null, ColorClasses::blue);
/* Buffer arm angles */
struct ArmAngles angles;
angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
angleBuffer.add(angles);
/* Reset in case of a fall or penalty */
if(theFallDownState.state == FallDownState::onGround || theRobotInfo.penalty != PENALTY_NONE)
{
leftErrorBuffer.init();
rightErrorBuffer.init();
}
Pose2D odometryOffset = theOdometryData - lastOdometry;
lastOdometry = theOdometryData;
const Vector3<>& leftHandPos3D = theRobotModel.limbs[MassCalibration::foreArmLeft].translation;
Vector2<> leftHandPos(leftHandPos3D.x, leftHandPos3D.y);
Vector2<> leftHandSpeed = (odometryOffset + Pose2D(leftHandPos) - Pose2D(lastLeftHandPos)).translation / theFrameInfo.cycleTime;
float leftFactor = std::max(0.f, 1.f - leftHandSpeed.abs() / p.speedBasedErrorReduction);
lastLeftHandPos = leftHandPos;
const Vector3<>& rightHandPos3D = theRobotModel.limbs[MassCalibration::foreArmRight].translation;
Vector2<> rightHandPos(rightHandPos3D.x, rightHandPos3D.y);
Vector2<> rightHandSpeed = (odometryOffset + Pose2D(rightHandPos) - Pose2D(lastRightHandPos)).translation / theFrameInfo.cycleTime;
float rightFactor = std::max(0.f, 1.f - rightHandSpeed.abs() / p.speedBasedErrorReduction);
lastRightHandPos = rightHandPos;
/* Check for arm contact */
// motion types to take into account: stand, walk (if the robot is upright)
if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
(theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering) &&
(theGameInfo.state == STATE_PLAYING || theGameInfo.state == STATE_READY) &&
(theRobotInfo.penalty == PENALTY_NONE)) // TICKET 897: ArmContact only if robot is not penalized
{
checkArm(LEFT, leftFactor);
checkArm(RIGHT, rightFactor);
//left and right are projections of the 3 dimensional shoulder-joint vector
//onto the x-y plane.
Vector2f left = leftErrorBuffer.getAverageFloat();
Vector2f right = rightErrorBuffer.getAverageFloat();
//Determine if we are being pushed or not
bool leftX = fabs(left.x) > fromDegrees(p.errorXThreshold);
bool leftY = fabs(left.y) > fromDegrees(p.errorYThreshold);
bool rightX = fabs(right.x)> fromDegrees(p.errorXThreshold);
bool rightY = fabs(right.y)> fromDegrees(p.errorYThreshold);
// update the model
model.contactLeft = leftX || leftY;
model.contactRight = rightX || rightY;
// The duration of the contact is counted upwards as long as the error
//remains. Otherwise it is reseted to 0.
model.durationLeft = model.contactLeft ? model.durationLeft + 1 : 0;
model.durationRight = model.contactRight ? model.durationRight + 1 : 0;
model.contactLeft &= model.durationLeft < p.malfunctionThreshold;
model.contactRight &= model.durationRight < p.malfunctionThreshold;
if(model.contactLeft)
{
model.timeOfLastContactLeft = theFrameInfo.time;
}
if(model.contactRight)
{
model.timeOfLastContactRight = theFrameInfo.time;
}
model.pushDirectionLeft = getDirection(LEFT, leftX, leftY, left);
model.pushDirectionRight = getDirection(RIGHT, rightX, rightY, right);
model.lastPushDirectionLeft = model.pushDirectionLeft != ArmContactModel::NONE ? model.pushDirectionLeft : model.lastPushDirectionLeft;
model.lastPushDirectionRight = model.pushDirectionRight != ArmContactModel::NONE ? model.pushDirectionRight : model.lastPushDirectionRight;
PLOT("module:ArmContactModelProvider:errorLeftX", toDegrees(left.x));
PLOT("module:ArmContactModelProvider:errorRightX", toDegrees(right.x));
PLOT("module:ArmContactModelProvider:errorLeftY", toDegrees(left.y));
PLOT("module:ArmContactModelProvider:errorRightY", toDegrees(right.y));
PLOT("module:ArmContactModelProvider:errorDurationLeft", model.durationLeft);
PLOT("module:ArmContactModelProvider:errorDurationRight", model.durationRight);
PLOT("module:ArmContactModelProvider:errorYThreshold", toDegrees(p.errorYThreshold));
PLOT("module:ArmContactModelProvider:errorXThreshold", toDegrees(p.errorXThreshold));
PLOT("module:ArmContactModelProvider:contactLeft", model.contactLeft ? 10.0 : 0.0);
PLOT("module:ArmContactModelProvider:contactRight", model.contactRight ? 10.0 : 0.0);
ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(left.y) * SCALE), toDegrees(left.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::green);
ARROW("module:ArmContactModelProvider:armContact", 0, 0, toDegrees(right.y) * SCALE, toDegrees(right.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::red);
COMPLEX_DRAWING("module:ArmContactModelProvider:armContact",
{
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1300, 20, ColorClasses::black, "LEFT");
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(left.x));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(left.y));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionLeft));
DRAWTEXT("module:ArmContactModelProvider:armContact", -2300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactLeft);
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1300, 20, ColorClasses::black, "RIGHT");
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 1100, 20, ColorClasses::black, "ErrorX: " << toDegrees(right.x));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 900, 20, ColorClasses::black, "ErrorY: " << toDegrees(right.y));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 500, 20, ColorClasses::black, ArmContactModel::getName(model.pushDirectionRight));
DRAWTEXT("module:ArmContactModelProvider:armContact", 1300, 300, 20, ColorClasses::black, "Time: " << model.timeOfLastContactRight);
if (model.contactLeft)
{
CROSS("module:ArmContactModelProvider:armContact", -2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
}
if (model.contactRight)
{
CROSS("module:ArmContactModelProvider:armContact", 2000, 0, 100, 20, Drawings::ps_solid, ColorClasses::red);
}
});
void JointDataPrediction::draw() const
{
DECLARE_PLOT("representation:JointDataPrediction:position:lHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:position:lHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:lAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:position:rHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:position:rHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:position:rAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:lAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:velocity:rAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:lAnkleRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipYawPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipRoll");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rHipPitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rKneePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rAnklePitch");
DECLARE_PLOT("representation:JointDataPrediction:acceleration:rAnkleRoll");
PLOT("representation:JointDataPrediction:velocity:lHipYawPitch",
velocities[Joints::lHipYawPitch]);
PLOT("representation:JointDataPrediction:velocity:lHipRoll",
velocities[Joints::lHipRoll]);
PLOT("representation:JointDataPrediction:velocity:lHipPitch",
velocities[Joints::lHipPitch]);
PLOT("representation:JointDataPrediction:velocity:lKneePitch",
velocities[Joints::lKneePitch]);
PLOT("representation:JointDataPrediction:velocity:lAnklePitch",
velocities[Joints::lAnklePitch]);
PLOT("representation:JointDataPrediction:velocity:lAnkleRoll",
velocities[Joints::lAnkleRoll]);
PLOT("representation:JointDataPrediction:velocity:rHipYawPitch",
velocities[Joints::rHipYawPitch]);
PLOT("representation:JointDataPrediction:velocity:rHipRoll",
velocities[Joints::rHipRoll]);
PLOT("representation:JointDataPrediction:velocity:rHipPitch",
velocities[Joints::rHipPitch]);
PLOT("representation:JointDataPrediction:velocity:rKneePitch",
velocities[Joints::rKneePitch]);
PLOT("representation:JointDataPrediction:velocity:rAnklePitch",
velocities[Joints::rAnklePitch]);
PLOT("representation:JointDataPrediction:velocity:rAnkleRoll",
velocities[Joints::rAnkleRoll]);
PLOT("representation:JointDataPrediction:position:lHipYawPitch",
angles[Joints::lHipYawPitch]);
PLOT("representation:JointDataPrediction:position:lHipRoll",
angles[Joints::lHipRoll]);
PLOT("representation:JointDataPrediction:position:lHipPitch",
angles[Joints::lHipPitch]);
PLOT("representation:JointDataPrediction:position:lKneePitch",
angles[Joints::lKneePitch]);
PLOT("representation:JointDataPrediction:position:lAnklePitch",
angles[Joints::lAnklePitch]);
PLOT("representation:JointDataPrediction:position:lAnkleRoll",
angles[Joints::lAnkleRoll]);
PLOT("representation:JointDataPrediction:position:rHipYawPitch",
angles[Joints::rHipYawPitch]);
PLOT("representation:JointDataPrediction:position:rHipRoll",
angles[Joints::rHipRoll]);
PLOT("representation:JointDataPrediction:position:rHipPitch",
angles[Joints::rHipPitch]);
PLOT("representation:JointDataPrediction:position:rKneePitch",
angles[Joints::rKneePitch]);
PLOT("representation:JointDataPrediction:position:rAnklePitch",
angles[Joints::rAnklePitch]);
PLOT("representation:JointDataPrediction:position:rAnkleRoll",
angles[Joints::rAnkleRoll]);
PLOT("representation:JointDataPrediction:acceleration:lHipYawPitch",
accelerations[Joints::lHipYawPitch]);
PLOT("representation:JointDataPrediction:acceleration:lHipRoll",
accelerations[Joints::lHipRoll]);
PLOT("representation:JointDataPrediction:acceleration:lHipPitch",
accelerations[Joints::lHipPitch]);
PLOT("representation:JointDataPrediction:acceleration:lKneePitch",
accelerations[Joints::lKneePitch]);
PLOT("representation:JointDataPrediction:acceleration:lAnklePitch",
accelerations[Joints::lAnklePitch]);
PLOT("representation:JointDataPrediction:acceleration:lAnkleRoll",
accelerations[Joints::lAnkleRoll]);
PLOT("representation:JointDataPrediction:acceleration:rHipYawPitch",
accelerations[Joints::rHipYawPitch]);
PLOT("representation:JointDataPrediction:acceleration:rHipRoll",
accelerations[Joints::rHipRoll]);
PLOT("representation:JointDataPrediction:acceleration:rHipPitch",
accelerations[Joints::rHipPitch]);
PLOT("representation:JointDataPrediction:acceleration:rKneePitch",
accelerations[Joints::rKneePitch]);
PLOT("representation:JointDataPrediction:acceleration:rAnklePitch",
accelerations[Joints::rAnklePitch]);
PLOT("representation:JointDataPrediction:acceleration:rAnkleRoll",
accelerations[Joints::rAnkleRoll]);
DECLARE_DEBUG_DRAWING3D("representation:JointDataPrediction:com", "robot");
CROSS3D("representation:JointDataPrediction:com", com.x(), com.y(), com.z(),
50, 50, ColorRGBA::red);
LINE3D("representation:JointDataPrediction:com", com.x(), com.y(), com.z(),
com.x() + comAcceleration.x(), com.y() + comAcceleration.y(),
com.z() + comAcceleration.z(), 10, ColorRGBA::red);
DECLARE_PLOT("representation:JointDataPrediction:comAccX");
DECLARE_PLOT("representation:JointDataPrediction:comAccY");
DECLARE_PLOT("representation:JointDataPrediction:comAccZ");
DECLARE_PLOT("representation:JointDataPrediction:comX");
DECLARE_PLOT("representation:JointDataPrediction:comY");
DECLARE_PLOT("representation:JointDataPrediction:comAtan2");
DECLARE_PLOT("representation:JointDataPrediction:comAccAtan2");
PLOT("representation:JointDataPrediction:comAccX", comAcceleration.x());
PLOT("representation:JointDataPrediction:comAccY", comAcceleration.y());
PLOT("representation:JointDataPrediction:comAccZ", comAcceleration.z());
PLOT("representation:JointDataPrediction:comAccAtan2", atan2(comAcceleration.y(), comAcceleration.x()));
PLOT("representation:JointDataPrediction:comX", com.x());
PLOT("representation:JointDataPrediction:comY", com.y());
PLOT("representation:JointDataPrediction:comAtan2", atan2(com.y(), com.x()));
DECLARE_PLOT("representation:JointDataPrediction:angleSum");
float sum = 0.0f;
for(int i = 0; i < Joints::numOfJoints; ++i)
{
sum += angles[i];
}
PLOT("representation:JointDataPrediction:angleSum", sum);
DECLARE_PLOT("representation:JointDataPrediction:squareVelSum");
float velSum = 0.0f;
for(int i = 0; i < Joints::numOfJoints; ++i)
{
velSum += velocities[i] * velocities[i];
}
PLOT("representation:JointDataPrediction:squareVelSum", velSum);
}
void ExpGroundContactDetector::update(GroundContactState& groundContactState)
{
MODIFY("module:ExpGroundContactDetector:parameters", p);
DECLARE_PLOT("module:ExpGroundContactDetector:angleNoiseX");
DECLARE_PLOT("module:ExpGroundContactDetector:angleNoiseY");
DECLARE_PLOT("module:ExpGroundContactDetector:accNoiseX");
DECLARE_PLOT("module:ExpGroundContactDetector:accNoiseY");
DECLARE_PLOT("module:ExpGroundContactDetector:accNoiseZ");
DECLARE_PLOT("module:ExpGroundContactDetector:gyroNoiseX");
DECLARE_PLOT("module:ExpGroundContactDetector:gyroNoiseY");
MODIFY("module:ExpGroundContactDetector:contact", contact);
bool ignoreSensors = (theMotionInfo.motion != MotionRequest::walk && theMotionInfo.motion != MotionRequest::stand) ||
(theMotionRequest.motion != MotionRequest::walk && theMotionRequest.motion != MotionRequest::stand) ||
(theMotionInfo.motion == MotionRequest::walk && theMotionInfo.walkRequest.kickType != WalkRequest::none);
if(!ignoreSensors)
{
if(contact)
{
if(theInertiaSensorData.acc.z != InertiaSensorData::off)
calibratedAccZValues.add(theInertiaSensorData.acc.z);
Vector3<> angleDiff = ((const RotationMatrix&)(theTorsoMatrix.rotation * expectedRotationInv)).getAngleAxis();
angleNoises.add(Vector2<>(sqr(angleDiff.x), sqr(angleDiff.y)));
Vector2<> angleNoise = angleNoises.getAverage();
PLOT("module:ExpGroundContactDetector:angleNoiseX", angleNoise.x);
PLOT("module:ExpGroundContactDetector:angleNoiseY", angleNoise.y);
if(!useAngle && angleNoises.isFull() && angleNoise.x < p.contactAngleActivationNoise && angleNoise.y < p.contactAngleActivationNoise)
useAngle = true;
if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)) ||
(calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
{
/*
if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)))
OUTPUT_ERROR("lost ground contact via angle");
if((calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
OUTPUT_ERROR("lost ground contact via acc");
*/
contact = false;
accNoises.clear();
gyroNoises.clear();
accValues.clear();
gyroValues.clear();
angleNoises.clear();
#ifndef TARGET_SIM
if(contactStartTime != 0)
SoundPlayer::play("high.wav");
#endif
}
}
else
{
Vector3<> accAverage = accValues.getAverage();
Vector2<> gyroAverage = gyroValues.getAverage();
if(theInspectedInertiaSensorData.acc.x != InertiaSensorData::off)
{
accValues.add(theInspectedInertiaSensorData.acc);
gyroValues.add(theInspectedInertiaSensorData.gyro);
if(accValues.isFull())
{
accNoises.add(Vector3<>(sqr(theInspectedInertiaSensorData.acc.x - accAverage.x), sqr(theInspectedInertiaSensorData.acc.y - accAverage.y), sqr(theInspectedInertiaSensorData.acc.z - accAverage.z)));
gyroNoises.add(Vector2<>(sqr(theInspectedInertiaSensorData.gyro.x - gyroAverage.x), sqr(theInspectedInertiaSensorData.gyro.y - gyroAverage.y)));
}
}
Vector3<> accNoise = accNoises.getAverage();
Vector2<> gyroNoise = gyroNoises.getAverage();
PLOT("module:ExpGroundContactDetector:accNoiseX", accNoise.x);
PLOT("module:ExpGroundContactDetector:accNoiseY", accNoise.y);
PLOT("module:ExpGroundContactDetector:accNoiseZ", accNoise.z);
PLOT("module:ExpGroundContactDetector:gyroNoiseX", gyroNoise.x);
PLOT("module:ExpGroundContactDetector:gyroNoiseY", gyroNoise.y);
if(accNoises.isFull() &&
accAverage.z < -5.f && std::abs(accAverage.x) < 5.f && std::abs(accAverage.y) < 5.f &&
accNoise.x < p.noContactMinAccXNoise && accNoise.y < p.noContactMinAccYNoise && accNoise.z < p.noContactMinAccZNoise &&
gyroNoise.x < p.noContactMinGyroNoise && gyroNoise.y < p.noContactMinGyroNoise)
{
contact = true;
useAngle = false;
contactStartTime = theFrameInfo.time;
angleNoises.clear();
calibratedAccZValues.clear();
}
}
}
groundContactState.contact = contact;
expectedRotationInv = theRobotModel.limbs[MassCalibration::footLeft].translation.z > theRobotModel.limbs[MassCalibration::footRight].translation.z ? theRobotModel.limbs[MassCalibration::footLeft].rotation : theRobotModel.limbs[MassCalibration::footRight].rotation;
}
void GroundContactDetector::update(GroundContactState& groundContactState)
{
DECLARE_PLOT("module:GroundContactDetector:angleNoiseX");
DECLARE_PLOT("module:GroundContactDetector:angleNoiseY");
DECLARE_PLOT("module:GroundContactDetector:accNoiseX");
DECLARE_PLOT("module:GroundContactDetector:accNoiseY");
DECLARE_PLOT("module:GroundContactDetector:accNoiseZ");
DECLARE_PLOT("module:GroundContactDetector:gyroNoiseX");
DECLARE_PLOT("module:GroundContactDetector:gyroNoiseY");
MODIFY("module:GroundContactDetector:contact", contact);
if(theMotionInfo.motion == MotionRequest::getUp) //hack to avoid long pause after get up
{
contact = true;
useAngle = false;
groundContactState.contact = contact;
contactStartTime = theFrameInfo.time;
}
bool ignoreSensors = (theMotionInfo.motion != MotionRequest::walk && theMotionInfo.motion != MotionRequest::stand &&
theMotionInfo.motion != MotionRequest::specialAction && theMotionInfo.motion != MotionRequest::getUp) ||
(theMotionRequest.motion != MotionRequest::walk && theMotionRequest.motion != MotionRequest::stand &&
theMotionRequest.motion != MotionRequest::specialAction && theMotionRequest.motion != MotionRequest::getUp) ||
(theMotionInfo.motion == MotionRequest::walk && theMotionInfo.walkRequest.kickType != WalkRequest::none) ||
(theMotionRequest.motion == MotionRequest::walk && theMotionRequest.walkRequest.kickType != WalkRequest::none);
if(!ignoreSensors)
{
if(contact)
{
calibratedAccZValues.add(theSensorData.data[SensorData::accZ]);
Vector3<> angleDiff = ((const RotationMatrix&)(theTorsoMatrix.rotation * expectedRotationInv)).getAngleAxis();
angleNoises.add(Vector2<>(sqr(angleDiff.x), sqr(angleDiff.y)));
Vector2<> angleNoise = angleNoises.getAverage();
PLOT("module:GroundContactDetector:angleNoiseX", angleNoise.x);
PLOT("module:GroundContactDetector:angleNoiseY", angleNoise.y);
if(!useAngle && angleNoises.isFull() && angleNoise.x < contactAngleActivationNoise && angleNoise.y < contactAngleActivationNoise)
useAngle = true;
if((useAngle && (angleNoise.x > contactMaxAngleNoise || angleNoise.y > contactMaxAngleNoise)) ||
(calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > contactMaxAccZ))
{
/*
if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)))
OUTPUT_ERROR("lost ground contact via angle");
if((calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
OUTPUT_ERROR("lost ground contact via acc");
*/
contact = false;
accNoises.clear();
gyroNoises.clear();
accValues.clear();
gyroValues.clear();
angleNoises.clear();
if(SystemCall::getMode() == SystemCall::physicalRobot && contactStartTime != 0)
SystemCall::playSound("high.wav");
}
}
else
{
const Vector3<> accAverage = accValues.getAverage();
const Vector2<> gyroAverage = gyroValues.getAverage();
const Vector2<> gyro = Vector2<>(theSensorData.data[SensorData::gyroX], theSensorData.data[SensorData::gyroY]);
const Vector3<> acc = Vector3<>(theSensorData.data[SensorData::accX], theSensorData.data[SensorData::accY], theSensorData.data[SensorData::accZ]);
accValues.add(acc);
gyroValues.add(gyro);
if(accValues.isFull())
{
accNoises.add(Vector3<>(sqr(acc.x - accAverage.x), sqr(acc.y - accAverage.y), sqr(acc.z - accAverage.z)));
gyroNoises.add(Vector2<>(sqr(gyro.x - gyroAverage.x), sqr(gyro.y - gyroAverage.y)));
}
Vector3<> accNoise = accNoises.getAverage();
Vector2<> gyroNoise = gyroNoises.getAverage();
PLOT("module:GroundContactDetector:accNoiseX", accNoise.x);
PLOT("module:GroundContactDetector:accNoiseY", accNoise.y);
PLOT("module:GroundContactDetector:accNoiseZ", accNoise.z);
PLOT("module:GroundContactDetector:gyroNoiseX", gyroNoise.x);
PLOT("module:GroundContactDetector:gyroNoiseY", gyroNoise.y);
if(accNoises.isFull() &&
accAverage.z < -5.f && std::abs(accAverage.x) < 5.f && std::abs(accAverage.y) < 5.f &&
accNoise.x < noContactMinAccNoise && accNoise.y < noContactMinAccNoise && accNoise.z < noContactMinAccNoise &&
gyroNoise.x < noContactMinGyroNoise && gyroNoise.y < noContactMinGyroNoise)
{
contact = true;
useAngle = false;
contactStartTime = theFrameInfo.time;
angleNoises.clear();
calibratedAccZValues.clear();
}
}
}
groundContactState.contact = contact;
expectedRotationInv = theRobotModel.limbs[MassCalibration::footLeft].translation.z > theRobotModel.limbs[MassCalibration::footRight].translation.z ? theRobotModel.limbs[MassCalibration::footLeft].rotation : theRobotModel.limbs[MassCalibration::footRight].rotation;
}
void FootContactModelProvider::update(FootContactModel& model)
{
MODIFY("module:FootContactModelProvider:parameters", p);
// Check, if any bumper is pressed
bool leftFootLeft = checkContact(KeyStates::leftFootLeft, leftFootLeftDuration);
bool leftFootRight = checkContact(KeyStates::leftFootRight, leftFootRightDuration);
bool rightFootLeft = checkContact(KeyStates::rightFootLeft, rightFootLeftDuration);
bool rightFootRight = checkContact(KeyStates::rightFootRight, rightFootRightDuration);
// Update statistics
if (leftFootLeft || leftFootRight)
{
contactBufferLeft.add(1);
contactDurationLeft++;
}
else
{
contactBufferLeft.add(0);
contactDurationLeft = 0;
}
if (rightFootLeft || rightFootRight)
{
contactBufferRight.add(1);
contactDurationRight++;
}
else
{
contactBufferRight.add(0);
contactDurationRight = 0;
}
// Generate model
if ((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
(theGameInfo.state == STATE_READY || theGameInfo.state == STATE_SET || theGameInfo.state == STATE_PLAYING) && //The bumper is used for configuration in initial
(theFallDownState.state == FallDownState::upright))
{
if(contactBufferLeft.getSum() > p.contactThreshold)
{
model.contactLeft = true;
model.contactDurationLeft = contactDurationLeft;
model.lastContactLeft = theFrameInfo.time;
}
else
{
model.contactLeft = false;
model.contactDurationLeft = 0;
}
if(contactBufferRight.getSum() > p.contactThreshold)
{
model.contactRight = true;
model.contactDurationRight = contactDurationRight;
model.lastContactRight = theFrameInfo.time;
}
else
{
model.contactRight = false;
model.contactDurationRight = 0;
}
}
else
{
model.contactLeft = false;
model.contactRight = false;
model.contactDurationLeft = 0;
model.contactDurationRight = 0;
}
// Debugging stuff:
if(p.debug && theFrameInfo.getTimeSince(lastSoundTime) > (int)p.soundDelay &&
(model.contactLeft || model.contactRight))
{
lastSoundTime = theFrameInfo.time;
SoundPlayer::play("doh.wav");
}
DECLARE_PLOT("module:FootContactModelProvider:sumLeft");
DECLARE_PLOT("module:FootContactModelProvider:durationLeft");
DECLARE_PLOT("module:FootContactModelProvider:contactLeft");
DECLARE_PLOT("module:FootContactModelProvider:sumRight");
DECLARE_PLOT("module:FootContactModelProvider:durationRight");
DECLARE_PLOT("module:FootContactModelProvider:contactRight");
DECLARE_PLOT("module:FootContactModelProvider:leftFootLeft");
DECLARE_PLOT("module:FootContactModelProvider:leftFootRight");
DECLARE_PLOT("module:FootContactModelProvider:rightFootLeft");
DECLARE_PLOT("module:FootContactModelProvider:rightFootRight");
PLOT("module:FootContactModelProvider:sumLeft", contactBufferLeft.getSum());
PLOT("module:FootContactModelProvider:durationLeft", contactDurationLeft);
PLOT("module:FootContactModelProvider:sumRight", contactBufferRight.getSum());
PLOT("module:FootContactModelProvider:durationRight", contactDurationRight);
PLOT("module:FootContactModelProvider:contactLeft", model.contactLeft ? 10 : 0);
PLOT("module:FootContactModelProvider:contactRight", model.contactRight ? 10 : 0);
PLOT("module:FootContactModelProvider:leftFootLeft", leftFootLeft ? 10 : 0);
PLOT("module:FootContactModelProvider:leftFootRight", leftFootRight ? 10 : 0);
PLOT("module:FootContactModelProvider:rightFootLeft", rightFootLeft ? 10 : 0);
PLOT("module:FootContactModelProvider:rightFootRight", rightFootRight ? 10 : 0);
}
void GroundContactDetector::update(GroundContactState& groundContactState)
{
DECLARE_PLOT("module:GroundContactDetector:angleNoiseX");
DECLARE_PLOT("module:GroundContactDetector:angleNoiseY");
DECLARE_PLOT("module:GroundContactDetector:accNoiseX");
DECLARE_PLOT("module:GroundContactDetector:accNoiseY");
DECLARE_PLOT("module:GroundContactDetector:accNoiseZ");
DECLARE_PLOT("module:GroundContactDetector:gyroNoiseX");
DECLARE_PLOT("module:GroundContactDetector:gyroNoiseY");
MODIFY("module:GroundContactDetector:contact", contact);
if(theMotionInfo.motion == MotionRequest::getUp) //hack to avoid long pause after get up
{
contact = true;
useAngle = false;
groundContactState.contact = contact;
contactStartTime = theFrameInfo.time;
}
bool ignoreSensors = (theMotionInfo.motion != MotionRequest::walk && theMotionInfo.motion != MotionRequest::stand &&
theMotionInfo.motion != MotionRequest::specialAction && theMotionInfo.motion != MotionRequest::getUp) ||
(theMotionRequest.motion != MotionRequest::walk && theMotionRequest.motion != MotionRequest::stand &&
theMotionRequest.motion != MotionRequest::specialAction && theMotionRequest.motion != MotionRequest::getUp) ||
(theMotionInfo.motion == MotionRequest::walk && theMotionInfo.walkRequest.kickType != WalkRequest::none) ||
(theMotionRequest.motion == MotionRequest::walk && theMotionRequest.walkRequest.kickType != WalkRequest::none) ||
(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction != SpecialActionRequest::standHigh) ||
(theMotionRequest.motion == MotionRequest::specialAction && theMotionRequest.specialActionRequest.specialAction != SpecialActionRequest::standHigh);
if(!ignoreSensors)
{
if(contact)
{
calibratedAccZValues.push_front(theInertialData.acc.z());
Vector3f angleDiff = (theTorsoMatrix.rotation * expectedRotationInv).getPackedAngleAxis();
angleNoises.push_front(Vector2f(sqr(angleDiff.x()), sqr(angleDiff.y())));
Vector2f angleNoise = angleNoises.average();
PLOT("module:GroundContactDetector:angleNoiseX", angleNoise.x());
PLOT("module:GroundContactDetector:angleNoiseY", angleNoise.y());
if(!useAngle && angleNoises.full() && angleNoise.x() < contactAngleActivationNoise && angleNoise.y() < contactAngleActivationNoise)
useAngle = true;
if((useAngle && (angleNoise.x() > contactMaxAngleNoise || angleNoise.y() > contactMaxAngleNoise)) ||
(calibratedAccZValues.full() && calibratedAccZValues.average() > contactMaxAccZ))
{
/*
if((useAngle && (angleNoise.x > p.contactMaxAngleNoise || angleNoise.y > p.contactMaxAngleNoise)))
OUTPUT_ERROR("lost ground contact via angle");
if((calibratedAccZValues.isFull() && calibratedAccZValues.getAverage() > p.contactMaxAccZ))
OUTPUT_ERROR("lost ground contact via acc");
*/
contact = false;
accNoises.clear();
gyroNoises.clear();
accValues.clear();
gyroValues.clear();
angleNoises.clear();
if(SystemCall::getMode() == SystemCall::physicalRobot && contactStartTime != 0)
SystemCall::playSound("high.wav");
}
}
else
{
const Vector3f accAverage = accValues.average();
const Vector2f gyroAverage = gyroValues.average();
const Vector2f gyro = theInertialData.gyro.head<2>().cast<float>();
const Vector3f acc = theInertialData.acc.cast<float>();
accValues.push_front(acc);
gyroValues.push_front(gyro);
if(accValues.full())
{
accNoises.push_front((acc - accAverage).array().square());
gyroNoises.push_front((gyro - gyroAverage).array().square());
}
Vector3f accNoise = accNoises.average();
Vector2f gyroNoise = gyroNoises.average();
PLOT("module:GroundContactDetector:accNoiseX", accNoise.x());
PLOT("module:GroundContactDetector:accNoiseY", accNoise.y());
PLOT("module:GroundContactDetector:accNoiseZ", accNoise.z());
PLOT("module:GroundContactDetector:gyroNoiseX", gyroNoise.x());
PLOT("module:GroundContactDetector:gyroNoiseY", gyroNoise.y());
if(accNoises.full() &&
std::abs(accAverage.z() - Constants::g_1000) < 5.f && std::abs(accAverage.x()) < 5.f && std::abs(accAverage.y()) < 5.f &&
accNoise.x() < noContactMinAccNoise && accNoise.y() < noContactMinAccNoise && accNoise.z() < noContactMinAccNoise &&
gyroNoise.x() < noContactMinGyroNoise && gyroNoise.y() < noContactMinGyroNoise)
{
contact = true;
useAngle = false;
contactStartTime = theFrameInfo.time;
angleNoises.clear();
calibratedAccZValues.clear();
}
}
}
groundContactState.contact = contact;
expectedRotationInv = theRobotModel.limbs[Limbs::footLeft].translation.z() > theRobotModel.limbs[Limbs::footRight].translation.z() ? theRobotModel.limbs[Limbs::footLeft].rotation : theRobotModel.limbs[Limbs::footRight].rotation;
}
void InertialDataFilter::update(InertialData& inertialData)
{
DECLARE_PLOT("module:InertialDataFilter:expectedAccX");
DECLARE_PLOT("module:InertialDataFilter:accX");
DECLARE_PLOT("module:InertialDataFilter:expectedAccY");
DECLARE_PLOT("module:InertialDataFilter:accY");
DECLARE_PLOT("module:InertialDataFilter:expectedAccZ");
DECLARE_PLOT("module:InertialDataFilter:accZ");
// check whether the filter shall be reset
if(!lastTime || theFrameInfo.time <= lastTime)
{
if(theFrameInfo.time == lastTime)
return; // weird log file replaying?
reset();
}
if(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
{
reset();
}
// get foot positions
Pose3f leftFoot = theRobotModel.limbs[Limbs::footLeft];
Pose3f rightFoot = theRobotModel.limbs[Limbs::footRight];
leftFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
rightFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight);
const Pose3f leftFootInvert(leftFoot.inverse());
const Pose3f rightFootInvert(rightFoot.inverse());
// calculate rotation and position offset using the robot model (joint data)
const Pose3f leftOffset(lastLeftFoot.translation.z() != 0.f ? Pose3f(lastLeftFoot).conc(leftFootInvert) : Pose3f());
const Pose3f rightOffset(lastRightFoot.translation.z() != 0.f ? Pose3f(lastRightFoot).conc(rightFootInvert) : Pose3f());
// detect the foot that is on ground
bool useLeft = true;
if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x() != 0)
{
useLeft = theWalkingEngineOutput.speed.translation.x() > 0 ?
(leftOffset.translation.x() > rightOffset.translation.x()) :
(leftOffset.translation.x() < rightOffset.translation.x());
}
else
{
Pose3f left(mean.rotation);
Pose3f right(mean.rotation);
left.conc(leftFoot);
right.conc(rightFoot);
useLeft = left.translation.z() < right.translation.z();
}
// update the filter
const float timeScale = theFrameInfo.cycleTime;
predict(RotationMatrix::fromEulerAngles(theInertialSensorData.gyro.x() * timeScale,
theInertialSensorData.gyro.y() * timeScale, 0));
// insert calculated rotation
safeRawAngle = theInertialSensorData.angle.head<2>().cast<float>();
bool useFeet = true;
MODIFY("module:InertialDataFilter:useFeet", useFeet);
if(useFeet &&
(theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand ||
(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::standHigh)) &&
std::abs(safeRawAngle.x()) < calculatedAccLimit.x() && std::abs(safeRawAngle.y()) < calculatedAccLimit.y())
{
const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation);
Vector3f accGravOnly(usedRotation.col(0).z(), usedRotation.col(1).z(), usedRotation.col(2).z());
accGravOnly *= Constants::g_1000;
readingUpdate(accGravOnly);
}
else // insert acceleration sensor values
readingUpdate(theInertialSensorData.acc);
// fill the representation
inertialData.angle = Vector2a(std::atan2(mean.rotation.col(1).z(), mean.rotation.col(2).z()), std::atan2(-mean.rotation.col(0).z(), mean.rotation.col(2).z()));
inertialData.acc = theInertialSensorData.acc;
inertialData.gyro = theInertialSensorData.gyro;
inertialData.orientation = Rotation::removeZRotation(Quaternionf(mean.rotation));
// this keeps the rotation matrix orthogonal
mean.rotation.normalize();
// store some data for the next iteration
lastLeftFoot = leftFoot;
lastRightFoot = rightFoot;
lastTime = theFrameInfo.time;
// plots
Vector3f angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(inertialData.orientation));
PLOT("module:InertialDataFilter:angleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:angleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:angleZ", toDegrees(angleAxisVec.z()));
PLOT("module:InertialDataFilter:unfilteredAngleX", theInertialSensorData.angle.x().toDegrees());
PLOT("module:InertialDataFilter:unfilteredAngleY", theInertialSensorData.angle.y().toDegrees());
angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(mean.rotation));
PLOT("module:InertialDataFilter:interlanAngleX", toDegrees(angleAxisVec.x()));
PLOT("module:InertialDataFilter:interlanAngleY", toDegrees(angleAxisVec.y()));
PLOT("module:InertialDataFilter:interlanAngleZ", toDegrees(angleAxisVec.z()));
}
void JointDynamicsProvider::update(JointDynamicsBH& jointDynamics)
{
DECLARE_PLOT("module:JointDynamicsProvider:leftHipPitchPosition");
DECLARE_PLOT("module:JointDynamicsProvider:leftHipPitchVelocity");
DECLARE_PLOT("module:JointDynamicsProvider:leftHipPitchAcceleration");
DECLARE_PLOT("module:JointDynamicsProvider:leftHipPitchPositionModel");
DECLARE_PLOT("module:JointDynamicsProvider:leftHipPitchVelocityModel");
DECLARE_PLOT("module:JointDynamicsProvider:gyroXMeasure");
DECLARE_PLOT("module:JointDynamicsProvider:gyroXModel");
DECLARE_PLOT("module:JointDynamicsProvider:gyroYMeasure");
DECLARE_PLOT("module:JointDynamicsProvider:gyroYModel");
DECLARE_PLOT("module:JointDynamicsProvider:supportAnkleRollDifference");
if(theFrameInfoBH.cycleTime != cycleTime)
{
cycleTime = theFrameInfoBH.cycleTime;
assembleTimeDependentMatrices();
}
/* Try to set the support leg. */
if(theMotionRequestBH.motion == MotionRequestBH::indykick)
jointDynamics.supportLeg = theMotionRequestBH.indykickRequest.supportLeg;
else if(theMotionInfoBH.motion == MotionRequestBH::indykick)
jointDynamics.supportLeg = theIndykickEngineOutputBH.executedIndykickRequest.supportLeg;
else
jointDynamics.supportLeg = IndykickRequest::unspecified;
if(covariancesNeedUpdate)
{
assembleCovariances();
covariancesNeedUpdate = false;
}
jointRequestsBuffer.add(theJointRequestBH);
if(theMotionInfoBH.motion == MotionRequestBH::indykick && theIndykickEngineOutputBH.useGyroCorrection)
for(int i = 0; i < JointDataBH::LAnkleRoll - JointDataBH::LHipYawPitch + 1; ++i)
{
JointRequestBH& request = jointRequestsBuffer[0];
request.angles[JointDataBH::LHipYawPitch + i] = theIndykickEngineOutputBH.notGyroCorrectedLeftLegAngles[i];
request.angles[JointDataBH::RHipYawPitch + i] = theIndykickEngineOutputBH.notGyroCorrectedRightLegAngles[i];
}
if(jointRequestsBuffer.getNumberOfEntries() < maxFrameDelay)
{
jointDynamicsInitialized = false;
}
else if(!jointDynamicsInitialized)
{
for(int i = 0; i < numOfNonLegJoints; ++i)
{
Eigen::Vector3f& s = stateNoLeg[i];
for(int j = 0; j < 3; ++j)
s[j] = 0.0f;
covarianceNoLeg[i] = smallCovarianceProcess * 2.0f;
}
for(int i = 0; i < numOfLegJoints; ++i)
{
const int index = i * 3;
for(int j = 0; j < 3; ++j)
{
stateLeftLeg[index + j] = 0.0f;
stateRightLeg[index + j] = 0.0f;
}
covarianceLeftLeg = covarianceProcess * 2.0f;
covarianceRightLeg = covarianceProcess * 2.0f;
}
jointDynamicsInitialized = true;
}
else
{
ASSERT(jointRequestsBuffer.getNumberOfEntries() >= maxFrameDelay);
const JointRequestBH& next = jointRequestsBuffer[frameDelay];
STOP_TIME_ON_REQUEST("module:JointDynamicsProvider:applyGyroMeasurementModel", applyGyroMeasurementModel(jointDynamics););
STOP_TIME_ON_REQUEST("module:JointDynamicsProvider:updateModel", updateModel(next, model, velocity););
bool TeamRobot::main()
{
{
SYNC;
OUTPUT(idProcessBegin, bin, 't');
DECLARE_DEBUG_DRAWING("representation:RobotPose", "drawingOnField"); // The robot pose
DECLARE_DEBUG_DRAWING("representation:RobotPose:deviation", "drawingOnField"); // The robot pose
DECLARE_DEBUG_DRAWING("origin:RobotPose", "drawingOnField"); // Set the origin to the robot's current position
DECLARE_DEBUG_DRAWING("representation:BallModel", "drawingOnField"); // drawing of the ball model
DECLARE_DEBUG_DRAWING("representation:CombinedWorldModel", "drawingOnField"); // drawing of the combined world model
DECLARE_DEBUG_DRAWING("representation:RobotsModel:robots", "drawingOnField"); // drawing of the robots model a
DECLARE_DEBUG_DRAWING("representation:RobotsModel:covariance", "drawingOnField"); // drawing of the robots model b
DECLARE_DEBUG_DRAWING("representation:GoalPercept:Field", "drawingOnField"); // drawing of the goal percept
DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector
DECLARE_DEBUG_DRAWING("representation:FreePartOfOpponentGoalModel", "drawingOnField"); // drawing of free part
DECLARE_DEBUG_DRAWING("representation:ObstacleModelReduced", "drawingOnField"); // drawing of obstacle model
DECLARE_DEBUG_DRAWING("representation:LinePercept:Field", "drawingOnField");
DECLARE_PLOT("tgb");
DECLARE_PLOT("tob");
int teamColor = 0,
swapSides = 0;
MODIFY("teamColor", teamColor);
MODIFY("swapSides", swapSides);
if(SystemCall::getTimeSince(robotPoseReceived) < 1000)
robotPose.draw(!teamColor);
if(SystemCall::getTimeSince(robotsModelReceived) < 1000)
robotsModel.draw();
if(SystemCall::getTimeSince(ballModelReceived) < 1000)
ballModel.draw();
if(SystemCall::getTimeSince(combinedWorldModelReceived) < 1000)
combinedWorldModel.draw();
if(SystemCall::getTimeSince(goalPerceptReceived) < 1000)
goalPercept.draw();
if(SystemCall::getTimeSince(motionRequestReceived) < 1000)
motionRequest.draw();
if(SystemCall::getTimeSince(obstacleModelReceived) < 1000)
obstacleModel.draw();
if(SystemCall::getTimeSince(freePartOfOpponentGoalModelReceived) < 1000)
freePartOfOpponentGoalModel.draw();
if(SystemCall::getTimeSince(linePerceptReceived) < 1000)
linePercept.drawOnField(fieldDimensions, 0);
if(swapSides ^ teamColor)
{
ORIGIN("field polygons", 0, 0, pi2); // hack: swap sides!
}
fieldDimensions.draw();
fieldDimensions.drawPolygons(teamColor);
DECLARE_DEBUG_DRAWING("robotState", "drawingOnField"); // text decribing the state of the robot
int lineY = 3550;
DRAWTEXT("robotState", -5100, lineY, 150, ColorClasses::white, "batteryLevel: " << robotHealth.batteryLevel << " %");
DRAWTEXT("robotState", 3700, lineY, 150, ColorClasses::white, "role: " << BehaviorStatus::getName(behaviorStatus.role));
lineY -= 180;
DRAWTEXT("robotState", -5100, lineY, 150, ColorClasses::white, "temperatures: joint: " << robotHealth.maxJointTemperature << " C, cpu: " << robotHealth.cpuTemperature << " C, board: " << robotHealth.boardTemperature << " C");
lineY -= 180;
DRAWTEXT("robotState", -5100, lineY, 150, ColorClasses::white, "rates: cognition: " << roundNumberToInt(robotHealth.cognitionFrameRate) << " fps, motion: " << roundNumberToInt(robotHealth.motionFrameRate) << " fps");
if(ballModel.timeWhenLastSeen)
{
DRAWTEXT("robotState", 3700, lineY, 150, ColorClasses::white, "ballLastSeen: " << SystemCall::getRealTimeSince(ballModel.timeWhenLastSeen) << " ms");
}
else
{
DRAWTEXT("robotState", 3700, lineY, 150, ColorClasses::white, "ballLastSeen: never");
}
//DRAWTEXT("robotState", -5100, lineY, 150, ColorClasses::white, "ballPercept: " << ballModel.lastPerception.position.x << ", " << ballModel.lastPerception.position.y);
lineY -= 180;
DRAWTEXT("robotState", -5100, lineY, 150, ColorClasses::white, "memory usage: " << robotHealth.memoryUsage << " %");
if(goalPercept.timeWhenGoalPostLastSeen)
{
DRAWTEXT("robotState", 3700, lineY, 150, ColorClasses::white, "goalLastSeen: " << SystemCall::getRealTimeSince(goalPercept.timeWhenGoalPostLastSeen) << " ms");
}
else
{
DRAWTEXT("robotState", 3700, lineY, 150, ColorClasses::white, "goalLastSeen: never");
}
lineY -= 180;
DRAWTEXT("robotState", -5100, lineY, 150, ColorClasses::white, "load average: " << (float(robotHealth.load[0]) / 10.f) << " " << (float(robotHealth.load[1]) / 10.f) << " " << (float(robotHealth.load[2]) / 10.f));
DRAWTEXT("robotState", -4100, 0, 150, ColorRGBA(255, 255, 255, 50), robotHealth.robotName);
DECLARE_DEBUG_DRAWING("robotOffline", "drawingOnField"); // A huge X to display the online/offline state of the robot
if(SystemCall::getTimeSince(robotPoseReceived) > 500
|| (SystemCall::getTimeSince(isPenalizedReceived) < 1000 && isPenalized)
|| (SystemCall::getTimeSince(hasGroundContactReceived) < 1000 && !hasGroundContact)
|| (SystemCall::getTimeSince(isUprightReceived) < 1000 && !isUpright))
{
LINE("robotOffline", -5100, 3600, 5100, -3600, 50, Drawings::ps_solid, ColorRGBA(0xff, 0, 0));
LINE("robotOffline", 5100, 3600, -5100, -3600, 50, Drawings::ps_solid, ColorRGBA(0xff, 0, 0));
}
if(SystemCall::getTimeSince(isPenalizedReceived) < 1000 && isPenalized)
{
// Draw a text in red letters to tell that the robot is penalized
DRAWTEXT("robotState", -2000, 0, 200, ColorClasses::red, "PENALIZED");
}
if(SystemCall::getTimeSince(hasGroundContactReceived) < 1000 && !hasGroundContact)
{
// Draw a text in red letters to tell that the robot doesn't have ground contact
DRAWTEXT("robotState", 300, 0, 200, ColorClasses::red, "NO GROUND CONTACT");
}
if(SystemCall::getTimeSince(isUprightReceived) < 1000 && !isUpright)
{
// Draw a text in red letters to tell that the robot is fallen down
DRAWTEXT("robotState", 300, 0, 200, ColorClasses::red, "NOT UPRIGHT");
}
DEBUG_RESPONSE_ONCE("automated requests:DrawingManager", OUTPUT(idDrawingManager, bin, Global::getDrawingManager()););
DEBUG_RESPONSE_ONCE("automated requests:DrawingManager3D", OUTPUT(idDrawingManager3D, bin, Global::getDrawingManager3D()););
