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;
}
void TorsoMatrixProvider::update(TorsoMatrix& torsoMatrix)
{
const Vector3f axis(theInertialData.angle.x(), theInertialData.angle.y(), 0.0f);
const RotationMatrix torsoRotation = Rotation::AngleAxis::unpack(axis);
// calculate "center of hip" position from left foot
Pose3f fromLeftFoot = Pose3f(torsoRotation) *= theRobotModel.soleLeft;
fromLeftFoot.translation *= -1.;
fromLeftFoot.rotation = torsoRotation;
// calculate "center of hip" position from right foot
Pose3f fromRightFoot = Pose3f(torsoRotation) *= theRobotModel.soleRight;
fromRightFoot.translation *= -1.;
fromRightFoot.rotation = torsoRotation;
// get foot z-rotations
const Pose3f& leftFootInverse(theRobotModel.limbs[Limbs::footLeft].inverse());
const Pose3f& rightFootInverse(theRobotModel.limbs[Limbs::footRight].inverse());
const float leftFootZRotation = leftFootInverse.rotation.getZAngle();
const float rightFootZRotation = rightFootInverse.rotation.getZAngle();
// determine used foot
const bool useLeft = fromLeftFoot.translation.z() > fromRightFoot.translation.z();
torsoMatrix.leftSupportFoot = useLeft;
// calculate foot span
const Vector3f newFootSpan(fromRightFoot.translation - fromLeftFoot.translation);
// and construct the matrix
Pose3f newTorsoMatrix;
newTorsoMatrix.translate(newFootSpan.x() / (useLeft ? 2.f : -2.f), newFootSpan.y() / (useLeft ? 2.f : -2.f), 0);
newTorsoMatrix.conc(useLeft ? fromLeftFoot : fromRightFoot);
// calculate torso offset
if(torsoMatrix.translation.z() != 0) // the last torso matrix should be valid
{
Pose3f& torsoOffset = torsoMatrix.offset;
torsoOffset = torsoMatrix.inverse();
torsoOffset.translate(lastFootSpan.x() / (useLeft ? 2.f : -2.f), lastFootSpan.y() / (useLeft ? 2.f : -2.f), 0);
torsoOffset.rotateZ(useLeft ? float(leftFootZRotation - lastLeftFootZRotation) : float(rightFootZRotation - lastRightFootZRotation));
torsoOffset.translate(newFootSpan.x() / (useLeft ? -2.f : 2.f), newFootSpan.y() / (useLeft ? -2.f : 2.f), 0);
torsoOffset.conc(newTorsoMatrix);
}
// adopt new matrix and footSpan
static_cast<Pose3f&>(torsoMatrix) = newTorsoMatrix;
lastLeftFootZRotation = leftFootZRotation;
lastRightFootZRotation = rightFootZRotation;
lastFootSpan = newFootSpan;
// valid?
torsoMatrix.isValid = theGroundContactState.contact;
//
PLOT("module:TorsoMatrixProvider:footSpanX", newFootSpan.x());
PLOT("module:TorsoMatrixProvider:footSpanY", newFootSpan.y());
PLOT("module:TorsoMatrixProvider:footSpanZ", newFootSpan.z());
}
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 MotionCombinator::update(OdometryData& odometryData)
{
if(!theRobotInfo.hasFeature(RobotInfo::zGyro) || (theFallDownState.state != FallDownState::upright && theLegMotionSelection.targetMotion != MotionRequest::getUp))
this->odometryData.rotation += theFallDownState.odometryRotationOffset;
this->odometryData.rotation.normalize();
odometryData = this->odometryData;
Pose2f odometryOffset = odometryData;
odometryOffset -= lastOdometryData;
PLOT("module:MotionCombinator:odometryOffsetX", odometryOffset.translation.x());
PLOT("module:MotionCombinator:odometryOffsetY", odometryOffset.translation.y());
PLOT("module:MotionCombinator:odometryOffsetRotation", odometryOffset.rotation.toDegrees());
lastOdometryData = odometryData;
}
/*
* The spectrum of a simple AM wave
*/
void am_wave(void) {
plot_t p;
p.outfile = "output3.png";
p.title = "AM wave";
p.xlabel = "t [s]";
p.ylabel = "u(t) [m]";
p.xmin = 0;
p.xmax = 0;
init_gnuplot(&p);
gsl_complex_packed_array data = calloc(2*N, sizeof(double));
BEGIN_SCATTER(p, "u(t)");
double t = 0;
for (int i = 0; i < 2*N; i += 2) {
data[i] = u(t);
PLOT(p, t, data[i]);
t += dt;
}
END_PLOT(p);
p.outfile = "output4.png";
p.title = "FFT of AM wave";
p.xlabel = "f [Hz]";
p.ylabel = "S(f) = |U(f)|^2 [m^2]";
p.xmin = 0;
p.xmax = 40;
init_gnuplot(&p);
gsl_fft_complex_radix2_forward(data, 1, N);
BEGIN_PLOT(p, "S(f)");
double f = 0;
for (int i = 0; i < N; i += 2) {
PLOT(p, f, (data[i]*data[i] + data[i+1]*data[i+1])/(N*N));
f += 1.0;
}
END_PLOT(p);
free(data);
}
void BH2011GoalSymbols::update()
{
timeSinceOppGoalWasSeen = (float) frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen);
timeSinceOwnGoalWasSeen = (float) frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen);
timeSinceAnyGoalWasSeen = timeSinceOppGoalWasSeen < timeSinceOwnGoalWasSeen ? timeSinceOppGoalWasSeen : timeSinceOwnGoalWasSeen;
oppGoalWasSeen = timeSinceOppGoalWasSeen < 500;
goalWasSeen = timeSinceAnyGoalWasSeen < 500;
/* Calculate seen goals in ready state*/
if(gameInfo.state == STATE_READY)
{
if(frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen) == 0 ||
frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen) == 0)
goalsSeenInReadyState += 1.0;
}
else
{
goalsSeenInReadyState = 0;
}
/* Calculate seen goals since last pickup */
if((groundContactState.noContactSafe && damageConfiguration.useGroundContactDetectionForSafeStates)
&& (fallDownState.state == FallDownState::upright || fallDownState.state == FallDownState::staggering))
{
goalsSeenSinceLastPickup = 0.0;
}
else
{
if(frameInfo.getTimeSince(goalPercept.timeWhenOppGoalLastSeen) == 0 ||
frameInfo.getTimeSince(goalPercept.timeWhenOwnGoalLastSeen) == 0)
goalsSeenSinceLastPickup += 1.0;
}
PLOT("Behavior:goalsSeenSinceLastPickup", goalsSeenSinceLastPickup);
/* determine angles to sides of free part of opponent goal */
Vector2<> centerOfOppGoalAbs = Vector2<>((float) fieldDimensions.xPosOpponentGoalpost, (float)(fieldDimensions.yPosLeftGoal + fieldDimensions.yPosRightGoal) / 2.0f);
Vector2<> centerOfOppGoalRel = Geometry::fieldCoord2Relative(robotPose, centerOfOppGoalAbs);
float leftDistance = abs(centerOfOppGoalAbs.y - freePartOfOpponentGoalModel.leftEnd.y);
float rightDistance = abs(centerOfOppGoalAbs.y - freePartOfOpponentGoalModel.rightEnd.y);
if(leftDistance < rightDistance)
{
innerSide = freePartOfOpponentGoalModel.leftEnd;
outerSide = freePartOfOpponentGoalModel.rightEnd;
}
else
{
innerSide = freePartOfOpponentGoalModel.rightEnd;
outerSide = freePartOfOpponentGoalModel.leftEnd;
}
/* draw angles to free part into worldstate, see draw()-function below */
DECLARE_DEBUG_DRAWING("behavior:GoalSymbols:FreePartAnglesOnField", "drawingOnField");
COMPLEX_DRAWING("behavior:GoalSymbols:FreePartAnglesOnField", drawingOnField());
}
/*
void TorsoMatrixProvider::update(FilteredOdometryOffset& odometryOffset)
{
Pose2DBH odometryOffset;
if(lastTorsoMatrix.translation.z != 0.)
{
Pose3DBH odometryOffset3D(lastTorsoMatrix);
odometryOffset3D.conc(theTorsoMatrixBH.offset);
odometryOffset3D.conc(theTorsoMatrixBH.invert());
odometryOffset.translation.x = odometryOffset3D.translation.x;
odometryOffset.translation.y = odometryOffset3D.translation.y;
odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
}
PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));
(Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}
*/
void TorsoMatrixProvider::update(OdometryDataBH& odometryData)
{
Pose2DBH odometryOffset;
if(lastTorsoMatrix.translation.z != 0.)
{
Pose3DBH odometryOffset3D(lastTorsoMatrix);
odometryOffset3D.conc(theTorsoMatrixBH.offset);
odometryOffset3D.conc(theTorsoMatrixBH.invert());
odometryOffset.translation.x = odometryOffset3D.translation.x;
odometryOffset.translation.y = odometryOffset3D.translation.y;
odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
}
PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));
odometryData += odometryOffset;
(Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}
void TorsoMatrixProvider::update(OdometryData& odometryData)
{
Pose2f odometryOffset;
if(lastTorsoMatrix.translation.z() != 0.)
{
Pose3f odometryOffset3D(lastTorsoMatrix);
odometryOffset3D.conc(theTorsoMatrix.offset);
odometryOffset3D.conc(theTorsoMatrix.inverse());
odometryOffset.translation.x() = odometryOffset3D.translation.x();
odometryOffset.translation.y() = odometryOffset3D.translation.y();
odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
}
PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x());
PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y());
PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", odometryOffset.rotation.toDegrees());
odometryData += odometryOffset;
(Pose3f&)lastTorsoMatrix = theTorsoMatrix;
}
Vector4f LIPStateEstimator::measure(SupportFoot supportFoot, const Vector2f& LIPOrigin) const
{
const Pose3f& supportFootToTorso = supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight;
const Quaternionf& torsoToWorld = theInertialData.orientation2D;
const Pose3f originToTorso = supportFootToTorso + Vector3f(LIPOrigin.x(), LIPOrigin.y(), 0.f);
const Vector3f comInOrigin = originToTorso.inverse() * theRobotModel.centerOfMass;
const Vector3f com = (torsoToWorld * originToTorso.rotation) * comInOrigin;
PLOT("module:ZmpWalkingEngine:LIPStateEstimator:Estimate:measuredComHeight", com.z());
const Vector2f accInWorld = (torsoToWorld * theInertialData.acc * 1000.f).head<2>();
const Vector2f zmp = com.head<2>().array() - (accInWorld.array() / LIP3D(LIPHeights).getK().square());
return (Vector4f() << com.head<2>(), zmp).finished();
}
struct GroundContactDetector::ContactState GroundContactDetector::checkLoad()
{
ContactState state;
float loadSum = theSensorData.currents[JointData::LHipPitch];
loadSum += theSensorData.currents[JointData::LKneePitch];
loadSum += theSensorData.currents[JointData::LAnklePitch];
loadSum += theSensorData.currents[JointData::RHipPitch];
loadSum += theSensorData.currents[JointData::RKneePitch];
loadSum += theSensorData.currents[JointData::RAnklePitch];
loadSum += theSensorData.currents[JointData::LHipRoll];
loadSum += theSensorData.currents[JointData::LHipYawPitch];
loadSum += theSensorData.currents[JointData::LAnkleRoll];
loadSum += theSensorData.currents[JointData::RHipRoll];
loadSum += theSensorData.currents[JointData::RHipYawPitch];
loadSum += theSensorData.currents[JointData::RAnkleRoll];
PLOT("module:GroundContactDetector:contactLoadSum", loadSum);
state.contact = loadSum > p.loadThreshold;
//max angleY: pi_4 (45°)
state.confidence = 1 - min(abs(theSensorData.data[SensorData::angleY]), pi_4) / pi_4;
return state;
}
void InertialDataFilter::readingUpdate(const Vector3f& reading)
{
generateSigmaPoints();
for(int i = 0; i < 5; ++i)
readingModel(sigmaPoints[i], sigmaReadings[i]);
meanOfSigmaReadings();
PLOT("module:InertialDataFilter:expectedAccX", readingMean.x());
PLOT("module:InertialDataFilter:accX", reading.x());
PLOT("module:InertialDataFilter:expectedAccY", readingMean.y());
PLOT("module:InertialDataFilter:accY", reading.y());
PLOT("module:InertialDataFilter:expectedAccZ", readingMean.z());
PLOT("module:InertialDataFilter:accZ", reading.z());
const Matrix3x2f readingsSigmaPointsCov = covOfSigmaReadingsAndSigmaPoints();
const Matrix3f readingsCov = covOfSigmaReadings();
const Matrix3f sensorCov = accNoise.array().square().matrix().asDiagonal();
const Matrix2x3f kalmanGain = readingsSigmaPointsCov.transpose() * (readingsCov + sensorCov).inverse();
mean += kalmanGain * (reading - readingMean);
cov -= kalmanGain * readingsSigmaPointsCov;
}
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 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 FootBumperStateProvider::update(FootBumperState& footBumperState)
{
// Check, if any bumper is pressed
const bool leftFootLeft = !theDamageConfigurationBody.sides[Legs::left].footBumperDefect && checkContact(KeyStates::leftFootLeft, leftFootLeftDuration);
const bool leftFootRight = !theDamageConfigurationBody.sides[Legs::left].footBumperDefect && checkContact(KeyStates::leftFootRight, leftFootRightDuration);
const bool rightFootLeft = !theDamageConfigurationBody.sides[Legs::right].footBumperDefect && checkContact(KeyStates::rightFootLeft, rightFootLeftDuration);
const bool rightFootRight = !theDamageConfigurationBody.sides[Legs::right].footBumperDefect && checkContact(KeyStates::rightFootRight, rightFootRightDuration);
const bool contactLeftFoot = leftFootLeft || leftFootRight;
const bool contactRightFoot = rightFootLeft || rightFootRight;
// Update statistics
if(contactLeftFoot)
{
contactBufferLeft.push_front(1);
contactDurationLeft++;
}
else
{
contactBufferLeft.push_front(0);
contactDurationLeft = 0;
}
if(contactRightFoot)
{
contactBufferRight.push_front(1);
contactDurationRight++;
}
else
{
contactBufferRight.push_front(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.sum() > contactThreshold)
{
footBumperState.status[Legs::left].contact = true;
footBumperState.status[Legs::left].contactDuration = contactDurationLeft;
if(contactLeftFoot)
footBumperState.status[Legs::left].lastContact = theFrameInfo.time;
}
else
{
footBumperState.status[Legs::left].contact = false;
footBumperState.status[Legs::left].contactDuration = 0;
}
if(contactBufferRight.sum() > contactThreshold)
{
footBumperState.status[Legs::right].contact = true;
footBumperState.status[Legs::right].contactDuration = contactDurationRight;
if(contactRightFoot)
footBumperState.status[Legs::right].lastContact = theFrameInfo.time;
}
else
{
footBumperState.status[Legs::right].contact = false;
footBumperState.status[Legs::right].contactDuration = 0;
}
}
else
{
footBumperState.status[Legs::left].contact = false;
footBumperState.status[Legs::right].contact = false;
footBumperState.status[Legs::left].contactDuration = 0;
footBumperState.status[Legs::right].contactDuration = 0;
}
// Debugging stuff:
if(debug && theFrameInfo.getTimeSince(lastSoundTime) > (int)soundDelay && (footBumperState.status[Legs::left].contact || footBumperState.status[Legs::right].contact))
{
lastSoundTime = theFrameInfo.time;
SystemCall::playSound("jump.wav");
}
PLOT("module:FootBumperStateProvider:sumLeft", contactBufferLeft.sum());
PLOT("module:FootBumperStateProvider:durationLeft", contactDurationLeft);
PLOT("module:FootBumperStateProvider:sumRight", contactBufferRight.sum());
PLOT("module:FootBumperStateProvider:durationRight", contactDurationRight);
PLOT("module:FootBumperStateProvider:contactLeft", footBumperState.status[Legs::left].contact ? 10 : 0);
PLOT("module:FootBumperStateProvider:contactRight", footBumperState.status[Legs::right].contact ? 10 : 0);
PLOT("module:FootBumperStateProvider:leftFootLeft", leftFootLeft ? 10 : 0);
PLOT("module:FootBumperStateProvider:leftFootRight", leftFootRight ? 10 : 0);
PLOT("module:FootBumperStateProvider:rightFootLeft", rightFootLeft ? 10 : 0);
PLOT("module:FootBumperStateProvider: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);
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 HeadMotionEngine::update(HeadJointRequest& headJointRequest)
{
// update requested angles
requestedPan = theHeadAngleRequest.pan;
requestedTilt = theHeadAngleRequest.tilt;
//
float maxAcc = theGroundContactState.contact ? 10.f : 1.f; // arbitrary value that seems to be good...
MODIFY("module:HeadMotionEngine:maxAcceleration", maxAcc);
float pan = requestedPan == JointAngles::off ? JointAngles::off : Rangef(theHeadLimits.minPan(), theHeadLimits.maxPan()).limit(requestedPan);
float tilt = requestedTilt == JointAngles::off ? JointAngles::off : theHeadLimits.getTiltBound(pan).limit(requestedTilt);
const float deltaTime = theFrameInfo.cycleTime;
const Vector2f position(headJointRequest.pan == JointAngles::off ? theJointAngles.angles[Joints::headYaw] : headJointRequest.pan,
headJointRequest.tilt == JointAngles::off ? theJointAngles.angles[Joints::headPitch] : headJointRequest.tilt);
const Vector2f target(pan == JointAngles::off ? 0.f : pan, tilt == JointAngles::off ? 0.f : tilt);
Vector2f offset(target - position);
const float distanceToTarget = offset.norm();
// calculate max speed
const float maxSpeedForDistance = std::sqrt(2.f * distanceToTarget * maxAcc * 0.8f);
const float requestedSpeed = theHeadAngleRequest.stopAndGoMode
? theHeadAngleRequest.speed * (std::cos(pi2 / stopAndGoModeFrequenzy * theFrameInfo.time) / 2.f + .5f)
: static_cast<float>(theHeadAngleRequest.speed);
const float maxSpeed = std::min(maxSpeedForDistance, requestedSpeed);
// max speed clipping
if(distanceToTarget / deltaTime > maxSpeed)
offset *= maxSpeed * deltaTime / distanceToTarget; //<=> offset.normalize(maxSpeed * deltaTime);
// max acceleration clipping
Vector2f speed(offset / deltaTime);
Vector2f acc((speed - lastSpeed) / deltaTime);
const float accSquareAbs = acc.squaredNorm();
if(accSquareAbs > maxAcc * maxAcc)
{
acc *= maxAcc * deltaTime / std::sqrt(accSquareAbs);
speed = acc + lastSpeed;
offset = speed * deltaTime;
}
/* <=>
Vector2f speed(offset / deltaTime);
Vector2f acc((speed - lastSpeed) / deltaTime);
if(acc.squaredNorm() > maxAcc * maxAcc)
{
speed = acc.normalize(maxAcc * deltaTime) + lastSpeed;
offset = speed * deltaTime;
}
*/
PLOT("module:HeadMotionEngine:speed", toDegrees(speed.norm()));
// calculate new position
Vector2f newPosition(position + offset);
// set new position
headJointRequest.pan = pan == JointAngles::off ? JointAngles::off : newPosition.x();
headJointRequest.tilt = tilt == JointAngles::off ? JointAngles::off : newPosition.y();
headJointRequest.moving = pan != JointAngles::off && tilt != JointAngles::off && ((newPosition - position) / deltaTime).squaredNorm() > sqr(maxAcc * deltaTime * 0.5f);
// check reachability
headJointRequest.reachable = true;
if(pan != requestedPan || tilt != requestedTilt)
headJointRequest.reachable = false;
// store some values for the next iteration
lastSpeed = speed;
}
bool TeamDataProvider::handleMessage(InMessage& message)
{
/*
The robotNumber and the three flags hasGroundContact, isUpright and isPenalized should always be updated.
*/
switch(message.getMessageID())
{
case idNTPHeader:
VERIFY(ntp.handleMessage(message));
timeStamp = ntp.receiveTimeStamp;
return false;
case idNTPIdentifier:
case idNTPRequest:
case idNTPResponse:
return ntp.handleMessage(message);
case idRobot:
message.bin >> robotNumber;
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
theTeamMateData.timeStamps[robotNumber] = timeStamp;
return true;
case idReleaseOptions:
message.bin >> Global::getReleaseOptions();
return true;
case idSSLVisionData:
{
message.bin >> theSSLVisionData;
unsigned remoteTimestamp = theSSLVisionData.recentData.top().receiveTimestamp;
REMOTE_TO_LOCAL_TIME(theSSLVisionData.recentData.top().receiveTimestamp);
unsigned localTimestamp = theSSLVisionData.recentData.top().receiveTimestamp;
int offset = (int) localTimestamp - (int) remoteTimestamp;
PLOT("module:TeamDataProvider:sslVisionOffset", offset);
}
return true;
case idGroundTruthBallModel:
{
Vector2<> position;
message.bin >> theGroundTruthBallModel.timeWhenLastSeen >> position;
REMOTE_TO_LOCAL_TIME(theGroundTruthBallModel.timeWhenLastSeen);
if(theOwnTeamInfo.teamColor == TEAM_BLUE)
position *= -1;
theGroundTruthBallModel.lastPerception.setPositionAndVelocityInFieldCoordinates(
position, Vector2<>(), theGroundTruthRobotPose);
theGroundTruthBallModel.estimate = theGroundTruthBallModel.lastPerception;
}
return true;
case idGroundTruthRobotPose:
{
char teamColor,
id;
unsigned timeStamp;
Pose2D robotPose;
message.bin >> teamColor >> id >> timeStamp >> robotPose;
if(teamColor == (int)theOwnTeamInfo.teamColor && id == theRobotInfo.number)
{
if(theOwnTeamInfo.teamColor == TEAM_BLUE)
robotPose = Pose2D(pi) + robotPose;
(Pose2D&) theGroundTruthRobotPose = robotPose;
}
}
return true;
case idTeamMateIsPenalized:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
message.bin >> theTeamMateData.isPenalized[robotNumber];
return true;
case idTeamMateHasGroundContact:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
message.bin >> theTeamMateData.hasGroundContact[robotNumber];
return true;
case idTeamMateIsUpright:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
message.bin >> theTeamMateData.isUpright[robotNumber];
return true;
case idTeamMateTimeSinceLastGroundContact:
if(robotNumber != theRobotInfo.number)
if(robotNumber >= TeamMateData::firstPlayer && robotNumber < TeamMateData::numOfPlayers)
{
message.bin >> theTeamMateData.timeSinceLastGroundContact[robotNumber];
REMOTE_TO_LOCAL_TIME(theTeamMateData.timeSinceLastGroundContact[robotNumber]);
}
return true;
default:
break;
}
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 SensorFilter::update(FilteredSensorData& filteredSensorData,
const InertiaSensorData& theInertiaSensorData,
const SensorData& theSensorData,
const OrientationData& theOrientationData)
{
// copy sensor data (except gyro and acc)
Vector2BH<> gyro(filteredSensorData.data[SensorData::gyroX], filteredSensorData.data[SensorData::gyroY]);
Vector3BH<> acc(filteredSensorData.data[SensorData::accX], filteredSensorData.data[SensorData::accY], filteredSensorData.data[SensorData::accZ]);
(SensorData&)filteredSensorData = theSensorData;
filteredSensorData.data[SensorData::gyroX] = gyro.x;
filteredSensorData.data[SensorData::gyroY] = gyro.y;
filteredSensorData.data[SensorData::accX] = acc.x;
filteredSensorData.data[SensorData::accY] = acc.y;
filteredSensorData.data[SensorData::accZ] = acc.z;
// take calibrated inertia sensor data
for(int i = 0; i < 2; ++i)
{
if(theInertiaSensorData.gyro[i] != InertiaSensorData::off)
filteredSensorData.data[SensorData::gyroX + i] = theInertiaSensorData.gyro[i];
else if(filteredSensorData.data[SensorData::gyroX + i] == SensorData::off)
filteredSensorData.data[SensorData::gyroX + i] = 0.f;
}
filteredSensorData.data[SensorData::gyroZ] = 0.f;
for(int i = 0; i < 3; ++i)
{
if(theInertiaSensorData.acc[i] != InertiaSensorData::off)
filteredSensorData.data[SensorData::accX + i] = theInertiaSensorData.acc[i] / 9.80665f;
else if(filteredSensorData.data[SensorData::accX + i] == SensorData::off)
filteredSensorData.data[SensorData::accX + i] = 0.f;
}
// take orientation data
filteredSensorData.data[SensorData::angleX] = theOrientationData.orientation.x;
filteredSensorData.data[SensorData::angleY] = theOrientationData.orientation.y;
#ifndef RELEASE
if(filteredSensorData.data[SensorData::gyroX] != SensorData::off)
{
gyroAngleXSum += filteredSensorData.data[SensorData::gyroX] * (theSensorData.timeStamp - lastIteration) * 0.001f;
gyroAngleXSum = normalizeBH(gyroAngleXSum);
lastIteration = theSensorData.timeStamp;
}
PLOT("module:SensorFilter:gyroAngleXSum", gyroAngleXSum);
#endif
//
// PLOT("module:SensorFilter:rawAngleX", theSensorData.data[SensorData::angleX]);
// PLOT("module:SensorFilter:rawAngleY", theSensorData.data[SensorData::angleY]);
//
// PLOT("module:SensorFilter:rawAccX", theSensorData.data[SensorData::accX]);
// PLOT("module:SensorFilter:rawAccY", theSensorData.data[SensorData::accY]);
// PLOT("module:SensorFilter:rawAccZ", theSensorData.data[SensorData::accZ]);
//
// PLOT("module:SensorFilter:rawGyroX", theSensorData.data[SensorData::gyroX]);
// PLOT("module:SensorFilter:rawGyroY", theSensorData.data[SensorData::gyroY]);
// PLOT("module:SensorFilter:rawGyroZ", theSensorData.data[SensorData::gyroZ]);
//
// PLOT("module:SensorFilter:angleX", filteredSensorData.data[SensorData::angleX]);
// PLOT("module:SensorFilter:angleY", filteredSensorData.data[SensorData::angleY]);
//
// PLOT("module:SensorFilter:accX", filteredSensorData.data[SensorData::accX]);
// PLOT("module:SensorFilter:accY", filteredSensorData.data[SensorData::accY]);
// PLOT("module:SensorFilter:accZ", filteredSensorData.data[SensorData::accZ]);
//
// PLOT("module:SensorFilter:gyroX", filteredSensorData.data[SensorData::gyroX] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroX] : 0);
// PLOT("module:SensorFilter:gyroY", filteredSensorData.data[SensorData::gyroY] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroY] : 0);
// PLOT("module:SensorFilter:gyroZ", filteredSensorData.data[SensorData::gyroZ] != float(SensorData::off) ? filteredSensorData.data[SensorData::gyroZ] : 0);
//PLOT("module:SensorFilter:us", filteredSensorData.data[SensorData::us]);
}
/*
* Extracting information from a noisy signal
*/
void extract_noisy_signal(int count, double *values) {
plot_t p;
p.outfile = "output5.png";
p.title = "Noisy signal";
p.xlabel = "t [s]";
p.ylabel = "u(t) [m]";
p.xmin = 0;
p.xmax = 0;
init_gnuplot(&p);
gsl_complex_packed_array data = calloc(2*count, sizeof(double));
// Noisy signal
BEGIN_SCATTER(p, "u(t)");
double t = 0;
for (int i = 0; i < count; i++) {
PLOT(p, t, values[i]);
data[2*i] = values[i];
t += 1.0/count;
}
END_PLOT(p);
p.outfile = "output6.png";
p.title = "FFT of noisy signal";
p.xlabel = "f [Hz]";
p.ylabel = "U(f) [m]";
p.xmin = -1500;
p.xmax = 1500;
init_gnuplot(&p);
gsl_fft_complex_radix2_forward(data, 1, count);
// FFT of noisy signal
BEGIN_SCATTER(p, "U(f)");
double f = -count/2;
for (int i = count; i < 2*count; i += 2) {
double y = data[i];
data[i] = H(f) * data[i];
data[i+1] = H(f) * data[i+1];
PLOT(p, f, y);
f += 1.0;
}
for (int i = 0; i < count; i += 2) {
double y = data[i];
data[i] = H(f) * data[i];
data[i+1] = H(f) * data[i+1];
PLOT(p, f, y);
f += 1.0;
}
END_PLOT(p);
gsl_fft_complex_radix2_inverse(data, 1, count);
p.outfile = "output7.png";
p.title = "Inverse FFT of data";
p.xlabel = "t [s]";
p.ylabel = "u(t) [m]";
p.xmin = 0;
p.xmax = 1;
init_gnuplot(&p);
// Filtered signal
BEGIN_PLOT(p, "u(t)");
t = 0;
double umax = 0;
int periods = 0;
char letter = 0;
int bits = 0;
const double plen = 0.0078;
for (int i = 0; i < 2*count; i += 2) {
if (t >= plen * (double)periods && t < plen * ((double)periods + 1.0)) {
if (data[i] > umax) umax = data[i];
} else {
if (umax < 1) letter *= 2;
else letter = letter*2 + 1;
bits++;
if (bits % 8 == 0) {
printf("%c", letter);
bits = 0;
letter = 0;
}
umax = 0;
periods++;
}
PLOT(p, t, (data[i]));
t += 1.0/count;
}
if (umax < 1) letter *= 2;
else letter = letter*2 + 1;
printf("%c\n", letter);
END_PLOT(p);
}
void TorsoMatrixProvider::update(TorsoMatrixBH& torsoMatrix)
{
// remove the z-rotation from the orientation data rotation matrix
Vector3BH<> g(theOrientationDataBH.rotation.c1.z, -theOrientationDataBH.rotation.c0.z, 0.f);
float w = std::atan2(std::sqrt(g.x * g.x + g.y * g.y), theOrientationDataBH.rotation.c2.z);
RotationMatrixBH torsoRotation(g, w);
// calculate "center of hip" position from left foot
Pose3DBH fromLeftFoot(torsoRotation);
fromLeftFoot.conc(theRobotModelBH.limbs[MassCalibrationBH::footLeft]);
fromLeftFoot.translate(0, 0, -theRobotDimensionsBH.heightLeg5Joint);
fromLeftFoot.translation *= -1.;
fromLeftFoot.rotation = torsoRotation;
// calculate "center of hip" position from right foot
Pose3DBH fromRightFoot(torsoRotation);
fromRightFoot.conc(theRobotModelBH.limbs[MassCalibrationBH::footRight]);
fromRightFoot.translate(0, 0, -theRobotDimensionsBH.heightLeg5Joint);
fromRightFoot.translation *= -1.;
fromRightFoot.rotation = torsoRotation;
// get foot z-rotations
const Pose3DBH& leftFootInverse(theRobotModelBH.limbs[MassCalibrationBH::footLeft].invert());
const Pose3DBH& rightFootInverse(theRobotModelBH.limbs[MassCalibrationBH::footRight].invert());
const float leftFootZRotation = leftFootInverse.rotation.getZAngle();
const float rightFootZRotation = rightFootInverse.rotation.getZAngle();
// determine used foot
const bool useLeft = fromLeftFoot.translation.z > fromRightFoot.translation.z;
torsoMatrix.leftSupportFoot = useLeft;
// calculate foot span
const Vector3BH<> newFootSpan(fromRightFoot.translation - fromLeftFoot.translation);
// and construct the matrix
Pose3DBH newTorsoMatrix;
newTorsoMatrix.translate(newFootSpan.x / (useLeft ? 2.f : -2.f), newFootSpan.y / (useLeft ? 2.f : -2.f), 0);
newTorsoMatrix.conc(useLeft ? fromLeftFoot : fromRightFoot);
// calculate torso offset
if(torsoMatrix.translation.z != 0) // the last torso matrix should be valid
{
Pose3DBH& torsoOffset(torsoMatrix.offset);
torsoOffset = torsoMatrix.invert();
torsoOffset.translate(lastFootSpan.x / (useLeft ? 2.f : -2.f), lastFootSpan.y / (useLeft ? 2.f : -2.f), 0);
torsoOffset.rotateZ(useLeft ? float(leftFootZRotation - lastLeftFootZRotation) : float(rightFootZRotation - lastRightFootZRotation));
torsoOffset.translate(newFootSpan.x / (useLeft ? -2.f : 2.f), newFootSpan.y / (useLeft ? -2.f : 2.f), 0);
torsoOffset.conc(newTorsoMatrix);
}
// adopt new matrix and footSpan
(Pose3DBH&)torsoMatrix = newTorsoMatrix;
lastLeftFootZRotation = leftFootZRotation;
lastRightFootZRotation = rightFootZRotation;
lastFootSpan = newFootSpan;
// valid?
torsoMatrix.isValid = theGroundContactStateBH.contact;
//
PLOT("module:TorsoMatrixProvider:footSpanX", newFootSpan.x);
PLOT("module:TorsoMatrixProvider:footSpanY", newFootSpan.y);
PLOT("module:TorsoMatrixProvider:footSpanZ", newFootSpan.z);
PLOT("module:TorsoMatrixProvider:torsoMatrixX", torsoMatrix.translation.x);
PLOT("module:TorsoMatrixProvider:torsoMatrixY", torsoMatrix.translation.y);
PLOT("module:TorsoMatrixProvider:torsoMatrixZ", torsoMatrix.translation.z);
}
void MotionSelector::update(MotionSelection& motionSelection)
{
static int interpolationTimes[MotionRequest::numOfMotions];
interpolationTimes[MotionRequest::walk] = 790;
interpolationTimes[MotionRequest::kick] = 200;
interpolationTimes[MotionRequest::specialAction] = 200;
interpolationTimes[MotionRequest::stand] = 600;
interpolationTimes[MotionRequest::getUp] = 600;
static const int playDeadDelay(2000);
if(lastExecution)
{
MotionRequest::Motion requestedMotion = theMotionRequest.motion;
if(theMotionRequest.motion == MotionRequest::walk && !theGroundContactState.contact)
requestedMotion = MotionRequest::stand;
if(forceStand && (lastMotion == MotionRequest::walk || lastMotion == MotionRequest::stand))
{
requestedMotion = MotionRequest::stand;
forceStand = false;
}
// check if the target motion can be the requested motion (mainly if leaving is possible)
if((lastMotion == MotionRequest::walk && (!&theWalkingEngineOutput || theWalkingEngineOutput.isLeavingPossible || !theGroundContactState.contact)) ||
lastMotion == MotionRequest::stand || // stand can always be left
(lastMotion == MotionRequest::specialAction && theSpecialActionsOutput.isLeavingPossible) ||
(lastMotion == MotionRequest::kick && theKickEngineOutput.isLeavingPossible) ||
(lastMotion == MotionRequest::getUp && theGetUpEngineOutput.isLeavingPossible)) //never immediatly leave kick or get up
{
motionSelection.targetMotion = requestedMotion;
}
if(requestedMotion == MotionRequest::specialAction)
{
motionSelection.specialActionRequest = theMotionRequest.specialActionRequest;
}
else
{
motionSelection.specialActionRequest = SpecialActionRequest();
if(motionSelection.targetMotion == MotionRequest::specialAction)
motionSelection.specialActionRequest.specialAction = SpecialActionRequest::numOfSpecialActionIDs;
}
// increase / decrease all ratios according to target motion
const unsigned deltaTime(theFrameInfo.getTimeSince(lastExecution));
const int interpolationTime = prevMotion == MotionRequest::specialAction && lastActiveSpecialAction == SpecialActionRequest::playDead ? playDeadDelay : interpolationTimes[motionSelection.targetMotion];
float delta((float)deltaTime / interpolationTime);
ASSERT(SystemCall::getMode() == SystemCall::logfileReplay || delta > 0.00001f);
float sum(0);
for(int i = 0; i < MotionRequest::numOfMotions; i++)
{
if(i == motionSelection.targetMotion)
motionSelection.ratios[i] += delta;
else
motionSelection.ratios[i] -= delta;
motionSelection.ratios[i] = std::max(motionSelection.ratios[i], 0.0f); // clip ratios
sum += motionSelection.ratios[i];
}
ASSERT(sum != 0);
// normalize ratios
for(int i = 0; i < MotionRequest::numOfMotions; i++)
{
motionSelection.ratios[i] /= sum;
if(std::abs(motionSelection.ratios[i] - 1.f) < 0.00001f)
motionSelection.ratios[i] = 1.f; // this should fix a "motionSelection.ratios[motionSelection.targetMotion] remains smaller than 1.f" bug
}
if(motionSelection.ratios[MotionRequest::specialAction] < 1.f)
{
if(motionSelection.targetMotion == MotionRequest::specialAction)
motionSelection.specialActionMode = MotionSelection::first;
else
motionSelection.specialActionMode = MotionSelection::deactive;
}
else
motionSelection.specialActionMode = MotionSelection::active;
if(motionSelection.specialActionMode == MotionSelection::active && motionSelection.specialActionRequest.specialAction != SpecialActionRequest::numOfSpecialActionIDs)
lastActiveSpecialAction = motionSelection.specialActionRequest.specialAction;
}
lastExecution = theFrameInfo.time;
if(lastMotion != motionSelection.targetMotion)
prevMotion = lastMotion;
lastMotion = motionSelection.targetMotion;
PLOT("module:MotionSelector:ratios:walk", motionSelection.ratios[MotionRequest::walk]);
PLOT("module:MotionSelector:ratios:stand", motionSelection.ratios[MotionRequest::stand]);
PLOT("module:MotionSelector:ratios:specialAction", motionSelection.ratios[MotionRequest::specialAction]);
PLOT("module:MotionSelector:lastMotion", lastMotion);
PLOT("module:MotionSelector:prevMotion", prevMotion);
PLOT("module:MotionSelector:targetMotion", motionSelection.targetMotion);
}
/*
* The Fourier transform of a Gaussian
*/
void gaussian(void) {
plot_t p;
p.outfile = "output1.png";
p.title = "Gaussian";
p.xlabel = "t [s]";
p.ylabel = "x(t) [m]";
p.xmin = -N/2*dt;
p.xmax = N/2*dt;
init_gnuplot(&p);
gsl_complex_packed_array data = calloc(2*N, sizeof(double));
BEGIN_SCATTER(p, "x(t)");
double t = -N/2 * dt;
for (int i = 0; i < 2*N; i += 2) {
data[i] = x(t);
PLOT(p, t, data[i]);
t += dt;
}
END_PLOT(p);
gsl_fft_complex_radix2_forward(data, 1, N);
p.outfile = "output2.png";
p.title = "FFT of a Gauissian";
p.xlabel = "f [Hz]";
p.ylabel = "X(f) [m]";
p.xmin = -512;
p.xmax = 512;
init_gnuplot(&p);
fprintf(p.fp, "plot '-' title 'Real',"
"'-' title 'Imag',"
"'-' with lines title 'Analytical'\n");
// Real part
double f = -1.0/(2.0*dt);
for (int i = N; i < 2*N; i += 2) {
PLOT(p, f, creal(shift(f))*(data[i]/(double) N));
f += 1.0;
}
for (int i = 0; i < N; i += 2) {
PLOT(p, f, creal(shift(f))*(data[i]/(double) N));
f += 1.0;
}
// Imag. part
NEW_PLOT(p);
f = -1.0/(2.0*dt);
for (int i = N+1; i <= 2*N; i += 2) {
PLOT(p, f, cimag(shift(f))*data[i]/(double) N);
f += 1.0;
}
for (int i = 1; i <= N; i += 2) {
PLOT(p, f, cimag(shift(f))*data[i]/(double) N);
f += 1.0;
}
// Analytical
NEW_PLOT(p);
f = -1.0/(2.0*dt);
for (int i = 1; i <= 2*N; i += 2) {
PLOT(p, f, X(f));
f += 1.0;
}
END_PLOT(p);
free(data);
}
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
}
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 GroundContactDetector::update(GroundContactState& groundContactState)
{
MODIFY("module:GroundContactDetector:parameters", p);
PLOT("module:GroundContactDetector:groundContact", groundContactState.contact ? 0.75 : 0.25);
#ifdef TARGET_ROBOT
if(p.forceContact)
#endif
{
groundContactState.contact = true;
//groundContactState.contactSafe = true;
return;
}
// states
ContactState stateFsrLeft = checkFsr(true);
ContactState stateFsrRight = checkFsr(false);
ContactState stateLoad = checkLoad();
// contact plots
PLOT("module:GroundContactDetector:contactLoad", stateLoad.contact ? 0.75 : 0.25);
PLOT("module:GroundContactDetector:contactFsrLeft", stateFsrLeft.contact ? 0.75 : 0.25);
PLOT("module:GroundContactDetector:contactFsrRight", stateFsrRight.contact ? 0.75 : 0.25);
// confidence plots
PLOT("module:GroundContactDetector:confidenceLoad", stateLoad.confidence);
PLOT("module:GroundContactDetector:confidenceFsrLeft", stateFsrLeft.confidence);
PLOT("module:GroundContactDetector:confidenceFsrRight", stateFsrRight.confidence);
float confidenceContact = 0.f;
float confidenceNoContact = 0.f;
if(stateFsrLeft.contact) confidenceContact += stateFsrLeft.confidence;
else confidenceNoContact += stateFsrLeft.confidence;
if(stateFsrRight.contact) confidenceContact += stateFsrRight.confidence;
else confidenceNoContact += stateFsrRight.confidence;
if(stateLoad.contact) confidenceContact += stateLoad.confidence;
else confidenceNoContact += stateLoad.confidence;
confidenceContactBuffer.add(confidenceContact);
confidenceNoContactBuffer.add(confidenceNoContact);
if(confidenceContactBuffer.getSum() >= BUFFER_SIZE * p.contactThreshold)
contact = true;
else if(confidenceNoContactBuffer.getSum() >= BUFFER_SIZE * p.noContactThreshold)
contact = false;
groundContactState.contact = contact || theMotionRequest.motion == MotionRequest::specialAction || theMotionInfo.motion == MotionRequest::specialAction;
PLOT("module:GroundContactDetector:contactThreshold", BUFFER_SIZE * p.contactThreshold);
PLOT("module:GroundContactDetector:noContactThreshold", BUFFER_SIZE * p.noContactThreshold);
PLOT("module:GroundContactDetector:confidenceContact", confidenceContactBuffer.getSum());
PLOT("module:GroundContactDetector:confidenceNoContact", confidenceNoContactBuffer.getSum());
if((contact && !lastContact) || (contact && contactStartTime == 0))
contactStartTime = theFrameInfo.time;
//groundContactState.contactSafe = contact && theFrameInfo.getTimeSince(contactStartTime) >= p.safeContactTime;
if(!contact && lastContact)
{
#ifndef TARGET_SIM
if(contactStartTime != 0 && theMotionInfo.motion == MotionRequest::walk)
SoundPlayer::play("high.wav");
#endif
}
lastContact = contact;
}
void KickEngineData::ModifyData(const KickRequest& br, JointRequest& kickEngineOutput, std::vector<KickEngineParameters>& params)
{
auto& p = params.back();
MODIFY("module:KickEngine:newKickMotion", p);
strcpy(p.name, "newKick");
MODIFY("module:KickEngine:px", gyroP.x());
MODIFY("module:KickEngine:dx", gyroD.x());
MODIFY("module:KickEngine:ix", gyroI.x());
MODIFY("module:KickEngine:py", gyroP.y());
MODIFY("module:KickEngine:dy", gyroD.y());
MODIFY("module:KickEngine:iy", gyroI.y());
MODIFY("module:KickEngine:formMode", formMode);
MODIFY("module:KickEngine:lFootTraOff", limbOff[Phase::leftFootTra]);
MODIFY("module:KickEngine:rFootTraOff", limbOff[Phase::rightFootTra]);
MODIFY("module:KickEngine:lFootRotOff", limbOff[Phase::leftFootRot]);
MODIFY("module:KickEngine:rFootRotOff", limbOff[Phase::rightFootRot]);
MODIFY("module:KickEngine:lHandTraOff", limbOff[Phase::leftArmTra]);
MODIFY("module:KickEngine:rHandTraOff", limbOff[Phase::rightArmTra]);
MODIFY("module:KickEngine:lHandRotOff", limbOff[Phase::leftHandRot]);
MODIFY("module:KickEngine:rHandRotOff", limbOff[Phase::rightHandRot]);
//Plot com stabilizing
PLOT("module:KickEngine:comy", robotModel.centerOfMass.y());
PLOT("module:KickEngine:diffy", actualDiff.y());
PLOT("module:KickEngine:refy", ref.y());
PLOT("module:KickEngine:comx", robotModel.centerOfMass.x());
PLOT("module:KickEngine:diffx", actualDiff.x());
PLOT("module:KickEngine:refx", ref.x());
PLOT("module:KickEngine:lastdiffy", toDegrees(lastBody.y()));
PLOT("module:KickEngine:bodyErrory", toDegrees(bodyError.y()));
for(int i = 0; i < Phase::numOfLimbs; i++)
{
const int stiffness = limbOff[i] ? 0 : 100;
switch(static_cast<Phase::Limb>(i))
{
case Phase::leftFootTra:
kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipRoll] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipPitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lKneePitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnklePitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnkleRoll] = stiffness;
break;
case Phase::rightFootTra:
kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipRoll] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipPitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rKneePitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnklePitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnkleRoll] = stiffness;
break;
case Phase::leftFootRot:
kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnklePitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lAnkleRoll] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipYawPitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipYawPitch] = stiffness;
break;
case Phase::rightFootRot:
kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnklePitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rAnkleRoll] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rHipYawPitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lHipYawPitch] = stiffness;
break;
case Phase::leftArmTra:
kickEngineOutput.stiffnessData.stiffnesses[Joints::lShoulderPitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lShoulderRoll] = stiffness;
break;
case Phase::rightArmTra:
kickEngineOutput.stiffnessData.stiffnesses[Joints::rShoulderPitch] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rShoulderRoll] = stiffness;
break;
case Phase::leftHandRot:
kickEngineOutput.stiffnessData.stiffnesses[Joints::lElbowRoll] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lElbowYaw] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lWristYaw] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::lHand] = stiffness;
break;
case Phase::rightHandRot:
kickEngineOutput.stiffnessData.stiffnesses[Joints::rElbowRoll] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rElbowYaw] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rWristYaw] = stiffness;
kickEngineOutput.stiffnessData.stiffnesses[Joints::rHand] = stiffness;
break;
}
}
}
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;
}