SideConfidenceProvider::BallModelSideConfidence SideConfidenceProvider::computeCurrentBallConfidence()
{
// Some constant parameters
const float distanceObserved = lastBallObservation.abs();
const float angleObserved = lastBallObservation.angle();
const float& camZ = theCameraMatrix.translation.z;
const float distanceAsAngleObserved = (pi_2 - atan2(camZ,distanceObserved));
// Weighting for original pose
float originalWeighting = computeAngleWeighting(angleObserved, theCombinedWorldModel.ballStateOthers.position, theRobotPose,
parameters.standardDeviationBallAngle);
originalWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theCombinedWorldModel.ballStateOthers.position, theRobotPose,
camZ, parameters.standardDeviationBallDistance);
// Weighting for mirrored pose
const Pose2D mirroredPose = Pose2D(pi) + (Pose2D)(theRobotPose);
float mirroredWeighting = computeAngleWeighting(angleObserved, theCombinedWorldModel.ballStateOthers.position, mirroredPose,
parameters.standardDeviationBallAngle);
mirroredWeighting *= computeDistanceWeighting(distanceAsAngleObserved, theCombinedWorldModel.ballStateOthers.position, mirroredPose,
camZ, parameters.standardDeviationBallDistance);
// Decide state based on weights
if((mirroredWeighting > parameters.weightingFactor * originalWeighting) || (originalWeighting > parameters.weightingFactor * mirroredWeighting))
return mirroredWeighting > originalWeighting ? MIRROR : OK;
return UNKNOWN;
}
Pose2D Pose2D::random(const Range<>& x,
const Range<>& y,
const Range<>& angle)
{
// angle should even work in wrap around case!
return Pose2D(float(::randomFloat() * (angle.max - angle.min) + angle.min),
Vector2<>(float(::randomFloat() * (x.max - x.min) + x.min),
float(::randomFloat() * (y.max - y.min) + y.min)));
}
void SelfLocator::motionUpdate(bool noise)
{
Pose2D odometryOffset = bb->theOdometryData - lastOdometry;
const float transNoise = noise ? parameter->translationNoise : 0;
const float rotNoise = noise ? parameter->rotationNoise : 0;
const int transX = (int) odometryOffset.translation.x;
const int transY = (int) odometryOffset.translation.y;
const float dist = odometryOffset.translation.abs();
const float angle = abs(odometryOffset.rotation);
lastOdometry += Pose2D(odometryOffset.rotation, (float) transX, (float) transY);
// precalculate rotational error that has to be adapted to all samples
const float rotError = max(rotNoise,
max(dist * parameter->movedDistWeight,
angle * parameter->movedAngleWeight));
// precalculate translational error that has to be adapted to all samples
const int transXError = (int) max(transNoise,
(float) max(abs(transX * parameter->majorDirTransWeight),
abs(transY * parameter->minorDirTransWeight)));
const int transYError = (int) max(transNoise,
(float) max(abs(transY * parameter->majorDirTransWeight),
abs(transX * parameter->minorDirTransWeight)));
for(int i = 0; i < samples->size(); i++)
{
Sample& s(samples->at(i));
// the translational error vector
const Vector2<int> transOffset((((transX - transXError) << 10) + (transXError << 1) * ((rand() & 0x3ff) + 1) + 512) >> 10,
(((transY - transYError) << 10) + (transYError << 1) * ((rand() & 0x3ff) + 1) + 512) >> 10);
// update the sample
s.translation = Vector2<int>(((s.translation.x << 10) + transOffset.x * s.rotation.x - transOffset.y * s.rotation.y + 512) >> 10,
((s.translation.y << 10) + transOffset.x * s.rotation.y + transOffset.y * s.rotation.x + 512) >> 10);
s.angle += odometryOffset.rotation + (randomFloat() * 2 - 1) * rotError;
s.angle = normalize(s.angle);
s.rotation = Vector2<int>(int(cos((float)s.angle) * 1024), int(sin((float)s.angle) * 1024));
// clip to field boundary
Vector2<> translationAsDouble((float) s.translation.x, (float) s.translation.y);
bb->theFieldDimensions.clipToCarpet(translationAsDouble);
s.translation.x = static_cast<int>(translationAsDouble.x);
s.translation.y = static_cast<int>(translationAsDouble.y);
}
}
void SelfLocator::preExecution(RobotPose& robotPose)
{
sampleTemplateGenerator.bufferNewPerceptions();
// Reset all weightings to 1.0f
for(int i = 0; i < samples->size(); ++i)
samples->at(i).weighting = 1.0f;
//DEBUG_RESPONSE("module:SelfLocator:createFieldModel", fieldModel.create(););
// Table for checking point / line validity:
//DEBUG_RESPONSE("module:SelfLocator:PerceptValidityChecker:saveGoalNetTable", perceptValidityChecker->saveGoalNetTable(););
//DEBUG_RESPONSE("module:SelfLocator:PerceptValidityChecker:recomputeGoalNetTable", perceptValidityChecker->computeGoalNetTable(););
// change module for pose calculation or use debug request to reload the pose calculator
PoseCalculatorType newPoseCalculatorType(poseCalculatorType);
//MODIFY_ENUM("module:SelfLocator:poseCalculatorType", newPoseCalculatorType);
bool reloadPoseCalculator = (newPoseCalculatorType != poseCalculatorType);
//DEBUG_RESPONSE("module:SelfLocator:reloadPoseCalculator", reloadPoseCalculator = true;);
if(reloadPoseCalculator)
{
delete poseCalculator;
switch(newPoseCalculatorType)
{
case POSE_CALCULATOR_2D_BINNING:
poseCalculator = new PoseCalculator2DBinning< Sample, SampleSet<Sample>, 10 >
(*samples, bb->theFieldDimensions);
break;
case POSE_CALCULATOR_PARTICLE_HISTORY:
poseCalculator = new PoseCalculatorParticleHistory< Sample, SampleSet<Sample> >(*samples);
poseCalculator->init();
break;
case POSE_CALCULATOR_BEST_PARTICLE:
poseCalculator = new PoseCalculatorBestParticle< Sample, SampleSet<Sample> >(*samples);
break;
case POSE_CALCULATOR_OVERALL_AVERAGE:
poseCalculator = new PoseCalculatorOverallAverage< Sample, SampleSet<Sample> >(*samples);
break;
case POSE_CALCULATOR_K_MEANS_CLUSTERING:
poseCalculator = new PoseCalculatorKMeansClustering< Sample, SampleSet<Sample>, 5, 1000 >
(*samples, bb->theFieldDimensions);
break;
default:
ASSERT(false);
}
poseCalculatorType = newPoseCalculatorType;
}
// if the considerGameState flag is set, reset samples when necessary
if(parameter->considerGameState)
{
// penalty shoot: if game state switched to playing reset samples to start pos
if(bb->theGameInfo.secondaryState == STATE2_PENALTYSHOOT)
{
if((gameInfoGameStateLastFrame == STATE_SET && bb->theGameInfo.state == STATE_PLAYING) ||
(gameInfoPenaltyLastFrame != PENALTY_NONE && bb->theRobotInfo.penalty == PENALTY_NONE))
{
vector<Pose2D> poses;
vector<Pose2D> stdDevs;
//this only works for the penalty shootout, but since there's no
//way to detect a penalty shootout from within the game..... =|
if(bb->theOwnTeamInfo.teamColour == TEAM_RED)
{
//striker pose (center of the pitch)
poses.push_back(Pose2D(0.0f, 0.0f, 0.0f));
stdDevs.push_back(Pose2D(0.2f, 200, 200));
}
else
{
//goalie pose (in the center of the goal)
poses.push_back(Pose2D(0.0f, static_cast<float>(bb->theFieldDimensions.xPosOwnGoalpost), 0.0f));
stdDevs.push_back(Pose2D(0.2f, 200, 200));
}
initSamplesAtGivenPositions(poses, stdDevs);
}
}
// normal game: if penalty is over reset samples to reenter positions
else if(bb->theGameInfo.secondaryState == STATE2_NORMAL)
{
if(gameInfoPenaltyLastFrame != PENALTY_NONE && bb->theRobotInfo.penalty == PENALTY_NONE)
{
vector<Pose2D> poses;
vector<Pose2D> stdDevs;
poses.push_back(Pose2D(-pi / 2.0f, 0.0f, (float) bb->theFieldDimensions.yPosLeftSideline));
poses.push_back(Pose2D(pi / 2.0f, 0.0f, (float) bb->theFieldDimensions.yPosRightSideline));
stdDevs.push_back(Pose2D(0.2f, 200, 200));
stdDevs.push_back(Pose2D(0.2f, 200, 200));
initSamplesAtGivenPositions(poses, stdDevs);
}
}
gameInfoPenaltyLastFrame = bb->theRobotInfo.penalty;
gameInfoGameStateLastFrame = bb->theGameInfo.state;
}
// Initialize sample set again:
//DEBUG_RESPONSE("module:SelfLocator:resetSamples", init(););
// Set team color in robot pose
//robotPose.ownTeamColorForDrawing = theOwnTeamInfo.teamColor == TEAM_BLUE ? ColorRGBA(0, 0, 255) : ColorRGBA(255, 0, 0);
}
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 ObstacleCombinator::generateObstacleFromCurrentCluster(std::vector<ObstacleModel::Obstacle>& obstacles)
{
Vector2<int>& c = currentCluster.cells[0];
int xMin(c.x);
int yMin(c.y);
int xMax(c.x);
int yMax(c.y);
float rightAngle = 10.0;
float leftAngle = -10.0;
Vector2<int> rightCorner;
Vector2<int> leftCorner;
Vector2<> centerCells;
Vector2<int> robotPosition(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2);
Vector2<int> closestPoint(USObstacleGrid::GRID_LENGTH, USObstacleGrid::GRID_LENGTH);
int closestPointSqrDist(USObstacleGrid::GRID_SIZE * 2);
for(unsigned int i = 0; i < currentCluster.cells.size(); ++i)
{
c = currentCluster.cells[i];
centerCells.x += c.x;
centerCells.y += c.y;
Vector2<int> point(c.x - robotPosition.x, c.y - robotPosition.y);
int sqrDistToRobot = sqr(point.x) + sqr(point.y);
float angleToRobot = atan2(static_cast<float>(point.y), static_cast<float>(point.x));
if(angleToRobot < rightAngle)
{
rightAngle = angleToRobot;
rightCorner = Vector2<int>(c.x, c.y);
}
if(angleToRobot > leftAngle)
{
leftAngle = angleToRobot;
leftCorner = Vector2<int>(c.x, c.y);
}
if(sqrDistToRobot < closestPointSqrDist)
{
closestPoint = c;
closestPointSqrDist = sqrDistToRobot;
}
if(c.x < xMin)
xMin = c.x;
else if(c.x > xMax)
xMax = c.x;
if(c.y < yMin)
yMin = c.y;
else if(c.y > yMax)
yMax = c.y;
}
centerCells /= static_cast<float>(currentCluster.cells.size());
const float angleToCenterPoint = Geometry::angleTo(Pose2D(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2), centerCells); //calculates the angle to the center of the cluster in grid coordinate system (independent of robot rotation)
const float cosinus = cos(-angleToCenterPoint);
const float sinus = sin(-angleToCenterPoint);
float newX;
float newY;
//initializing of the rectangle
float xMinRotated(FLT_MAX);
float yMinRotated(FLT_MAX);
float xMaxRotated(-FLT_MAX);
float yMaxRotated(-FLT_MAX);
for(unsigned int i = 0; i < currentCluster.cells.size(); ++i)
{
newX = cosinus * currentCluster.cells[i].x - sinus * currentCluster.cells[i].y; // rotates each cell of the cluster
newY = sinus * currentCluster.cells[i].x + cosinus * currentCluster.cells[i].y;
//sets new values for rectangle
if(newX < xMinRotated)
xMinRotated = newX;
else if(newX > xMaxRotated)
xMaxRotated = newX;
if(newY < yMinRotated)
yMinRotated = newY;
else if(newY > yMaxRotated)
yMaxRotated = newY;
}
Vector2<> closestPointWorld = gridToWorld(Vector2<>(closestPoint.x + 0.5f, closestPoint.y + 0.5f));
Vector2<> centerWorld = gridToWorld(centerCells);
//expansion (length of x- and y-axis through the center point) and orientation (dependent on robot rotation) of the cluster
Vector3<> covarianceEllipse(((xMaxRotated - xMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, ((yMaxRotated - yMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, atan2(centerWorld.y, centerWorld.x));
Matrix2x2<> covariance(covarianceEllipse.x, 0, 0, covarianceEllipse.y); // covariance is initialised with uncorrelated values (only expansion of cluster as variances)
rotateMatrix(covariance, covarianceEllipse.z); // rotates the covariance so that it fits to the orientation and expansion of the cluster
obstacles.push_back(ObstacleModel::Obstacle(gridToWorld(Vector2<>((float)(leftCorner.x), (float)(leftCorner.y))),
gridToWorld(Vector2<>((float)(rightCorner.x), (float)(rightCorner.y))), centerWorld, closestPointWorld,
static_cast<int>(currentCluster.cells.size()), covariance));
}
Vector2<> Geometry::vectorTo(const Pose2D& from, const Vector2<>& to)
{
return (Pose2D(to) - from).translation;
}
float Geometry::distanceTo(const Pose2D& from, const Vector2<>& to)
{
return (Pose2D(to) - from).translation.abs();
}
float Geometry::angleTo(const Pose2D& from, const Vector2<>& to)
{
Pose2D relPos = Pose2D(to) - from;
return atan2(relPos.translation.y, relPos.translation.x);
}
// Pose is the average of all the particles
const Pose2D& ParticleFilter::pose() const {
if(dirty_) {
typedef Point2D Centroid;
// k means clustering
//
// Initialize random centroids
set<Centroid, centroid_comp_> centroids;
for (int i = 0; i < NUM_CLUSTERS; i++) {
Centroid centroid;
centroid.x = (rand() % (2*2500)) - 2500;
centroid.y = (rand() % (2*1250)) - 1250;
centroids.insert(centroid);
}
// Centroid centroid;
// centroid.x = -1000;
// centroid.y = 0;
// centroids.insert(centroid);
//
// centroid.x = 1000;
// centroid.y = 0;
// centroids.insert(centroid);
// cout << "========> BEGIN K-MEANS <========" << endl;
// cout << "PARTICLES" << endl;
// for (auto p : particles())
// cout << p.x << " " << p.y << endl;
// cout << endl;
////
// cout << "Random Centroids" << endl;
// for (auto centroid : centroids)
// cout << centroid.x << " " << centroid.y << endl;
// cout << endl;
// Iterate until convergence
map<Centroid, set<Particle, particle_comp_>, centroid_comp_> closest_particles;
set<Point2D, centroid_comp_> new_centroids = centroids;
map<Centroid, int, centroid_comp_> cluster_size;
int i = 0;
do {
// cout << "PARTICLES" << endl;
// for (auto p : particles())
// cout << p.x << " " << p.y << endl;
// cout << endl;
centroids = new_centroids;
new_centroids.clear();
cluster_size.clear();
// Compute the closest centroid to each particle
double closest_centroid_distance = 50000;
Centroid closest_centroid = NULL;
for (Particle p : particles()) {
for (Centroid centroid : centroids) {
// cout << "Current closest distance: " << closest_centroid_distance << endl;
double distance = sqrt(pow(p.x-centroid.x, 2) + pow(p.y-centroid.y, 2));
// Record the closest centroid
// cout << "Particle " << p.x << " " << p.y << endl;
// cout << "Comparing centroid: " << centroid.x << " " << centroid.y << endl;
// cout << "Proposal Distance: " << distance << endl;
if (distance < closest_centroid_distance) {
// cout << "Proposed distance is closer!!!" << endl;
closest_centroid_distance = distance;
closest_centroid = centroid;
}
}
// cout << "C(" << closest_centroid.x << " " << closest_centroid.y << ") -> P(" << p.x << " " << p.y << ")" << endl;
closest_particles[closest_centroid].insert(p);
// Record the closest centroid
// cout << "Particle " << p.x << " " << p.y << endl;
// cout << "Closest centroid: " << closest_centroid.x << " " << closest_centroid.y << endl;
// cout << "Centroid distance: " << closest_centroid_distance << endl << endl;
// for (auto c : closest_particles) {
// cout << "CENTROID " << c.first.x << " " << c.first.y << " at this point..." << endl;
// for (auto pp : c.second) {
// cout << pp.x << " " << pp.y << endl;
// }
// cout << endl;
// }
// cout << endl;
closest_centroid = NULL;
closest_centroid_distance = 50000;
}
// // Print closest particles
// for (auto elem : closest_particles) {
// cout << "Centroid: " << elem.first.x << " " << elem.first.y << endl;
//
// for (auto p : elem.second) {
// cout << "Particle: " << p.x << " " << p.y << endl;
// }
//
// cout << endl;
// }
// Recompute the centroids based on the mean of all the closest particles
for (Centroid centroid : centroids) {
double sum_x = 0;
double sum_y = 0;
for (Particle p : closest_particles[centroid]) {
sum_x += p.x;
sum_y += p.y;
}
int num_particles = closest_particles[centroid].size();
if (num_particles > 0) {
new_centroids.insert(Centroid(sum_x/num_particles, sum_y/num_particles));
cluster_size[centroid] = num_particles;
}
}
// cout << "Centroids" << endl;
// for (auto centroid : centroids)
// cout << centroid.x << " " << centroid.y << " (" << cluster_size[centroid] << ")" << endl;
// cout << endl;
//
// cout << "New Centroids" << endl;
// for (auto centroid : new_centroids)
// cout << centroid.x << " " << centroid.y << " (" << cluster_size[centroid] << ")" << endl;
// cout << endl << endl;
i++;
} while (i < 3);
// cout << "Centroids" << endl;
// for (auto centroid : centroids)
// cout << centroid.x << " " << centroid.y << " (" << cluster_size[centroid] << ")" << endl;
// cout << endl;
Centroid best_cluster;
double max_particles = -1;
for (Centroid centroid : centroids) {
if (cluster_size[centroid] > max_particles) {
best_cluster = centroid;
max_particles = cluster_size[centroid];
}
}
// cout << "Best cluster: " << best_cluster.x << " " << best_cluster.y << " (" << cluster_size[best_cluster] << ")" << endl;
//
// cout << "========> END K-MEANS <========" << endl << endl;
// Compute the mean pose estimate
mean_ = Pose2D();
using T = decltype(mean_.translation);
for(const auto& p : particles()) {
mean_.translation += T(p.x,p.y);
mean_.rotation += p.t;
}
if(particles().size() > 0)
mean_ /= particles().size();
// k-means update
mean_.translation = best_cluster;
dirty_ = false;
}
return mean_;
}
void SpecialActions::update(SpecialActionsOutput& specialActionsOutput)
{
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 < JointData::numOfJoints; ++i)
lastRequest.angles[i] = theFilteredJointData.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 hardness
if(wasEndOfSpecialAction)
{
specialActionsOutput.jointHardness.resetToDefault();
if(!mirror)
lastHardnessRequest = currentHardnessRequest;
else
lastHardnessRequest.mirror(currentHardnessRequest);
currentHardnessRequest.resetToDefault();
}
wasEndOfSpecialAction = false;
// search next data, leave on transition to external motion
if(!getNextData(theMotionSelection.specialActionRequest, specialActionsOutput))
{
wasActive = true;
wasEndOfSpecialAction = true;
specialActionsOutput.odometryOffset = Pose2D();
return;
}
}
else
{
dataRepetitionCounter -= int(theFrameInfo.cycleTime * 1000 * speedFactor);
hardnessInterpolationCounter -= 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 = Pose2D(-currentInfo.odometryOffset.rotation, currentInfo.odometryOffset.translation.x, -currentInfo.odometryOffset.translation.y);
else
specialActionsOutput.odometryOffset = currentInfo.odometryOffset;
else
specialActionsOutput.odometryOffset = Pose2D();
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 = JointData::LShoulderPitch; i <= JointData::RElbowRoll; ++i)
if(randomFloat() < 0.25)
specialActionsOutput.angles[i] = JointData::off;
}
wasActive = theMotionSelection.specialActionMode == MotionSelection::active;
}
void CameraMatrixProvider::drawFieldLines(const CameraMatrix& cameraMatrix, const RobotPose& theRobotPose, const FieldDimensions& theFieldDimensions, const CameraInfo& theCameraInfo) const
{
Vector3<> start0C, start1C, end0C, end1C;
Vector2<> start0I, start1I, end0I, end1I;
const Pose2D& robotPoseInv(theRobotPose.invert());
const Pose3D& cameraMatrixInv(cameraMatrix.invert());
int halfFieldLinesWidth = theFieldDimensions.fieldLinesWidth / 2;
for(unsigned int i = 0; i < theFieldDimensions.fieldLines.lines.size(); ++i)
{
Pose2D relativeLine(robotPoseInv);
relativeLine.conc(theFieldDimensions.fieldLines.lines[i].corner);
const Vector2<> start0(Pose2D(relativeLine).translate(0, (float) - halfFieldLinesWidth).translation);
const Vector2<> end1(Pose2D(relativeLine).translate(theFieldDimensions.fieldLines.lines[i].length, (float) halfFieldLinesWidth).translation);
start0C = cameraMatrixInv * Vector3<>(start0.x, start0.y, 0.); // field2camera
end1C = cameraMatrixInv * Vector3<>(end1.x, end1.y, 0.); // field2camera
if(start0C.x <= 200 && end1C.x <= 200)
continue;
const Vector2<>& start1(Pose2D(relativeLine).translate(0, (float) halfFieldLinesWidth).translation);
const Vector2<>& end0(Pose2D(relativeLine).translate(theFieldDimensions.fieldLines.lines[i].length, (float) - halfFieldLinesWidth).translation);
start1C = cameraMatrixInv * Vector3<>(start1.x, start1.y, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(end0.x, end0.y, 0.); // field2camera
if(start0C.x <= 200)
intersectLineWithCullPlane(start0C, end0C - start0C, start0C);
else if(end0C.x <= 200)
intersectLineWithCullPlane(start0C, end0C - start0C, end0C);
if(start1C.x <= 200)
intersectLineWithCullPlane(start1C, end1C - start1C, start1C);
else if(end1C.x <= 200)
intersectLineWithCullPlane(start1C, end1C - start1C, end1C);
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
camera2image(start1C, start1I, theCameraInfo);
camera2image(end1C, end1I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorRGBA(0, 0, 0));
//LINE("module:CameraMatrixProvider:calibrationHelper", start1I.x, start1I.y, end1I.x, end1I.y, 0, Drawings::ps_solid, ColorRGBA(0, 0, 0));
}
start0C = cameraMatrixInv * Vector3<>(100, -11, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(0, -11, 0.); // field2camera
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorClasses::blue);
start0C = cameraMatrixInv * Vector3<>(100, 11, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(0, 11, 0.); // field2camera
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorClasses::blue);
start0C = cameraMatrixInv * Vector3<>(110, 1000, 0.); // field2camera
end0C = cameraMatrixInv * Vector3<>(110, -1000, 0.); // field2camera
camera2image(start0C, start0I, theCameraInfo);
camera2image(end0C, end0I, theCameraInfo);
//LINE("module:CameraMatrixProvider:calibrationHelper", start0I.x, start0I.y, end0I.x, end0I.y, 0, Drawings::ps_solid, ColorClasses::blue);
}
Pose2D OccupancyMapParams::getPoseFromPixel(const Pose2Di& pixel) const {
return Pose2D(pixel.x * metersPerPixel + originOffset.x,
-pixel.y * metersPerPixel + originOffset.y, pixel.theta);
}
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 PreviewWalk::goNextFrame(WalkingEngineOutput& lastWalkOoutput, WalkingEngineOutput& walkingEngineOutput, const NaoStructure& naoStructure)
{
lastWalkStatus = walkStatus;
// Copy from global naoStructure, as we will temporary modify this buffer for some calculation
naoStructTmp = naoStructure;
//first check for cmd change
if(isCmdPending && isCmdChangePossible() && !(lastWalkStatus==WalkTypeBase::standing&&pendingCmd.speed==Pose2D(0,0,0)))
{
//std::cout<<"---isCmdPending shoule be true: "<<pendingCmd.speed.translation.x<<" "<<pendingCmd.speed.translation.y<<" "<<pendingCmd.speed.rotation<<std::endl;
executingCmd = pendingCmd;
isCmdPending = false;
// TODO: when we switch from other walk type , we need to reset the generator
if(!preBufferGenrator.hasInitialized()
|| (executingCmd.walkType != lastwalktype/*lastWalkStatus*/ && lastWalkStatus == WalkTypeBase::standing))
{
//std::cout<<"------previewwalk reset---"<<std::endl;
preBufferGenrator.reset(executingCmd,naoStructTmp);
preBufferGenrator.changeSpeed(executingCmd, naoStructTmp);
initialSlowCounter = walkParam.PG;
// TODO: use measured COM Vector(p, vel, acc)
double cposx = naoStructure.pCOM.x;
double cposy = naoStructure.pCOM.y - (naoStructure.uLink[NaoStructure::lFootLink].p.y + naoStructure.uLink[NaoStructure::rFootLink].p.y)/2 ;
previewController.resetController(Vector3<double>(cposx,0,0), Vector3<double>(cposy,0,0));
pRefCoMLast = naoStructure.pCOM;
}else
{
preBufferGenrator.changeSpeed(executingCmd, naoStructTmp);
if (lastWalkStatus == WalkTypeBase::standing)
{
double cposx = naoStructure.pCOM.x;
double cposy = naoStructure.pCOM.y - (naoStructure.uLink[NaoStructure::lFootLink].p.y + naoStructure.uLink[NaoStructure::rFootLink].p.y)/2 ;
previewController.resetController(Vector3<double>(cposx,0,0), Vector3<double>(cposy,0,0));
pRefCoMLast = naoStructure.pCOM;
}
//std::cout<<"------previewwalk changeSpeed---"<<std::endl;
}
lastwalktype = executingCmd.walkType;
}
if(!preBufferGenrator.hasInitialized())
return;
//=======
PreviewState state;
PreviewBuffer& previewBuffer = preBufferGenrator.getPreviewBuffer();
if(!previewBuffer.empty())
state= previewBuffer.front();
if(state.walkStatus == WalkTypeBase::standing){
walkStatus = WalkTypeBase::standing;
//std::cout<<"------------standing standing standing ! ! !---------"<<std::endl;
walkingEngineOutput.isLeavingPossible = true;
return;
}
else{
walkingEngineOutput.isLeavingPossible = false;
}
int lastSupMode = state.supmod;
// lastWalkStatus = state.walkStatus
// preBufferGenrator go Next and generate new knots and state
preBufferGenrator.generateNext();
if(!previewBuffer.empty())
state = previewBuffer.front();
sup_mod = state.supmod;
walkStatus = state.walkStatus;
naoStructTmp.supportingMode = sup_mod;
//std::cout<<"------------WalkTypeBase: "<<walkStatus<<std::endl; //start 1; ending 2; running 3; standing 4;
if (walkStatus!=WalkTypeBase::ending&&lastWalkStatus==WalkTypeBase::ending)
{
preBufferGenrator.finishEnding();
}
/*if(walkStatus == WalkTypeBase::standing){
std::cout<<"------------standing standing standing ! ! !---------"<<std::endl;
smoothJoints(walkingEngineOutput, lastWalkOoutput);
lastWalkOoutput = walkingEngineOutput;
walkingEngineOutput.isLeavingPossible = true;
preBufferGenrator.generateNext();
return;
}*/
// Foot positioning modification according to zmp error and last walk status
// if(state.walkStatus == WalkTypeBase::running && previewBuffer[1].supmod != sup_mod &&
// (sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_RIGHT || sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT) )
// {
// Vector3<double> zmpm = RotationMatrix(0, 0, lastState.rSupFootZMP.z) *pRobotState->mZMP + lastState.pSupFootZMP;
// Vector3<double> zmperror = state.pZMP - zmpm;
// preBufferGenrator.doLandingEvent(zmperror, Vector3<double>(0,0,0));
// }
// TODO: If we use Sensor to detect supporting mode, the code bellow should be modified!!
// When Supporting Leg Changed, modify pCOM to value relative to current supporting Foot
if(lastSupMode != sup_mod &&
((sup_mod == NaoConfig::SUPPORT_MODE_LEFT || sup_mod == NaoConfig::SUPPORT_MODE_RIGHT) ||
(lastWalkStatus != walkStatus && lastWalkStatus == WalkTypeBase::standing)))
{
naoStructTmp.uLink[NaoStructure::bodyLink].p = Vector3<double>(0,0,0);
naoStructTmp.uLink[NaoStructure::bodyLink].R = RotationMatrix();
Kinematics::execForwardKinematics(naoStructTmp, NaoStructure::bodyLink);
Kinematics::recalcBodyPos2SupFoot(naoStructTmp, Vector3<double>(0,0,0), naoStructTmp.supportingMode);
Kinematics::execForwardKinematics(naoStructTmp, NaoStructure::bodyLink);
naoStructTmp.pCOM = Kinematics::calcCOM(naoStructTmp);
//In case we get NAN or IND num.... print some thing and halt execution when run on robot!!
ASSERT(naoStructTmp.pCOM == naoStructTmp.pCOM);
}
//===== Check Landing Step Error=============================
if(false && state.walkStatus == WalkTypeBase::running && lastSupMode != sup_mod &&
(sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_RIGHT || sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT) )
{
Vector3<double> pRealLandFootW,rRealLandFootW;
Vector3<double> pRealLandFoot2Sup;
Vector3<double> pRefLandFootW, rRefLandFootW;
const PreBufferGenerator::Knot* pKnot = 0;
int landFoot = 0;
if(sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_RIGHT)
landFoot = NaoStructure::lFootLink;
else if(sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
landFoot = NaoStructure::rFootLink;
pRealLandFoot2Sup = naoStructTmp.getPosition(landFoot, NaoStructure::Space_Supprot_Leg);
pRealLandFoot2Sup.z = 0;
pRealLandFootW = state.pSupFootZMP + RotationMatrix::getRotationZ(state.rSupFootZMP.z) * pRealLandFoot2Sup;
rRealLandFootW = state.rSupFootZMP;
rRealLandFootW.z += naoStructTmp.getRotAngles(landFoot, NaoStructure::Space_Supprot_Leg).z;
pKnot = preBufferGenrator.findNextKnot();
ASSERT(pKnot);
pRefLandFootW = pKnot->pSupFootZMP;
rRefLandFootW = pKnot->rSupFootZMP;
Vector3<double> pError = (pRefLandFootW - pRealLandFootW), rError(rRefLandFootW - rRealLandFootW);
preBufferGenrator.doLandingEvent(pError, rError);
// TODO: check here?
preBufferGenrator.generateNext();
if(!previewBuffer.empty())
state = previewBuffer.front();
sup_mod = state.supmod;
walkStatus = state.walkStatus;
naoStructTmp.supportingMode = sup_mod;
}
// ==== Get Swing Foot Infomation
int iswgleg = NaoConfig::LegIDLeft;
if(sup_mod == NaoConfig::SUPPORT_MODE_LEFT || sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
iswgleg= NaoConfig::LegIDRight;
Vector3<double> pSwgFootW, rSwgFootW;
preBufferGenrator.getCurrentSwgF(pSwgFootW, rSwgFootW, iswgleg);
pSwgFootW += Vector3<double>(0,0, NaoConfig::getInstance()->FootHeight);// TODO: muliply foot rotation
Vector3<double> pSwgFoot2Sup, rSwgFoot2Sup;
pSwgFoot2Sup = RotationMatrix::getRotationZ(-state.rSupFootZMP.z) * (pSwgFootW - state.pSupFootZMP);
rSwgFoot2Sup = rSwgFootW - state.rSupFootZMP;
// ====== CoM Control=====================
double bRoll = pSensorData->data[SensorData::angleX];
double bTilt = pSensorData->data[SensorData::angleY];
RotationMatrix Rbw = RotationMatrix().rotateX(bRoll).rotateY(bTilt);
RotationMatrix Rfw;//rotation of supporting foot in world cooridnate
if(naoStructTmp.supportingMode == NaoConfig::SUPPORT_MODE_LEFT || naoStructTmp.supportingMode == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
{
Rfw = naoStructTmp.uLink[NaoStructure::lFootLink].R * naoStructTmp.uLink[NaoStructure::bodyLink].R.invert();
}else
{
Rfw = naoStructTmp.uLink[NaoStructure::rFootLink].R * naoStructTmp.uLink[NaoStructure::bodyLink].R.invert();
}
Rfw = RotationMatrix::getRotationZ(state.rSupFootZMP.z) * Rfw * Rbw;
// check world com state
Vector3<double> comW = Rfw * naoStructure.getCoM(NaoStructure::Space_Supprot_Leg)+ lastState.pSupFootZMP;
Vector3<double> comWv = (comW - lastCoMW) / walkParam.dt;
Vector3<double> comWa = Rbw * Vector3<double>(pSensorData->data[SensorData::accX], pSensorData->data[SensorData::accY], pSensorData->data[SensorData::accZ]);
PrviewControllerState inputState;
inputState.p = comW; inputState.v = comWv; inputState.a = comWa;
Vector3<double> zmpW = RotationMatrix(0, 0, lastState.rSupFootZMP.z) *pRobotState->mZMP + lastState.pSupFootZMP;
//=======Compute step modification according to ZMP error and CoM information=========
Vector3<double> ezmp =pRobotState->mZMP - RotationMatrix(0, 0, -state.rSupFootZMP.z)* (state.pZMP - state.pSupFootZMP);// state.pZMP - zmpW;
if(state.supmod == NaoConfig::SUPPORT_MODE_RIGHT || state.supmod == NaoConfig::SUPPORT_MODE_DOUBLE_RIGHT)
ezmp.y = - ezmp.y;
//======Learning Step Modification Process=====
if(Open_Step_Learning)
{
if(state.walkStatus == WalkTypeBase::running){
sumZMPErrorTotal += Vector3<double>(fabs(ezmp.x), fabs(ezmp.y), fabs(ezmp.z));
numZMPErrorAdded ++;
updateCounter ++;
}
if((lastUpdateModelFlag != pRobotState->fallState.updateModel && pRobotState->fallState.updateModel == true)
|| (state.walkStatus == WalkTypeBase::running && updateCounter%600 == 0))
{
double critia1 = (sumZMPErrorTotal.x * 0.3+ sumZMPErrorTotal.y * 0.7)/ numZMPErrorAdded;//if we fall, the zmp error will also be significant
double performance = critia1;
double critia2 = (pi - atan(static_cast<double>(numZMPErrorAdded) * 2 / walkParam.PG -3)) /2;
performance = performance + performance * critia2;//walk longer, we walk better
double critia3 = 0;
if(numStepModify > 0)
critia3 = (sumStepModify.x / numStepModify * 0.5 + sumStepModify.y / numStepModify) * 0.5;
performance += critia3;
learnerAgent.updatePerform(performance);
learnerAgent.getPendingParam(spController);
//std::cout<<"===>LearnCritia: ("<<numZMPErrorAdded<<", "<<sumZMPErrorTotal.x<<", "<<sumZMPErrorTotal.y<<"), "<<critia1<<", "<<critia2<<", "<<critia3<<std::endl;
sumZMPErrorTotal = Vector3<double>(0,0,0);
numZMPErrorAdded = 0;
updateCounter = 1;
sumStepModify = Vector3<double>(0,0,0);
numStepModify = 0;
}
lastUpdateModelFlag = pRobotState->fallState.updateModel;
}
//============
if(Open_Step_Modify || Open_Step_Learning)
{
if(lastSupMode != sup_mod && sup_mod == NaoConfig::SUPPORT_MODE_LEFT)
{
errorZMP.x = 0; errorZMP.y = 0;nErrors = 0;onSeeSupMode = sup_mod;
spController.resetZMPError();
}else if(lastSupMode != sup_mod && sup_mod == NaoConfig::SUPPORT_MODE_RIGHT)
{
errorZMP.x = 0; errorZMP.y = 0;nErrors = 0;onSeeSupMode = sup_mod;
spController.resetZMPError();
}
if(state.walkStatus == WalkTypeBase::running && sup_mod == onSeeSupMode && nErrors < 10)
{
errorZMP += ezmp;
nErrors++;
spController.addZMPError(ezmp);
}
if(sup_mod == onSeeSupMode && nErrors == 10)
{
nErrors++;
//============switch to local system========
RotationMatrix R = RotationMatrix::getRotationZ(-state.rSupFootZMP.z) * Rfw;
Vector3<double> p = R * naoStructure.getCoM(NaoStructure::Space_Supprot_Leg);
Vector3<double> v = RotationMatrix::getRotationZ(-state.rSupFootZMP.z) * inputState.v;
Vector3<double> a = comWa;
if(state.supmod == NaoConfig::SUPPORT_MODE_RIGHT || state.supmod == NaoConfig::SUPPORT_MODE_DOUBLE_RIGHT)
{
p.y = - p.y; v.y = -v.y; a.y = -a.y;
}
Vector3<double> xState(p.x, v.x, a.x), yState(p.y, v.y, a.y);
Vector3<double> dP = spController.getOutput(xState, yState);
// Become effect in next frame....
preBufferGenrator.modifyStepPosition(Vector3<double>(dP.x, dP.y, 0), Vector3<double>(0,0,0), 16);
sumStepModify.x += dP.x * dP.x;
sumStepModify.y += dP.y * dP.y;
numStepModify ++;
//MATLABHELP_PLOT(dPy, "dPy");
//MATLABHELP_PLOT(spController.getSumZMPError(), "mErrrory");
}
}
//=== Get Preview Controller Output============
PrviewControllerOutput outputPC ;
// if(walkStatus != WalkTypeBase::starting && (sup_mod == 1 || sup_mod == 3))
// outputPC = previewController.getRefCOM(previewBuffer, walkParam, inputState);
// else
outputPC = previewController.getRefCOM(previewBuffer, walkParam);
Vector3<double> pCOMRefW(outputPC.p);
// CoM Ref world system to body system
Vector3<double> pCOMRef =RotationMatrix::getRotationZ(-state.rSupFootZMP.z) * (pCOMRefW - state.pSupFootZMP);
//Vector3<double> pCOMRef =Rfw.invert() * (pCOMRefW - state.pSupFootZMP);
Vector3<double> realCOM = naoStructTmp.pCOM;
Vector3<double> dCOM = (pCOMRef - realCOM);
// MATLABHELP_PLOT(dCOM.y, "dCOMy");
// MATLABHELP_PLOT(pCOMRef.y, "pCOMRefy");
//====Add ZMP mesaured feedback
// Vector3<double> zmpError = (lastState.pSupFootZMP - pRobotState->mZMP);
// dCOM -= Vector3<double>(zmpError.x * 0.01, zmpError.y * 0.1, 0);
Vector3<double> refRBody(0,walkParam.torsoAngleY,0);
/* refRBody += Vector3<double>(zmpError.y, zmpError.x * 0.1, 0);*/
double gain = 1;//1.2, 1.8
if(sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT || sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_RIGHT)
gain = 1;
Vector3<double> pBodyNext = dCOM *gain+ naoStructTmp.getPosition(NaoStructure::bodyLink, NaoStructure::Space_Supprot_Leg);
double fRr, fRl;//z rotation of right and left foot
if(sup_mod == NaoConfig::SUPPORT_MODE_LEFT || sup_mod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT){
fRr = rSwgFoot2Sup.z;
fRl = 0;
}else{
fRr = 0;
fRl = rSwgFoot2Sup.z;
}
Vector3<double> equalHipAngles;
double rHipYawPitchAngle;
//search for best rHipYawPitchAngle, Feed angle relative to body...
binarySearchforYawRollPitch(fRr, fRl, equalHipAngles, rHipYawPitchAngle);
RotationMatrix brz = RotationMatrix::getRotationY(walkParam.torsoAngleY);
double zmpErrorY = pRobotState->mZMP.y;
//brz.rotateX(-0.05*mRBody.x).rotateY(-0.05*mRBody.y);
if(state.supmod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT || state.supmod == NaoConfig::SUPPORT_MODE_LEFT)
{
brz.rotateZ(equalHipAngles.z);
Kinematics::calculateSafeLegJoints(walkingEngineOutput, NaoConfig::LegIDLeft, Vector3<double>(0,0,NaoConfig::getInstance()->FootHeight), RotationMatrix(), pBodyNext, brz,
rHipYawPitchAngle, equalHipAngles.x, equalHipAngles.y);
}else// if(state.supmod == NaoConfig::SUPPORT_MODE_DOUBLE_RIGHT)
{
brz.rotateZ(-equalHipAngles.z);
Kinematics::calculateSafeLegJoints(walkingEngineOutput, NaoConfig::LegIDRight, Vector3<double>(0,0,NaoConfig::getInstance()->FootHeight), RotationMatrix(), pBodyNext, brz,
rHipYawPitchAngle, equalHipAngles.x, equalHipAngles.y);
}
//======为消除身体的运动给游脚带来的影响, 先重新计算下Nao Structure
naoStructTmp.setLinksJoints(walkingEngineOutput);
naoStructTmp.uLink[NaoStructure::bodyLink].p = Vector3<double>(0,0,0);
naoStructTmp.uLink[NaoStructure::bodyLink].R = RotationMatrix();
Kinematics::execForwardKinematics(naoStructTmp, NaoStructure::bodyLink);
Kinematics::recalcBodyPos2SupFoot(naoStructTmp, Vector3<double>(0,0,0), naoStructTmp.supportingMode);
Kinematics::execForwardKinematics(naoStructTmp, NaoStructure::bodyLink);
naoStructTmp.pCOM = Kinematics::calcCOM(naoStructTmp);
//In case we get NAN or IND num.... print some thing and halt execution when run on robot!!
ASSERT(naoStructTmp.pCOM == naoStructTmp.pCOM);
//====== Swing Foot Joints================
double supRotNowZ = 0;//TODO: error
Vector3<double> psfoot = pSwgFoot2Sup;
Vector3<double> psupfoot = state.pSupFootZMP;
RotationMatrix frz = RotationMatrix::getRotationZ(rSwgFoot2Sup.z - supRotNowZ);
Vector3<double> bodyAnglesNow = naoStructTmp.getRotAngles(NaoStructure::bodyLink, NaoStructure::Space_Supprot_Leg);
if(state.supmod == NaoConfig::SUPPORT_MODE_LEFT || state.supmod == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
{
Kinematics::calculateSafeLegJoints(walkingEngineOutput, NaoConfig::LegIDRight, psfoot, frz, naoStructTmp.uLink[NaoStructure::bodyLink].p, naoStructTmp.uLink[NaoStructure::bodyLink].R,
rHipYawPitchAngle, equalHipAngles.x, equalHipAngles.y);
}else{
Kinematics::calculateSafeLegJoints(walkingEngineOutput, NaoConfig::LegIDLeft, psfoot, frz, naoStructTmp.uLink[NaoStructure::bodyLink].p, naoStructTmp.uLink[NaoStructure::bodyLink].R,
rHipYawPitchAngle, equalHipAngles.x, equalHipAngles.y);
}
//===========
// plotJointAngles(walkingEngineOutput, "O1_");
// if(state.supmod == NaoConfig::SUPPORT_MODE_LEFT && abs(zmpErrorY) > 0.01 && abs(zmpErrorY) < 0.025)
// walkingEngineOutput.angles[JointData::LAnkleRoll] += zmpErrorY * deg2rad(200);
// else if(state.supmod == NaoConfig::SUPPORT_MODE_RIGHT && abs(zmpErrorY) > 0.01 && abs(zmpErrorY) < 0.025)
// walkingEngineOutput.angles[JointData::RAnkleRoll] -= zmpErrorY * deg2rad(200);
// plotJointAngles(walkingEngineOutput, "O2_");
//=========LPF smoother=================
if(Use_Smooth_Filter)
smoothJoints(walkingEngineOutput, lastWalkOoutput);
lastWalkOoutput = walkingEngineOutput;
//===Compensate HipRoll When Single Supproting====//
if(walkParam.useHipRollCompensation && (executingCmd.speed.translation.x != 0.0) && (walkStatus == WalkTypeBase::running ))
{
compensateHipRoll(istep, sup_mod, walkParam,walkingEngineOutput);
}
if((walkParam.useHipRollCompensationRight&&(executingCmd.speed.translation.y < 0.0)||walkParam.useHipRollCompensationLeft&&(executingCmd.speed.translation.y > 0.0)) && (walkStatus == WalkTypeBase::running ))
{
compensateHipRollindependent(istep, sup_mod, walkParam,walkingEngineOutput,executingCmd);
}
finishFrame();
//======Print something=========
Vector3<double> pBodyRefW = RotationMatrix(state.rSupFootZMP.x, state.rSupFootZMP.y, state.rSupFootZMP.z) *pBodyNext + state.pSupFootZMP;
// TODO: Check direction of com and acc
//Vector3<double> comW = Rfw * naoStructure.getCoM(NaoStructure::Space_Supprot_Leg)+ lastState.pSupFootZMP;
Vector3<double> comWSup = naoStructure.getCoM(NaoStructure::Space_Supprot_Leg) + lastState.pSupFootZMP;
/*MATLABHELP_PLOT(pCOMRefW.x, "pCOMRefWx");
MATLABHELP_PLOT(pCOMRefW.y, "pCOMRefWy");
MATLABHELP_PLOT(pCOMRefW.z, "pCOMRefWz");*/
//MATLABHELP_PLOT(outputPC.v.x, "CoMRefWvx");
//MATLABHELP_PLOT(outputPC.v.y, "CoMRefWvy");
//MATLABHELP_PLOT(outputPC.v.z, "CoMRefWvz");
//MATLABHELP_PLOT(outputPC.a.x, "CoMRefWax");
//MATLABHELP_PLOT(outputPC.a.y, "CoMRefWay");
//MATLABHELP_PLOT(outputPC.a.z, "CoMRefWaz");
//MATLABHELP_PLOT(pBodyRefW.x, "pBodyWx");
//MATLABHELP_PLOT(pBodyRefW.y, "pBodyWy");
//MATLABHELP_PLOT(pBodyRefW.z, "pBodyWz");
/*MATLABHELP_PLOT(comW.x, "comWx");
MATLABHELP_PLOT(comW.y, "comWy");
MATLABHELP_PLOT(comW.z, "comWz");*/
//MATLABHELP_PLOT(comWSup.x, "comWSupx");
//MATLABHELP_PLOT(comWSup.y, "comWSupy");
//MATLABHELP_PLOT(rSwgFoot2Sup.z, "swg2Foot");
//MATLABHELP_PLOT(pSwgFootW.x, "pSwgFootx");
//MATLABHELP_PLOT(pSwgFootW.y, "pSwgFooty");
//MATLABHELP_PLOT(pSwgFootW.z, "pSwgFootz");
//MATLABHELP_PLOT(rSwgFootW.x, "rSwgFootx");
//MATLABHELP_PLOT(rSwgFootW.y, "rSwgFooty");
//MATLABHELP_PLOT(rSwgFootW.z, "rSwgFootz");
//Vector3<double> lfoot2B = naoStructTmp.getPosition(NaoStructure::lFootLink, NaoStructure::Space_Body);
////Vector3<double> rfoot2B = naoStructTmp.getPosition(NaoStructure::rFootLink, NaoStructure::Space_Body);
//MATLABHELP_PLOT(lfoot2B.x, "lfoot2Bx");
//MATLABHELP_PLOT(lfoot2B.y, "lfoot2By");
//MATLABHELP_PLOT(lfoot2B.z, "lfoot2Bz");
//ZMP measured from LIPM
/*MATLABHELP_PLOT(zmpW.x, "zmpWx");
MATLABHELP_PLOT(zmpW.y, "zmpWy");
MATLABHELP_PLOT(zmpW.z, "zmpWz");*/
//// zmp Ideal
//Vector3<double> zmpWIdeal = outputPC.p - outputPC.a * pCOMRefW.z / 9.81;
//MATLABHELP_PLOT(zmpWIdeal.x, "zmpWIdealx");
//MATLABHELP_PLOT(zmpWIdeal.y, "zmpWIdealy");
//cop
MDEBUG_SAVE(MDebug::idPreviewState,state)
MDEBUG_SAVE(MDebug::idRefCOM, pCOMRefW)
MDEBUG_SAVE(MDebug::idRealCOM, comW)
MDEBUG_SAVE(MDebug::idBodyPos, pBodyRefW)
//MDEBUG_SAVE(MDebug::idBodyAngles, )
Vector3<double> copW;
const CoP& LCoP = pRobotState->mCoPL;
const CoP& RCoP = pRobotState->mCoPR;
if(naoStructure.supportingMode == NaoConfig::SUPPORT_MODE_LEFT || naoStructure.supportingMode == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT){
copW = LCoP.p* LCoP.pressure + (naoStructure.getRotationMatrix(NaoStructure::rFootLink) * RCoP.p
+naoStructTmp.getPosition(NaoStructure::rFootLink, NaoStructure::Space_Supprot_Leg)) * RCoP.pressure;
copW /= (LCoP.pressure + RCoP.pressure);
}else{
copW = RCoP.p* RCoP.pressure + (naoStructure.getRotationMatrix(NaoStructure::lFootLink) * LCoP.p
+naoStructTmp.getPosition(NaoStructure::lFootLink, NaoStructure::Space_Supprot_Leg)) * LCoP.pressure;
copW /= (LCoP.pressure + RCoP.pressure);
}
copW = RotationMatrix(0, 0, lastState.rSupFootZMP.z) *copW + lastState.pSupFootZMP;
/*MATLABHELP_PLOT(copW.x, "copWx");
MATLABHELP_PLOT(copW.y, "copWy");
MATLABHELP_PLOT(copW.z, "copWz");
MATLABHELP_PLOT(pRobotState->groundContactLeft.contact?0.75:0.25, "isContactLeft");
MATLABHELP_PLOT(pRobotState->groundContactRight.contact?0.75:0.25, "isContactRight");*/
lastState = state;
lastCoMW = comW;
}
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 idGroundTruthBallModel:
{
Vector2<> position;
message.bin >> theGroundTruthBallModel.timeWhenLastSeen >> position;
REMOTE_TO_LOCAL_TIME(theGroundTruthBallModel.timeWhenLastSeen);
if(theOwnTeamInfo.teamColor == TEAM_BLUE)
position *= -1;
theGroundTruthBallModel.lastPerception = theGroundTruthRobotPose.invert() * position;
theGroundTruthBallModel.estimate.position = 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];
// This is a potentially evil quick workaround that should be replaced by a better handling of ground contacts of team mates
// at many different places in our code! For a detailed problem description, ask Tim.
if(!theTeamMateData.hasGroundContact[robotNumber])
theTeamMateData.hasGroundContact[robotNumber] = theFrameInfo.getTimeSince(theTeamMateData.timeLastGroundContact[robotNumber]) < 2000;
}
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.timeLastGroundContact[robotNumber];
REMOTE_TO_LOCAL_TIME(theTeamMateData.timeLastGroundContact[robotNumber]);
}
