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::ps_solid, ColorRGBA(0xcd, 0, 0));
CROSS("representation:MotionRequest", walkRequest.target.translation.x, walkRequest.target.translation.y, 50, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
Vector2<> 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::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
}
break;
case WalkRequest::speedMode:
case WalkRequest::percentageSpeedMode:
{
Vector2<> 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::ps_solid, ColorRGBA(0xcd, 0, 0));
if(walkRequest.target.rotation != 0.0f)
{
translation.x = translation.abs();
translation.y = 0;
translation.rotate(walkRequest.speed.rotation);
ARROW("representation:MotionRequest", 0, 0, translation.x, translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
}
}
break;
default:
break;
}
}
}
void CoordinateSystemProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
Geometry::Line horizon = Geometry::calculateHorizon(theCameraMatrix, theCameraInfo);
imageCoordinateSystem.origin = horizon.base;
imageCoordinateSystem.rotation.c[0] = horizon.direction;
imageCoordinateSystem.rotation.c[1] = Vector2<double>(-horizon.direction.y, horizon.direction.x);
imageCoordinateSystem.invRotation = imageCoordinateSystem.rotation.transpose();
RotationMatrix r(theCameraMatrix.rotation.transpose() * prevCameraMatrix.rotation);
imageCoordinateSystem.offset = Vector2<double>(r.getZAngle(), r.getYAngle());
calcScaleFactors(imageCoordinateSystem.a, imageCoordinateSystem.b);
imageCoordinateSystem.offsetInt = Vector2<int>(int(imageCoordinateSystem.offset.x * 1024 + 0.5),
int(imageCoordinateSystem.offset.y * 1024 + 0.5));
imageCoordinateSystem.aInt = int(imageCoordinateSystem.a * 1024 + 0.5);
imageCoordinateSystem.bInt = int(imageCoordinateSystem.b * 1024 + 0.5);
imageCoordinateSystem.cameraInfo = theCameraInfo;
prevCameraMatrix = theCameraMatrix;
prevTime = theFilteredJointData.timeStamp;
DECLARE_DEBUG_DRAWING("horizon", "drawingOnImage"); // displays the horizon
ARROW("horizon",
imageCoordinateSystem.origin.x,
imageCoordinateSystem.origin.y,
imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[0].x * 50,
imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[0].y * 50,
1, Drawings::ps_solid, ColorRGBA(255,0,0));
ARROW("horizon",
imageCoordinateSystem.origin.x,
imageCoordinateSystem.origin.y,
imageCoordinateSystem.origin.x + imageCoordinateSystem.rotation.c[1].x * 50,
imageCoordinateSystem.origin.y + imageCoordinateSystem.rotation.c[1].y * 50,
1, Drawings::ps_solid, ColorRGBA(255,0,0));
GENERATE_DEBUG_IMAGE(corrected,
INIT_DEBUG_IMAGE_BLACK(corrected);
int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y;
for(int ySrc = 0; ySrc < theImage.cameraInfo.resolutionHeight; ++ySrc)
for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y; yDest <= yDest2; ++yDest)
{
int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x;
for(int xSrc = 0; xSrc < theImage.cameraInfo.resolutionWidth; ++xSrc)
{
for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x; xDest <= xDest2; ++xDest)
{
DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x + 0.5),
yDest + int(theCameraInfo.opticalCenter.y + 0.5),
theImage.image[ySrc][xSrc].y,
theImage.image[ySrc][xSrc].cb,
theImage.image[ySrc][xSrc].cr);
}
}
}
SEND_DEBUG_IMAGE(corrected);
);
void ImageCoordinateSystem::draw() const
{
DECLARE_DEBUG_DRAWING("horizon", "drawingOnImage"); // displays the horizon
ARROW("horizon",
origin.x,
origin.y,
origin.x + rotation.c[0].x * 50,
origin.y + rotation.c[0].y * 50,
0, Drawings::ps_solid, ColorRGBA(255, 0, 0));
ARROW("horizon",
origin.x,
origin.y,
origin.x + rotation.c[1].x * 50,
origin.y + rotation.c[1].y * 50,
0, Drawings::ps_solid, ColorRGBA(255, 0, 0));
}
void
tkcvslinesize(TkCitem *i)
{
TkCline *l;
int j, w, as, shape[3], arrow;
l = TKobj(TkCline, i);
w = TKF2I(l->width);
i->p.bb = bbnil;
tkpolybound(i->p.drawpt, i->p.npoint, &i->p.bb);
l->arrowf = l->capstyle;
l->arrowl = l->capstyle;
if(l->arrow != 0) {
as = w/3;
if(as < 1)
as = 1;
for(j = 0; j < 3; j++) {
shape[j] = l->shape[j];
if(shape[j] == 0)
shape[j] = as * cvslshape[j];
}
arrow = ARROW(TKF2I(shape[0]), TKF2I(shape[1]), TKF2I(shape[2]));
if(l->arrow & TkCarrowf)
l->arrowf = arrow;
if(l->arrow & TkCarrowl)
l->arrowl = arrow;
w += shape[2];
}
i->p.bb = insetrect(i->p.bb, -w);
}
void CognitionLogDataProvider::update(ImageCoordinateSystem& imageCoordinateSystem)
{
imageCoordinateSystem.setCameraInfo(theCameraInfo);
DECLARE_DEBUG_DRAWING("loggedHorizon", "drawingOnImage"); // displays the horizon
ARROW("loggedHorizon",
imageCoordinateSystem.origin.x(),
imageCoordinateSystem.origin.y(),
imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 0) * 50,
imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 0) * 50,
0, Drawings::solidPen, ColorRGBA(255, 0, 0));
ARROW("loggedHorizon",
imageCoordinateSystem.origin.x(),
imageCoordinateSystem.origin.y(),
imageCoordinateSystem.origin.x() + imageCoordinateSystem.rotation(0, 1) * 50,
imageCoordinateSystem.origin.y() + imageCoordinateSystem.rotation(1, 1) * 50,
0, Drawings::solidPen, ColorRGBA(255, 0, 0));
COMPLEX_IMAGE(corrected)
{
if(Blackboard::getInstance().exists("Image"))
{
const Image& image = (const Image&) Blackboard::getInstance()["Image"];
INIT_DEBUG_IMAGE_BLACK(corrected, theCameraInfo.width, theCameraInfo.height);
int yDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, 0).y();
for(int ySrc = 0; ySrc < theCameraInfo.height; ++ySrc)
for(int yDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).y(); yDest <= yDest2; ++yDest)
{
int xDest = -imageCoordinateSystem.toCorrectedCenteredNeg(0, ySrc).x();
for(int xSrc = 0; xSrc < theCameraInfo.width; ++xSrc)
{
for(int xDest2 = -imageCoordinateSystem.toCorrectedCenteredNeg(xSrc, ySrc).x(); xDest <= xDest2; ++xDest)
{
DEBUG_IMAGE_SET_PIXEL_YUV(corrected, xDest + int(theCameraInfo.opticalCenter.x() + 0.5f),
yDest + int(theCameraInfo.opticalCenter.y() + 0.5f),
image[ySrc][xSrc].y,
image[ySrc][xSrc].cb,
image[ySrc][xSrc].cr);
}
}
}
SEND_DEBUG_IMAGE(corrected);
}
}
}
void ArmContactModelProvider::checkArm(bool left, float factor)
{
Vector2f retVal;
/* Calculate arm diffs */
struct ArmAngles angles = angleBuffer[p.frameDelay];
retVal.x = left
? theFilteredJointData.angles[JointData::LShoulderPitch] - angles.leftX
: theFilteredJointData.angles[JointData::RShoulderPitch] - angles.rightX;
retVal.y = left
? theFilteredJointData.angles[JointData::LShoulderRoll] - angles.leftY
: theFilteredJointData.angles[JointData::RShoulderRoll] - angles.rightY;
retVal *= factor;
if(left)
{
ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(retVal.y) * SCALE), toDegrees(retVal.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::blue);
leftErrorBuffer.add(retVal);
}
else
{
ARROW("module:ArmContactModelProvider:armContact", 0, 0, (toDegrees(retVal.y) * SCALE), toDegrees(retVal.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::blue);
rightErrorBuffer.add(retVal);
}
}
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 ExpObstacleModel::draw() const
{
DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:center", "drawingOnField");
DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:feetCircle", "drawingOnField");
DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:shoulderCircle", "drawingOnField");
DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:covariance", "drawingOnField");
DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:velocity", "drawingOnField");
DECLARE_DEBUG_DRAWING("representation:ExpObstacleModel:fallen", "drawingOnField");
ColorRGBA color;
for(const auto& obstacle : eobs)
{
switch(obstacle.type)
{
case ExpObstacle::GOALPOST:
{
color = ColorRGBA::yellow;
CROSS("representation:ExpObstacleModel:center", obstacle.center.x, obstacle.center.y, 50, 10, Drawings::ps_solid, ColorRGBA::yellow);
COVARIANCE2D("representation:ExpObstacleModel:covariance",
Matrix2x2<>(obstacle.covariance[0][0], obstacle.covariance[1][0], obstacle.covariance[0][1], obstacle.covariance[1][1]), obstacle.center);
continue;
}
case ExpObstacle::FALLENBLUE:
case ExpObstacle::ROBOTBLUE:
{
color = ColorRGBA::blue;
break;
}
case ExpObstacle::FALLENRED:
case ExpObstacle::ROBOTRED:
{
color = ColorRGBA::red;
break;
}
case ExpObstacle::SOMEFALLENROBOT:
case ExpObstacle::SOMEROBOT:
{
color = ColorRGBA::white;
break;
}
default:
{
color = ColorRGBA::black;
break;
}
}
CROSS("representation:ExpObstacleModel:center", obstacle.center.x, obstacle.center.y, 20, 10, Drawings::ps_solid, color);
CIRCLE("representation:ExpObstacleModel:feetCircle", obstacle.center.x, obstacle.center.y, feetRadius, 10,
Drawings::ps_solid, color, Drawings::bs_solid, color);
CIRCLE("representation:ExpObstacleModel:shoulderCircle", obstacle.center.x, obstacle.center.y, shoulderRadius, 10,
Drawings::ps_solid, color, Drawings::bs_solid, color);
COVARIANCE2D("representation:ExpObstacleModel:covariance",
Matrix2x2<>(obstacle.covariance[0][0], obstacle.covariance[1][0], obstacle.covariance[0][1], obstacle.covariance[1][1]), obstacle.center);
if(obstacle.velocity.abs() > 0)
ARROW("representation:ExpObstacleModel:velocity", obstacle.center.x, obstacle.center.y, obstacle.center.x + (obstacle.velocity.x * 1000.f), obstacle.center.y + (obstacle.velocity.y * 1000.f), 10, Drawings::ps_solid, color);
if(obstacle.type == ExpObstacle::SOMEFALLENROBOT || obstacle.type == ExpObstacle::FALLENBLUE || obstacle.type == ExpObstacle::FALLENRED)
{
DRAWTEXT("representation:ExpObstacleModel:fallen", obstacle.center.x, obstacle.center.y, 100, color, "FALLEN");
}
}
}
