void BH2011GoalSymbols::drawingOnField()
{
float dist = Geometry::distance(robotPose.translation, Vector2<>((float) fieldDimensions.xPosOpponentGroundline, (float)(fieldDimensions.yPosLeftGoal + fieldDimensions.yPosRightGoal) / 2.0f));
Vector2<> targetImage;
// draw angle to center of free part
targetImage = Geometry::relative2FieldCoord(robotPose, dist * cos(fromDegrees(getCenterAngleOfFreePart())), dist * sin(fromDegrees(getCenterAngleOfFreePart())));
LINE(
"behavior:GoalSymbols:FreePartAnglesOnField",
robotPose.translation.x, robotPose.translation.y, targetImage.x, targetImage.y,
15, Drawings::ps_solid, ColorClasses::black
);
// draw angle to inner side of free part
targetImage = Geometry::relative2FieldCoord(robotPose, dist * cos(fromDegrees(getInnerAngleOfFreePart())), dist * sin(fromDegrees(getInnerAngleOfFreePart())));
LINE(
"behavior:GoalSymbols:FreePartAnglesOnField",
robotPose.translation.x, robotPose.translation.y, targetImage.x, targetImage.y,
15, Drawings::ps_solid, ColorClasses::red
);
// draw angle to outer side of free part
targetImage = Geometry::relative2FieldCoord(robotPose, dist * cos(fromDegrees(getOuterAngleOfFreePart())), dist * sin(fromDegrees(getOuterAngleOfFreePart())));
LINE(
"behavior:GoalSymbols:FreePartAnglesOnField",
robotPose.translation.x, robotPose.translation.y, targetImage.x, targetImage.y,
15, Drawings::ps_solid, ColorClasses::blue
);
}
//MAKE_MODULE(InertiaSensorFilter, Sensing)
//
InertiaSensorFilter::InertiaSensorFilter() : lastTime(0)
{
p.processNoise = Vector2<>(0.004f, 0.004f);
p.accNoise = Vector3<>(1.f, 1.f, 1.f);
p.calculatedAccLimit = Vector2<>(fromDegrees(20.f), fromDegrees(30.f));
p.calculateConstants();
}
void SpecialActions::MotionNetData::load(In& stream)
{
for(int i = 0; i < SpecialActionRequest::numOfSpecialActionIDs; ++i)
stream >> label_extern_start[i];
label_extern_start[SpecialActionRequest::numOfSpecialActionIDs] = 0;
int numberOfNodes;
stream >> numberOfNodes;
if(nodeArray)
delete[] nodeArray;
nodeArray = new MotionNetNode[numberOfNodes];
for(int i = 0; i < numberOfNodes; ++i)
{
short s;
stream >> s;
switch(s)
{
case 2:
nodeArray[i].d[0] = (short) MotionNetNode::typeTransition;
stream >> nodeArray[i].d[1] >> nodeArray[i].d[JointData::numOfJoints + 3];
break;
case 1:
nodeArray[i].d[0] = (short) MotionNetNode::typeConditionalTransition;
stream >> nodeArray[i].d[1] >> nodeArray[i].d[2] >> nodeArray[i].d[JointData::numOfJoints + 3];
break;
case 4:
nodeArray[i].d[0] = (short) MotionNetNode::typeHardness;
for(int j = 1; j < JointData::numOfJoints + 3; j++)
stream >> nodeArray[i].d[j];
break;
case 3:
nodeArray[i].d[0] = (short) MotionNetNode::typeData;
for(int j = 1; j < JointData::numOfJoints + 1; ++j)
{
stream >> nodeArray[i].d[j];
if(nodeArray[i].d[j] != JointData::off &&
nodeArray[i].d[j] != JointData::ignore)
nodeArray[i].d[j] = fromDegrees(nodeArray[i].d[j]);
}
for(int k = JointData::numOfJoints + 1; k < JointData::numOfJoints + 4; ++k)
stream >> nodeArray[i].d[k];
break;
}
}
}
void CameraIntrinsics::serialize(In* in, Out* out)
{
float upperOpeningAngleWidth = toDegrees(this->upperOpeningAngleWidth);
float upperOpeningAngleHeight = toDegrees(this->upperOpeningAngleHeight);
float lowerOpeningAngleWidth = toDegrees(this->lowerOpeningAngleWidth);
float lowerOpeningAngleHeight = toDegrees(this->lowerOpeningAngleHeight);
STREAM_REGISTER_BEGIN;
STREAM(upperOpeningAngleWidth);
STREAM(upperOpeningAngleHeight);
STREAM(upperOpticalCenter);
STREAM(lowerOpeningAngleWidth);
STREAM(lowerOpeningAngleHeight);
STREAM(lowerOpticalCenter);
STREAM_REGISTER_FINISH;
if(in)
{
this->upperOpeningAngleWidth = fromDegrees(upperOpeningAngleWidth);
this->upperOpeningAngleHeight = fromDegrees(upperOpeningAngleHeight);
this->lowerOpeningAngleWidth = fromDegrees(lowerOpeningAngleWidth);
this->lowerOpeningAngleHeight = fromDegrees(lowerOpeningAngleHeight);
}
}
bool SearchBall::Stopped2RotateBody0_transition_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Stopped2RotateBody0_transition_code
ObjectState *ball = _BallDetector->getObj();
if ((getStopWatch() > 8000) ||
((fabs(ball->getAngle())>fromDegrees(60.0)) && (_BallDetector->elapsedTimeSinceLastObs() < ObjectState::LONG_TIME)))
{
if(ball->getAngle()!=0.0)
w = (ball->getAngle()/fabs(ball->getAngle()))*0.6;
else
w = 0.6;
return true;
}else
return false;
//BUILDER COMMENT. DO NOT REMOVE. end Stopped2RotateBody0_transition_code
}
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);
}
});
double MathSymbols::getSin()
{
return sin(fromDegrees(theInstance->alphaValue));
}
double MathSymbols::getNormalize()
{
return toDegrees(normalize(fromDegrees(theInstance->normalizeAngle)));
}
double MathSymbols::getCos()
{
return cos(fromDegrees(theInstance->alphaValue));
}
void BH2011ObstacleSymbols::computeDistanceToClosestCenterLeftAndRightAndCenterAndSomethingElse()
{
if(lastTime == frameInfo.time)
return;
lastTime = frameInfo.time;
freeKickAngleLeft = 0.f;
freeKickAngleRight = 0.f;
const float maxDistance = 3000.f; // infinite
const float maxSqrDistance = maxDistance * maxDistance;
const float maxKickAngleDistance = 1000.f;
const float maxSqrKickAngleDistance = maxKickAngleDistance * maxKickAngleDistance;
const float minKickOpeningAngle = fromDegrees(15.f) / 2.f;
distanceToClosest = distanceToClosestCenterLeft = distanceToClosestCenterRight = maxSqrDistance;
angleToClosest = 0.f;
for(std::vector<ObstacleModel::Obstacle>::const_iterator iter = obstacleModel.obstacles.begin(), end = obstacleModel.obstacles.end(); iter != end; ++iter)
{
const ObstacleModel::Obstacle& obstacle = *iter;
// Compute distance and angle to closest obstacle:
float sqrAbs = obstacle.closestPoint.squareAbs();
if(sqrAbs < distanceToClosest)
{
distanceToClosest = sqrAbs;
angleToClosest = obstacle.closestPoint.angle();
}
// Compute distances to sectors:
if(sqrAbs < distanceToClosestCenterLeft)
if((obstacle.rightCorner.x > 0.f && obstacle.rightCorner.y >= 0.f && obstacle.rightCorner.y < 150.f) || // right corner point in left rectangle
(obstacle.leftCorner.x > 0.f && obstacle.leftCorner.y >= 0.f && obstacle.leftCorner.y < 150.f) || // left corner point in left rectangle
((obstacle.rightCorner.x > 0.f || obstacle.leftCorner.x > 0.f) && obstacle.rightCorner.y < 0.f && obstacle.leftCorner.y > 0.f)) // line segement from left corner to right corner crosses the left rectangle
distanceToClosestCenterLeft = sqrAbs;
if(sqrAbs < distanceToClosestCenterRight)
if((obstacle.rightCorner.x > 0.f && obstacle.rightCorner.y > -150.f && obstacle.rightCorner.y <= 0.f) || // right corner point in right rectangle
(obstacle.leftCorner.x > 0.f && obstacle.leftCorner.y > -150.f && obstacle.leftCorner.y <= 0.f) || // left corner point in right rectangle
((obstacle.rightCorner.x > 0.f || obstacle.leftCorner.x > 0.f) && obstacle.rightCorner.y < 0.f && obstacle.leftCorner.y > 0.f)) // line segement from left corner to right corner crosses the right rectangle
distanceToClosestCenterRight = sqrAbs;
// Compute kick angles:
if(sqrAbs < maxSqrKickAngleDistance) // Consider this obstacle
{
float leftObstacleAngle = obstacle.leftCorner.angle();
float rightObstacleAngle = obstacle.rightCorner.angle();
// Check, if right angle is inside obstacle:
if((leftObstacleAngle + minKickOpeningAngle > freeKickAngleLeft) && (rightObstacleAngle - minKickOpeningAngle < freeKickAngleLeft))
{
freeKickAngleLeft = leftObstacleAngle + minKickOpeningAngle;
}
// Check, if left angle is inside obstacle:
if((leftObstacleAngle + minKickOpeningAngle > freeKickAngleRight) && (rightObstacleAngle - minKickOpeningAngle < freeKickAngleRight))
{
freeKickAngleRight = rightObstacleAngle - minKickOpeningAngle;
}
}
}
distanceToClosest = distanceToClosest < maxSqrDistance ? sqrt(distanceToClosest) : maxDistance;
distanceToClosestCenterLeft = distanceToClosestCenterLeft < maxSqrDistance ? sqrt(distanceToClosestCenterLeft) : maxDistance;
distanceToClosestCenterRight = distanceToClosestCenterRight < maxSqrDistance ? sqrt(distanceToClosestCenterRight) : maxDistance;
distanceToClosestCenter = distanceToClosestCenterLeft < distanceToClosestCenterRight ? distanceToClosestCenterLeft : distanceToClosestCenterRight;
angleToClosest = toDegrees(angleToClosest);
freeKickAngleLeft = toDegrees(freeKickAngleLeft);
freeKickAngleRight = toDegrees(freeKickAngleRight);
}
