SideConfidenceProvider::BallModelSideConfidence SideConfidenceProvider::computeCurrentBallConfidence()
{
// Some constant parameters
const float distanceObserved = lastBallObservation.norm();
const float angleObserved = lastBallObservation.angle();
const float& camZ = theCameraMatrix.translation.z();
const float distanceAsAngleObserved = (pi_2 - std::atan2(camZ,distanceObserved));
// Weighting for original pose
float originalWeighting = computeAngleWeighting(angleObserved, theLocalizationTeamBall.position, theRobotPose,
standardDeviationBallAngle);
originalWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theLocalizationTeamBall.position, theRobotPose,
camZ, standardDeviationBallDistance);
// Weighting for mirrored pose
const Pose2f mirroredPose = Pose2f(pi) + (Pose2f)(theRobotPose);
float mirroredWeighting = computeAngleWeighting(angleObserved, theLocalizationTeamBall.position, mirroredPose,
standardDeviationBallAngle);
mirroredWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theLocalizationTeamBall.position, mirroredPose,
camZ, standardDeviationBallDistance);
PLOT("module:SideConfidenceProvider:originalWeighting", originalWeighting);
PLOT("module:SideConfidenceProvider:mirroredWeighting", mirroredWeighting);
// Decide state based on weights
if((mirroredWeighting < minWeighting) && (originalWeighting < minWeighting))
return UNKNOWN;
if((mirroredWeighting > weightingFactor * originalWeighting) || (originalWeighting > weightingFactor * mirroredWeighting))
return mirroredWeighting > originalWeighting ? MIRROR : OK;
return UNKNOWN;
}
bool GoalFramePerceptor::calcGoalFrame(const Pose2f& prePose, const float yTranslation, GoalFrame& goalFrame) const
{
static const float borderToGroundLineDistance = theFieldDimensions.xPosOpponentFieldBorder - theFieldDimensions.xPosOpponentGroundline;
const Pose2f prePoseInverse(prePose.inverse());
int positive = 0, negative = 0;
for(const Vector2f& p : theFieldBoundary.boundaryOnField)
{
Vector2f pInField = prePoseInverse * p;
if(std::abs(std::abs(pInField.x()) - borderToGroundLineDistance) < allowedFieldBoundaryDivergence)
{
if(pInField.x() > 0)
positive++;
else
negative++;
}
}
if(neededConvexBoundaryPoints > positive && neededConvexBoundaryPoints > negative)
return false;
const float sign = (positive < negative) ? -1.f : 1.f;
goalFrame = Pose2f(prePose).rotate(-pi_2 + sign * pi_2);
goalFrame.translation = goalFrame * Vector2f(0.f, (goalFrame.inverse().translation.y() > 0.f ? 1.f : -1.f)).normalized(yTranslation);
return true;
}
void AutomaticCameraCalibrator::optimize()
{
if(optimizer == nullptr)
{
// since the parameters for the robot pose are correction parameters,
// an empty RobotPose is used instead of theRobotPose
optimizationParameters = pack(theCameraCalibration, Pose2f());
optimizer = new GaussNewtonOptimizer<numOfParameterTranslations>(functor);
successiveConvergations = 0;
framesToWait = 0;
}
else
{
// only do an iteration after some frames have passed
if(framesToWait <= 0)
{
framesToWait = numOfFramesToWait;
const float delta = optimizer->iterate(optimizationParameters, Parameters::Constant(0.0001f));
if(!std::isfinite(delta))
{
OUTPUT_TEXT("Restart optimize! An optimization error occured!");
delete optimizer;
optimizer = nullptr;
state = Accumulate;
}
OUTPUT_TEXT("AutomaticCameraCalibrator: delta = " << delta);
// the camera calibration is refreshed from the current optimizer state
Pose2f robotPoseCorrection;
unpack(optimizationParameters, nextCameraCalibration, robotPoseCorrection);
if(std::abs(delta) < terminationCriterion)
++successiveConvergations;
else
successiveConvergations = 0;
if(successiveConvergations >= minSuccessiveConvergations)
{
OUTPUT_TEXT("AutomaticCameraCalibrator: converged!");
OUTPUT_TEXT("RobotPoseCorrection: " << robotPoseCorrection.translation.x() * 1000.0f
<< " " << robotPoseCorrection.translation.y() * 1000.0f
<< " " << robotPoseCorrection.rotation.toDegrees() << "deg");
currentRobotPose.translation.x() += robotPoseCorrection.translation.x() * 1000.0f;
currentRobotPose.translation.y() += robotPoseCorrection.translation.y() * 1000.0f;
currentRobotPose.rotation = Angle::normalize(currentRobotPose.rotation + robotPoseCorrection.rotation);
abort();
if(setJointOffsets)
invert(theCameraCalibration);
}
}
--framesToWait;
}
}
void FieldFeatureOverviewProvider::update(FieldFeatureOverview& fieldFeatureOverview)
{
const FieldFeature* fieldFeatures[FieldFeatureOverview::numOfFeatures];
fieldFeatures[FieldFeatureOverview::GoalFeature] = &theGoalFeature;
fieldFeatures[FieldFeatureOverview::GoalFrame] = &theGoalFrame;
fieldFeatures[FieldFeatureOverview::MidCircle] = &theMidCircle;
fieldFeatures[FieldFeatureOverview::MidCorner] = &theMidCorner;
fieldFeatures[FieldFeatureOverview::OuterCorner] = &theOuterCorner;
fieldFeatures[FieldFeatureOverview::PenaltyArea] = &thePenaltyArea;
fieldFeatureOverview.combinedStatus.isValid = false;
FOREACH_ENUM((FieldFeatureOverview) Feature, i)
if(((fieldFeatureOverview.statuses[i] = Pose2f(*fieldFeatures[i])).isValid = fieldFeatures[i]->isValid) && (fieldFeatureOverview.combinedStatus.isValid = true))
fieldFeatureOverview.combinedStatus.lastSeen = fieldFeatureOverview.statuses[i].lastSeen = theFrameInfo.time;
fieldFeatureOverview.statuses[FieldFeatureOverview::OuterCorner].isRightSided = theOuterCorner.isRightCorner;
}
void KickEngineData::calcOdometryOffset(KickEngineOutput& output, const RobotModel& theRobotModel)
{
Pose3f ankleInAnkle;
//quickhack to compute support foot, could be worng if the motion is using the right foot as support foot as default
//could use theTorsoMatrix.leftSupportFoot, but if the swing foot steps into the ground the odometry jumps
if(currentKickRequest.mirror)
ankleInAnkle = theRobotModel.limbs[Limbs::ankleRight].inverse() * theRobotModel.limbs[Limbs::ankleLeft];
else
ankleInAnkle = theRobotModel.limbs[Limbs::ankleLeft].inverse() * theRobotModel.limbs[Limbs::ankleRight];
Pose2f currentOdometry(ankleInAnkle.translation.x() * 0.5f, ankleInAnkle.translation.y() * 0.5f);
output.odometryOffset = currentOdometry - lastOdometry;
if(phase == 0)
output.odometryOffset += Pose2f(currentParameters.phaseParameters[phaseNumber].odometryOffset.x(), currentParameters.phaseParameters[phaseNumber].odometryOffset.y());
lastOdometry = currentOdometry;
}
bool GoalFramePerceptor::searchByTT(GoalFrame& goalFrame) const
{
std::vector<const IntersectionsPercept::Intersection*> useTIntersections;
for(const IntersectionsPercept::Intersection& intersection : theIntersectionsPercept.intersections)
if(intersection.type == IntersectionsPercept::Intersection::T)
useTIntersections.push_back(&intersection);
static const float diffTT = theFieldDimensions.yPosLeftPenaltyArea - theFieldDimensions.yPosLeftGoal;
static const float yTTPos = diffTT / 2.f + theFieldDimensions.yPosLeftGoal;
for(unsigned i = 0u; i < useTIntersections.size(); i++)
for(unsigned j = i + 1u; j < useTIntersections.size(); j++)
if(std::abs((useTIntersections[i]->pos - useTIntersections[j]->pos).norm() - diffTT) < tTDistanceThreshold)
if(Angle(useTIntersections[i]->dir1.angle()).diffAbs(useTIntersections[j]->dir1.angle()) < allowedTTAngleDivergence)
if(calcGoalFrame(Pose2f(Angle(useTIntersections[i]->dir2.angle() + pi_2).normalize(),
useTIntersections[i]->pos + 0.5f * (useTIntersections[j]->pos - useTIntersections[i]->pos)),
yTTPos, goalFrame))
return true;
return false;
}
void SpecialActions::update(SpecialActionsOutput& specialActionsOutput)
{
if(!motionNetData.nodeArray)
{
specialActionsOutput.angles.fill(0);
specialActionsOutput.stiffnessData.stiffnesses.fill(0);
return;
}
float speedFactor = 1.0f;
MODIFY("parameters:SpecialActions:speedFactor", speedFactor);
if(theMotionSelection.specialActionMode != MotionSelection::deactive)
{
specialActionsOutput.isLeavingPossible = true;
if(dataRepetitionCounter <= 0)
{
if(!wasActive)
{
//entered from external motion
currentNode = 0;
for(int i = 0; i < Joints::numOfJoints; ++i)
lastRequest.angles[i] = theJointAngles.angles[i];
lastSpecialAction = SpecialActionRequest::numOfSpecialActionIDs;
}
// this is need when a special actions gets executed directly after another without
// switching to a different motion for interpolating the stiffness
if(wasEndOfSpecialAction)
{
specialActionsOutput.stiffnessData.resetToDefault();
if(!mirror)
lastStiffnessRequest = currentStiffnessRequest;
else
lastStiffnessRequest.mirror(currentStiffnessRequest);
currentStiffnessRequest.resetToDefault();
}
wasEndOfSpecialAction = false;
// search next data, leave on transition to external motion
if(!getNextData(theMotionSelection.specialActionRequest, specialActionsOutput))
{
wasActive = true;
wasEndOfSpecialAction = true;
specialActionsOutput.odometryOffset = Pose2f();
return;
}
}
else
{
dataRepetitionCounter -= int(theFrameInfo.cycleTime * 1000 * speedFactor);
stiffnessInterpolationCounter -= int(theFrameInfo.cycleTime * 1000 * speedFactor);
}
//set current joint values
calculateJointRequest(specialActionsOutput);
//odometry update
if(currentInfo.type == SpecialActionInfo::homogeneous || currentInfo.type == SpecialActionInfo::once)
if(mirror)
specialActionsOutput.odometryOffset = Pose2f(-currentInfo.odometryOffset.rotation, currentInfo.odometryOffset.translation.x(), -currentInfo.odometryOffset.translation.y());
else
specialActionsOutput.odometryOffset = currentInfo.odometryOffset;
else
specialActionsOutput.odometryOffset = Pose2f();
if(currentInfo.type == SpecialActionInfo::once)
currentInfo.type = SpecialActionInfo::none;
//store value if current data line finished
if(dataRepetitionCounter <= 0)
{
if(!mirror)
lastRequest = currentRequest;
else
lastRequest.mirror(currentRequest);
}
specialActionsOutput.isLeavingPossible = false;
if(deShakeMode)
for(int i = Joints::lShoulderPitch; i <= Joints::rElbowRoll; ++i)
if(randomFloat() < 0.25)
specialActionsOutput.angles[i] = JointAngles::off;
}
wasActive = theMotionSelection.specialActionMode != MotionSelection::deactive;
}
void MotionCombinator::update(JointRequest& jointRequest)
{
specialActionOdometry += theSpecialActionsOutput.odometryOffset;
copy(theLegJointRequest, jointRequest, theStiffnessSettings, Joints::headYaw, Joints::headPitch);
copy(theArmJointRequest, jointRequest, theStiffnessSettings, Joints::firstArmJoint, Joints::rHand);
copy(theLegJointRequest, jointRequest, theStiffnessSettings, Joints::firstLegJoint, Joints::rAnkleRoll);
ASSERT(jointRequest.isValid());
// Find fully active motion and set MotionInfo
if(theLegMotionSelection.ratios[theLegMotionSelection.targetMotion] == 1.f)
{
// default values
motionInfo.motion = theLegMotionSelection.targetMotion;
motionInfo.isMotionStable = true;
motionInfo.upcomingOdometryOffset = Pose2f();
Pose2f odometryOffset;
switch(theLegMotionSelection.targetMotion)
{
case MotionRequest::walk:
odometryOffset = theWalkingEngineOutput.odometryOffset;
motionInfo.walkRequest = theWalkingEngineOutput.executedWalk;
motionInfo.upcomingOdometryOffset = theWalkingEngineOutput.upcomingOdometryOffset;
break;
case MotionRequest::kick:
odometryOffset = theKickEngineOutput.odometryOffset;
motionInfo.kickRequest = theKickEngineOutput.executedKickRequest;
motionInfo.isMotionStable = theKickEngineOutput.isStable;
break;
case MotionRequest::specialAction:
odometryOffset = specialActionOdometry;
specialActionOdometry = Pose2f();
motionInfo.specialActionRequest = theSpecialActionsOutput.executedSpecialAction;
motionInfo.isMotionStable = theSpecialActionsOutput.isMotionStable;
break;
case MotionRequest::getUp:
motionInfo.isMotionStable = false;
odometryOffset = theGetUpEngineOutput.odometryOffset;
break;
case MotionRequest::stand:
default:
break;
}
if(theRobotInfo.hasFeature(RobotInfo::zGyro) && (theFallDownState.state == FallDownState::upright || theLegMotionSelection.targetMotion == MotionRequest::getUp))
{
Vector3f rotatedGyros = theInertialData.orientation * theInertialData.gyro.cast<float>();
odometryOffset.rotation = rotatedGyros.z() * theFrameInfo.cycleTime;
}
odometryData += odometryOffset;
}
if(emergencyOffEnabled && motionInfo.motion != MotionRequest::getUp)
{
if(theFallDownState.state == FallDownState::falling && motionInfo.motion != MotionRequest::specialAction)
{
safeFall(jointRequest);
centerHead(jointRequest);
currentRecoveryTime = 0;
ASSERT(jointRequest.isValid());
}
else if(theFallDownState.state == FallDownState::onGround && motionInfo.motion != MotionRequest::specialAction)
{
centerHead(jointRequest);
}
else
{
if(theFallDownState.state == FallDownState::upright)
{
headYawInSafePosition = false;
headPitchInSafePosition = false;
}
if(currentRecoveryTime < recoveryTime)
{
currentRecoveryTime += 1;
float ratio = (1.f / float(recoveryTime)) * currentRecoveryTime;
for(int i = 0; i < Joints::numOfJoints; i++)
{
jointRequest.stiffnessData.stiffnesses[i] = 30 + int(ratio * float(jointRequest.stiffnessData.stiffnesses[i] - 30));
}
}
}
}
if(debugArms)
debugReleaseArms(jointRequest);
eraseStiffness(jointRequest);
float sum = 0;
int count = 0;
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore &&
lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore &&
lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:all", sum / count);
sum = 0;
count = 0;
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:differenceToJointData:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
lastJointRequest = jointRequest;
PLOT("module:MotionCombinator:differenceToJointData:all", sum / count);
#ifndef NDEBUG
if(!jointRequest.isValid())
{
{
std::string logDir = "";
#ifdef TARGET_ROBOT
logDir = "../logs/";
#endif
OutMapFile stream(logDir + "jointRequest.log");
stream << jointRequest;
OutMapFile stream2(logDir + "motionSelection.log");
stream2 << theLegMotionSelection;
}
ASSERT(false);
}
#endif
}