void MotionRequest::draw() const
{
DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector
if(motion == walk)
{
switch(walkRequest.mode)
{
case WalkRequest::targetMode:
{
LINE("representation:MotionRequest", 0, 0, walkRequest.target.translation.x(), walkRequest.target.translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0));
CROSS("representation:MotionRequest", walkRequest.target.translation.x(), walkRequest.target.translation.y(), 50, 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0));
Vector2f rotation(500.f, 0.f);
rotation.rotate(walkRequest.target.rotation);
ARROW("representation:MotionRequest", walkRequest.target.translation.x(), walkRequest.target.translation.y(), walkRequest.target.translation.x() + rotation.x(), walkRequest.target.translation.y() + rotation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0, 127));
break;
}
case WalkRequest::speedMode:
case WalkRequest::percentageSpeedMode:
{
Vector2f translation = walkRequest.mode == WalkRequest::speedMode ? walkRequest.speed.translation * 10.f : walkRequest.speed.translation * 1000.f;
ARROW("representation:MotionRequest", 0, 0, translation.x(), translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0));
if(walkRequest.target.rotation != 0.0f)
{
translation.x() = translation.norm();
translation.y() = 0;
translation.rotate(walkRequest.speed.rotation);
ARROW("representation:MotionRequest", 0, 0, translation.x(), translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0, 127));
}
break;
}
}
}
}
bool GridStateSpace::stateValid(const Vector2f &pt) const {
return pt.x() >= 0
&& pt.y() >= 0
&& pt.x() < width()
&& pt.y() < height()
&& !obstacleAt(gridSquareForState(pt));
}
void RRTWidget::drawTerminalState(QPainter &painter, const Vector2f &pos, const Vector2f &vel, const QColor &color) {
// draw point
painter.setPen(QPen(color, 6));
QPointF rootLoc(pos.x(), pos.y());
painter.drawEllipse(rootLoc, 2, 2);
Vector2f tipOffset = vel * VelocityDrawingMultiplier;
Vector2f tipLocVec = pos + tipOffset;
QPointF tipLoc(tipLocVec.x(), tipLocVec.y());
// draw arrow shaft
painter.setPen(QPen(color, 3));
painter.drawLine(rootLoc, tipLoc);
// draw arrow head
Vector2f headBase = tipLocVec - tipOffset.normalized()*4;
Vector2f perp = Vector2f(-tipOffset.y(), tipOffset.x()).normalized();
Vector2f tipLeftVec = headBase + perp*4;
Vector2f tipRightVec = headBase - perp*4;
QPointF trianglePts[] = {
tipLoc,
QPointF(tipLeftVec.x(), tipLeftVec.y()),
QPointF(tipRightVec.x(), tipRightVec.y())
};
painter.drawPolygon(trianglePts, 3);
}
Vector4f::Vector4f( const Vector2f& xy, const Vector2f& zw )
{
m_elements[0] = xy.x();
m_elements[1] = xy.y();
m_elements[2] = zw.x();
m_elements[3] = zw.y();
}
bool GoalFramePerceptor::calcGoalFrame(const Pose2f& prePose, const float yTranslation, GoalFrame& goalFrame) const
{
static const float borderToGroundLineDistance = theFieldDimensions.xPosOpponentFieldBorder - theFieldDimensions.xPosOpponentGroundline;
const Pose2f prePoseInverse(prePose.inverse());
int positive = 0, negative = 0;
for(const Vector2f& p : theFieldBoundary.boundaryOnField)
{
Vector2f pInField = prePoseInverse * p;
if(std::abs(std::abs(pInField.x()) - borderToGroundLineDistance) < allowedFieldBoundaryDivergence)
{
if(pInField.x() > 0)
positive++;
else
negative++;
}
}
if(neededConvexBoundaryPoints > positive && neededConvexBoundaryPoints > negative)
return false;
const float sign = (positive < negative) ? -1.f : 1.f;
goalFrame = Pose2f(prePose).rotate(-pi_2 + sign * pi_2);
goalFrame.translation = goalFrame * Vector2f(0.f, (goalFrame.inverse().translation.y() > 0.f ? 1.f : -1.f)).normalized(yTranslation);
return true;
}
/** Render a string of text at the specified starting poisition. The string encoding
* is assumed to be Latin-1 (ISO 8859-1).
*
* Note that this method will not draw anything on systems with OpenGL ES 2.0 (typically mobile
* devices.) For compatibility with all systems, use renderStringToBuffer instead.
*/
Vector2f
TextureFont::render(const string& text, const Vector2f& startPosition) const
{
Vector2f currentPosition = startPosition;
#ifndef VESTA_NO_IMMEDIATE_MODE_3D
glBegin(GL_QUADS);
for (unsigned int i = 0; i < text.length(); ++i)
{
// The cast to unsigned char is critical for glyph lookup to work correctly;
// otherwise, extended characters will generate negative indices.
const Glyph* glyph = lookupGlyph((unsigned char) text[i]);
if (glyph)
{
Vector2f p = currentPosition + glyph->offset;
glTexCoord2fv(glyph->textureCoords[0].data());
glVertex2f(p.x(), p.y());
glTexCoord2fv(glyph->textureCoords[1].data());
glVertex2f(p.x() + glyph->size.x(), p.y());
glTexCoord2fv(glyph->textureCoords[2].data());
glVertex2f(p.x() + glyph->size.x(), p.y() + glyph->size.y());
glTexCoord2fv(glyph->textureCoords[3].data());
glVertex2f(p.x(), p.y() + glyph->size.y());
currentPosition.x() += glyph->advance;
}
}
glEnd();
#endif
return currentPosition;
}
void MandelbrotProcessorCPU::process(Vector2f from, Vector2f to,
Image<unsigned short>& iterations) {
assert (MAX_ITERATIONS < std::numeric_limits<unsigned short>::max());
assert (iterations.cn == 1);
size_t width = iterations.width;
size_t height = iterations.height;
float x_step = (to.x() - from.x()) / width;
float y_step = (to.y() - from.y()) / height;
#pragma omp parallel for
for (size_t py = 0; py < height; py++) {
float y0 = from.y() + y_step * py;
for (size_t px = 0; px < width; px++) {
float x0 = from.x() + x_step * px;
unsigned short iteration;
float x = 0.0f;
float y = 0.0f;
for (iteration = 0; iteration < MAX_ITERATIONS; iteration++) {
float xn = x * x - y * y + x0;
y = 2 * x * y + y0;
x = xn;
if (x * x + y * y > INFINITY) {
break;
}
}
iterations(py, px) = iteration;
}
}
}
void TeamData::draw() const
{
DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField");
for(auto const& teammate : teammates)
{
ColorRGBA posCol;
if(teammate.status == Teammate::PLAYING)
posCol = ColorRGBA::green;
else if(teammate.status == Teammate::FALLEN)
posCol = ColorRGBA::yellow;
else
posCol = ColorRGBA::red;
const Vector2f& rPos = teammate.theRobotPose.translation;
const float radius = std::max(50.f, teammate.theRobotPose.deviation);
Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f);
// Circle around Player
CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen,
posCol, Drawings::noBrush, ColorRGBA::white);
// Direction of the Robot
LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20,
Drawings::solidPen, posCol);
// Player number
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number);
// Ball position
const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position;
CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow);
}
}
void TextEditComponent::onCursorChanged()
{
if(isMultiline())
{
Vector2f textSize = mFont->getWrappedTextCursorOffset(mText, getTextAreaSize().x(), mCursor);
if(mScrollOffset.y() + getTextAreaSize().y() < textSize.y() + mFont->getHeight()) //need to scroll down?
{
mScrollOffset[1] = textSize.y() - getTextAreaSize().y() + mFont->getHeight();
}else if(mScrollOffset.y() > textSize.y()) //need to scroll up?
{
mScrollOffset[1] = textSize.y();
}
}else{
Vector2f cursorPos = mFont->sizeText(mText.substr(0, mCursor));
if(mScrollOffset.x() + getTextAreaSize().x() < cursorPos.x())
{
mScrollOffset[0] = cursorPos.x() - getTextAreaSize().x();
}else if(mScrollOffset.x() > cursorPos.x())
{
mScrollOffset[0] = cursorPos.x();
}
}
}
/**
* @brief Draw all the nodes generated in the RRT algorithm.
* @param painter
*/
void RenderArea::drawNodes(QPainter &painter)
{
painter.save();
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(Qt::black);
painter.setBrush(QBrush(Qt::black));
Vector2f pos;
for(int i = 0; i < (int)rrt->nodes.size(); i++) {
for(int j = 0; j < (int)rrt->nodes[i]->children.size(); j++) {
pos = rrt->nodes[i]->children[j]->position;
painter.drawEllipse(pos.x()-1.5, pos.y()-1.5, 3, 3);
}
pos = rrt->nodes[i]->position;
painter.drawEllipse(pos.x() - NODE_RADIUS, pos.y() - NODE_RADIUS, 2 * NODE_RADIUS, 2 * NODE_RADIUS);
}
painter.setPen(Qt::red);
painter.setBrush(QBrush(Qt::red));
// if a path exists, draw it.
for(int i = 0; i < (int)rrt->path.size() - 1; i++) {
QPointF p1(rrt->path[i]->position.x(), rrt->path[i]->position.y());
QPointF p2(rrt->path[i+1]->position.x(), rrt->path[i+1]->position.y());
painter.drawLine(p1, p2);
}
painter.restore();
}
// static
Vector3f Vector2f::cross( const Vector2f& v0, const Vector2f& v1 ) {
return Vector3f
(
0,
0,
v0.x() * v1.y() - v0.y() * v1.x()
);
}
void test_autodiff_scalar()
{
Vector2f p = Vector2f::Random();
typedef AutoDiffScalar<Vector2f> AD;
AD ax(p.x(),Vector2f::UnitX());
AD ay(p.y(),Vector2f::UnitY());
AD res = foo<AD>(ax,ay);
VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y()));
}
void KickEngineData::calcLegJoints(const Joints::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions, const DamageConfigurationBody& theDamageConfigurationBody)
{
const int sign = joint == Joints::lHipYawPitch ? 1 : -1;
const Vector3f& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra];
const Vector3f& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot];
const RotationMatrix rotateBodyTilt = RotationMatrix::aroundX(bodyAngle.x()).rotateY(bodyAngle.y());
Vector3f anklePos = rotateBodyTilt * (footPos - Vector3f(0.f, 0, -theRobotDimensions.footHeight));
anklePos -= Vector3f(0.f, sign * (theRobotDimensions.yHipOffset), 0);
//const RotationMatrix zRot = RotationMatrix::aroundZ(footRotAng.z()).rotateX(sign * pi_4);
//anklePos = zRot * anklePos;
const float leg0 = 0;//std::atan2(-anklePos.x(), anklePos.y());
const float diagonal = anklePos.norm();
// upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
float a1 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength -
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength + diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * diagonal);
//if(std::abs(a1) > 1.f) OUTPUT_TEXT("clipped a1");
a1 = std::abs(a1) > 1.f ? 0.f : std::acos(a1);
float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength +
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength);
//if(std::abs(a2) > 1.f) OUTPUT_TEXT("clipped a2");
a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);
const float leg2 = -a1 - std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm() * -sgn(anklePos.z()));
const float leg1 = anklePos.z() == 0.0f ? 0.0f : (std::atan(anklePos.y() / -anklePos.z()));
const float leg3 = pi - a2;
const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(0).rotateX(bodyAngle.x()).rotateY(bodyAngle.y());
//calculate inverse foot rotation so that they are flat to the ground
RotationMatrix footRot = RotationMatrix::aroundX(leg1).rotateY(leg2 + leg3);
footRot = footRot.inverse() /* * zRot*/ * rotateBecauseOfHip;
//and add additonal foot rotation (which is probably not flat to the ground)
const float leg4 = std::atan2(footRot(0, 2), footRot(2, 2)) + footRotAng.y();
const float leg5 = std::asin(-footRot(1, 2)) + footRotAng.x() ;
jointRequest.angles[joint] = leg0;
jointRequest.angles[joint + 1] = (/*-pi_4 * sign + */leg1);
jointRequest.angles[joint + 2] = leg2;
jointRequest.angles[joint + 3] = leg3;
jointRequest.angles[joint + 4] = leg4;
jointRequest.angles[joint + 5] = leg5;
//quickhack which allows calibration, but works only if the left foot is the support foot in .kmc file
Vector2f tiltCalibration = currentKickRequest.mirror ? theDamageConfigurationBody.startTiltRight : theDamageConfigurationBody.startTiltLeft;
if(currentKickRequest.mirror)
tiltCalibration.x() *= -1.f;
jointRequest.angles[Joints::lAnkleRoll] += tiltCalibration.x();
jointRequest.angles[Joints::lAnklePitch] += tiltCalibration.y();
}
bool BoxFilterFilm::AddSample(const rose::Spectrum &s, const Vector2ui &pixel, const Vector2f &sample) {
// Check we are inside pixel boundaries
if (sample.x() < 0.f || sample.x() > 1.f || sample.y() < 0.f || sample.y() > 1.f) { return false; }
// Add sample
raster[pixel.y() * width + pixel.x()] += s;
// Increase number of samples of the pixel
num_samples[pixel.y() * width + pixel.x()]++;
return true;
}
SwingFoot::SwingFoot(bool isLeft,
const Vector2f& p,
float footHeight,
AngDeg ang,
AngDeg rotateFoot,
float bodyHeight,
float duration,
Task* primary)
:LowerLimbsMotion(duration, primary),
mIsLeft(isLeft)
{
if ( mIsLeft ){
// left foot
mDesiredFootL.rotationZ(ang);
mDesiredFootL.p() = Vector3f(p.x(),p.y(),footHeight);
// right foot
mDesiredFootR.identity();
}
else{
// right foot
mDesiredFootR.rotationZ(ang);
mDesiredFootR.p() = Vector3f(p.x(),p.y(),footHeight);
// left foot
mDesiredFootL.identity();
}
mDesiredFootL.p().x() -= HUMANOID.getHalfFeetWidth();
mDesiredFootR.p().x() += HUMANOID.getHalfFeetWidth();
mDesiredFootL.p().z() += HUMANOID.getMinFootHeight();
mDesiredFootR.p().z() += HUMANOID.getMinFootHeight();
// body
AngDeg dirR = mDesiredFootR.rotatedAngZ();
AngDeg dirL = mDesiredFootL.rotatedAngZ();
if ( dirR > dirL ){
std::swap(dirR, dirL);
}
AngDeg dirBody = calBisectorTwoAngles(dirR,dirL);
mDesiredBody.rotationX(-10);
mDesiredBody.rotateLocalZ( dirBody );
mDesiredBody.pos() = (mDesiredFootL.p()+mDesiredFootR.p())*0.5f;
// mDesiredBody.pos().x() = 0;
mDesiredBody.pos().z() = bodyHeight;
//dynamic step
//mDesiredBody.p().y()-=addStepY;
if ( p.y()>0 ){
if(mIsLeft){
mDesiredFootL.rotateLocalX(rotateFoot);
}
else{
mDesiredFootR.rotateLocalX(rotateFoot);
}
}
}
void TeamPlayersLocator::update(TeamPlayersModel& teamPlayersModel)
{
teamPlayersModel.obstacles.clear();
std::vector<Obstacle> ownTeam;
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);
if(theRobotInfo.penalty == PENALTY_NONE && theGroundContactState.contact)
{
ownTeam.emplace_back(theRobotPose.covariance.topLeftCorner(2, 2), theRobotPose.translation); // pose and covariance of the robot itself
if(theFallDownState.state == theFallDownState.upright) //why is this?
for(const auto& obstacle : theObstacleModel.obstacles)
{
if(obstacle.type == GOALPOST)
continue;
// if seen robots are of the opponent team and are not detected outside the field
const Vector2f p = theRobotPose * obstacle.center;
if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
teamPlayersModel.obstacles.emplace_back(obstacle.covariance, p, obstacle.type);
}
}
for(auto const& teammate : theTeammateData.teammates)
{
if(teammate.status == Teammate::FULLY_ACTIVE)
{
if(teammate.pose.deviation < 50.f) //todo, magic number will be replaced by parameter. 50.f might be too low
ownTeam.emplace_back(teammate.pose.covariance.topLeftCorner(2, 2), teammate.pose.translation); //position of teammates
for(const auto& obstacle : teammate.obstacleModel.obstacles)
{
if(obstacle.type == GOALPOST)
continue;
// if seen robots are of the opponent team and are not detected outside the field
const Vector2f p = teammate.pose * obstacle.center;
if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
{
Matrix2f covariance = (Matrix2f() << obstacle.covXX, obstacle.covXY, obstacle.covXY, obstacle.covYY).finished();
Obstacle converted = Obstacle(rotateCovariance(covariance, teammate.pose.rotation), p, obstacle.type);
merge(converted, teamPlayersModel.obstacles);
}
}
}
}
for(auto& teammate : ownTeam)
{
removeAround(teammate, teamPlayersModel.obstacles);
}
}
bool BoxFilterFilmBlock::AddSample(const Spectrum &s, const Vector2ui &global_pixel, const Vector2f &sample) {
// Check we are inside pixel boundaries
if (sample.x() < 0.f || sample.x() > 1.f || sample.y() < 0.f || sample.y() > 1.f) { return false; }
// Compute local pixel coordinates
Vector2ui local_pixel = global_pixel - Vector2ui(width_offset, height_offset);
// Add sample
raster[local_pixel.y() * width + local_pixel.x()] += s;
// Increase number of samples of the pixel
num_samples[local_pixel.y() * width + local_pixel.x()]++;
return true;
}
int PxController::bound(Vector2f &n) {
n.x() = -n.x();
if(n.dot(v_) >= 0) {
return 1;
}
v_ += -2*(n.dot(v_))*n;
bound_locked_ = true;
std::cout << "----bound----- "<< std::endl;
std::cout << " n:" << n.x() << "," << n.y() << std::endl;
std::cout<< " v:" << v_.x() << "," << v_.y() << std::endl;
return 0;
}
void GlobalFieldCoverageProvider::setCoverageAtFieldPosition(GlobalFieldCoverage& globalFieldCoverage, const Vector2f& positionOnField, const int coverage) const
{
if(theFieldDimensions.isInsideField(positionOnField))
{
ASSERT(std::isfinite(positionOnField.x()));
ASSERT(std::isfinite(positionOnField.y()));
const int x = static_cast<int>((positionOnField.x() - theFieldDimensions.xPosOwnGroundline) / cellLengthX);
const int y = static_cast<int>((positionOnField.y() - theFieldDimensions.yPosRightSideline) / cellLengthY);
globalFieldCoverage.grid[y * numOfCellsX + x].timestamp = theFrameInfo.time;
globalFieldCoverage.grid[y * numOfCellsX + x].coverage = coverage;
}
}
LIPStateEstimator::SupportFoot LIPStateEstimator::guessSupportFoot(const Vector2f& leftOrigin) const
{
const Pose3f leftOriginToTorso = theRobotModel.soleLeft + Vector3f(leftOrigin.x(), leftOrigin.y(), 0.f);
const Pose3f rightOriginToTorso = theRobotModel.soleRight + Vector3f(leftOrigin.x(), -leftOrigin.y(), 0.f);
const Pose3f torsoToWorld(theInertialData.orientation2D);
const Pose3f leftOriginToWorld = torsoToWorld * leftOriginToTorso;
const Pose3f rightOriginToWorld = torsoToWorld * rightOriginToTorso;
if(leftOriginToWorld.translation.z() < rightOriginToWorld.translation.z())
return SupportFoot::left;
else
return SupportFoot::right;
}
void TeamData::draw() const
{
DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField");
for(auto const& teammate : teammates)
{
ColorRGBA posCol;
if(teammate.status == Teammate::PLAYING)
posCol = ColorRGBA::green;
else if(teammate.status == Teammate::FALLEN)
posCol = ColorRGBA::yellow;
else
posCol = ColorRGBA::red;
const Vector2f& rPos = teammate.theRobotPose.translation;
const float radius = std::max(50.f, teammate.theRobotPose.deviation);
Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f);
// Circle around Player
CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen,
posCol, Drawings::noBrush, ColorRGBA::white);
// Direction of the Robot
LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20,
Drawings::solidPen, posCol);
// Player number
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number);
// Role
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 150, 100,
ColorRGBA::black, Role::getName(teammate.theBehaviorStatus.role));
// Time to reach ball
int ttrb = teammate.theBehaviorStatus.role == Role::striker
? static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBallStriker)
: static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBall);
if(Blackboard::getInstance().exists("FrameInfo"))
{
const FrameInfo& theFrameInfo = (const FrameInfo&) Blackboard::getInstance()["FrameInfo"];
ttrb = theFrameInfo.getTimeSince(ttrb);
}
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 300, 100, ColorRGBA::black, "TTRB: " << ttrb);
//Line from Robot to WalkTarget
LINE("representation:TeamData", rPos.x(), rPos.y(),
teammate.theSPLStandardBehaviorStatus.walkingTo.x(),
teammate.theSPLStandardBehaviorStatus.walkingTo.y(),
10, Drawings::dashedPen, ColorRGBA::magenta);
// Ball position
const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position;
CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow);
}
}
void UKFPose2D::lineSensorUpdate(bool vertical, const Vector2f& reading, const Matrix2f& readingCov)
{
generateSigmaPoints();
// computeLineReadings
Vector2f lineReadings[7];
if(vertical)
for(int i = 0; i < 7; ++i)
lineReadings[i] = Vector2f(sigmaPoints[i].y(), sigmaPoints[i].z());
else
for(int i = 0; i < 7; ++i)
lineReadings[i] = Vector2f(sigmaPoints[i].x(), sigmaPoints[i].z());
// computeMeanOfLineReadings
Vector2f lineReadingMean = lineReadings[0];
for(int i = 1; i < 7; ++i)
lineReadingMean += lineReadings[i];
lineReadingMean *= 1.f / 7.f;
// computeCovOfLineReadingsAndSigmaPoints
Matrix2x3f lineReadingAndMeanCov = Matrix2x3f::Zero();
for(int i = 0; i < 3; ++i)
{
const Vector2f d1 = lineReadings[i * 2 + 1] - lineReadingMean;
lineReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
const Vector2f d2 = lineReadings[i * 2 + 2] - lineReadingMean;
lineReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
}
lineReadingAndMeanCov *= 0.5f;
// computeCovOfLineReadingsReadings
const Vector2f d = lineReadings[0] - lineReadingMean;
Matrix2f lineReadingCov = (Matrix2f() << d * d.x(), d * d.y()).finished();
for(int i = 1; i < 7; ++i)
{
const Vector2f d = lineReadings[i] - lineReadingMean;
lineReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished();
}
lineReadingCov *= 0.5f;
lineReadingMean.y() = Angle::normalize(lineReadingMean.y());
const Matrix3x2f kalmanGain = lineReadingAndMeanCov.transpose() * (lineReadingCov + readingCov).inverse();
Vector2f innovation = reading - lineReadingMean;
innovation.y() = Angle::normalize(innovation.y());
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z() = Angle::normalize(mean.z());
cov -= kalmanGain * lineReadingAndMeanCov;
Covariance::fixCovariance(cov);
}
inline Vector2f VectorLocalization2D::observationFunction(line2f l, Vector2f p) const
{
static const bool debug = false;
Vector2f attraction(0.0,0.0), dir(V2COMP(l.Dir())), p0(V2COMP(l.P0())), p1(V2COMP(l.P1()));
float location = (p-p0).dot(dir);
attraction = (p-p0) - dir*location ;
if(debug){
debugLines.push_back( line2f( vector2f(p.x(),p.y()) ,vector2f((p-attraction).x(),(p-attraction).y()) ) );
const float crossSize = 0.002;
debugLines.push_back( line2f( vector2f(p.x()+crossSize,p.y()) , vector2f(p.x()-crossSize,p.y())) );
debugLines.push_back( line2f( vector2f(p.x(),p.y()+crossSize) , vector2f(p.x(),p.y()-crossSize)) );
}
return attraction;
}
void UKFPose2D::landmarkSensorUpdate(const Vector2f& landmarkPosition, const Vector2f& reading, const Matrix2f& readingCov)
{
generateSigmaPoints();
// computeLandmarkReadings
Vector2f landmarkReadings[7];
for(int i = 0; i < 7; ++i)
{
Pose2f pose(sigmaPoints[i].z(), sigmaPoints[i].head<2>());
Vector2f landmarkPosRel = pose.invert() * landmarkPosition; // TODO: optimize this
landmarkReadings[i] = Vector2f(landmarkPosRel.x(), landmarkPosRel.y());
}
// computeMeanOfLandmarkReadings
Vector2f landmarkReadingMean = landmarkReadings[0];
for(int i = 1; i < 7; ++i)
landmarkReadingMean += landmarkReadings[i];
landmarkReadingMean *= 1.f / 7.f;
// computeCovOfLandmarkReadingsAndSigmaPoints
Matrix2x3f landmarkReadingAndMeanCov = Matrix2x3f::Zero();
for(int i = 0; i < 3; ++i)
{
const Vector2f d1 = landmarkReadings[i * 2 + 1] - landmarkReadingMean;
landmarkReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
const Vector2f d2 = landmarkReadings[i * 2 + 2] - landmarkReadingMean;
landmarkReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
}
landmarkReadingAndMeanCov *= 0.5f;
// computeCovOfLandmarkReadingsReadings
const Vector2f d = landmarkReadings[0] - landmarkReadingMean;
Matrix2f landmarkReadingCov = Matrix2f::Zero();
landmarkReadingCov << d * d.x(), d * d.y();
for(int i = 1; i < 7; ++i)
{
const Vector2f d = landmarkReadings[i] - landmarkReadingMean;
landmarkReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished();
}
landmarkReadingCov *= 0.5f;
const Matrix3x2f kalmanGain = landmarkReadingAndMeanCov.transpose() * (landmarkReadingCov + readingCov).inverse();
Vector2f innovation = reading - landmarkReadingMean;
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z() = Angle::normalize(mean.z());
cov -= kalmanGain * landmarkReadingAndMeanCov;
Covariance::fixCovariance(cov);
}
void Button::autosize()
{
myLabel.autosize();
myImage.autosize();
// HACK: we add some default margin to the label.
Vector2f size = Vector2f::Zero();
if(myTextEnabled)
{
size = myLabel.getSize();
}
size[0] += myImage.getSize()[0];
size[1] = max(size[1], myImage.getSize()[1]);
myLabel.setHeight(size[1]);
// Commented: avoid stretching the image.
//myImage.setHeight(size[1]);
if(myCheckable)
{
size += Vector2f(size[1] + 10, 4);
}
else
{
size += Vector2f(10, 4);
}
if(isImageEnabled())
{
size.x() += myImage.getSize().x();
}
setSize(size);
}
Vector4f::Vector4f( float x, float y, const Vector2f& zw )
{
m_elements[0] = x;
m_elements[1] = y;
m_elements[2] = zw.x();
m_elements[3] = zw.y();
}
Vector4f::Vector4f( float x, const Vector2f& yz, float w )
{
m_elements[0] = x;
m_elements[1] = yz.x();
m_elements[2] = yz.y();
m_elements[3] = w;
}
Vector4f::Vector4f( const Vector2f& xy, float z, float w )
{
m_elements[0] = xy.x();
m_elements[1] = xy.y();
m_elements[2] = z;
m_elements[3] = w;
}
void ManualHeadMotionProvider::update(HeadMotionRequest& headMotionRequest)
{
bool parametersChanged = xImg != currentX || yImg != currentY;
if(parametersChanged && camera == theCameraInfo.camera)
{
currentX = xImg;
currentY = yImg;
Vector2f targetOnField;
if(Transformation::imageToRobot(currentX, currentY, theCameraMatrix, theCameraInfo, targetOnField))
{
headMotionRequest.target.x() = targetOnField.x();
headMotionRequest.target.y() = targetOnField.y();
headMotionRequest.target.z() = 0;
headMotionRequest.mode = HeadMotionRequest::targetOnGroundMode;
headMotionRequest.watchField = false;
//Use the camera that the user is seeing right now.
switch(camera)
{
case CameraInfo::lower:
headMotionRequest.cameraControlMode = HeadMotionRequest::lowerCamera;
break;
case CameraInfo::upper:
headMotionRequest.cameraControlMode = HeadMotionRequest::upperCamera;
break;
default:
ASSERT(false);
}
headMotionRequest.speed = 150_deg;
}
}
}
void SideConfidenceProvider::findAgreemates()
{
// If I have not seen the ball recently, no matches can be found anyway ...
if(!ballModelCanBeUsed(theBallModel, theWorldModelPrediction.robotPose))
return;
const Vector2f myGlobalBallPosition = theWorldModelPrediction.robotPose * theBallModel.estimate.position;
const Vector2f myGlobalBallPositionMirrored(-myGlobalBallPosition.x(), -myGlobalBallPosition.y());
// Check all teammates:
for(const auto& teammate : theTeamData.teammates)
{
// Robots that are not playing are of no interest
if(teammate.status != Teammate::PLAYING)
continue;
// Only trust B-Human robots
if(teammate.mateType != Teammate::TeamOrigin::BHumanRobot)
continue;
if(ballModelCanBeUsed(teammate.theBallModel, teammate.theRobotPose))
{
const Vector2f teammateGlobalBallPosition = teammate.theRobotPose * teammate.theBallModel.estimate.position;
const TeammateBallCompatibility bc = compareBallWithTeammateBall(myGlobalBallPosition, myGlobalBallPositionMirrored,
teammateGlobalBallPosition);
addToAgreemateList(teammate, bc);
}
}
}
