void
SexpParser::SexpToPredicate
(boost::shared_ptr<PredicateList>& predList, const sexp_t* const sexp)
{
// throw away outer brackets (i.e. we have a list at the top
// level)
if (sexp->ty != SEXP_LIST)
{
return;
}
sexp_t* s = const_cast<sexp_t*>(sexp->list);
if (
(s == 0) ||
(s->ty != SEXP_VALUE)
)
{
return;
}
Predicate& predicate = predList->AddPredicate();
predicate.name = string(s->val);
SexpToList(predicate.parameter,s->next);
}
bool ImagePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
mRender->RequestRender();
int size = mRender->GetDataSize();
if (size == 0)
return false;
Predicate &predicate = predList->AddPredicate();
predicate.name = mPredicateName + "IMG";
predicate.parameter.Clear();
ParameterList &sizeElement = predicate.parameter.AddList();
sizeElement.AddValue(std::string("s"));
sizeElement.AddValue(mRender->GetWidth());
sizeElement.AddValue(mRender->GetHeight());
ParameterList &dataElement = predicate.parameter.AddList();
dataElement.AddValue(std::string("d"));
const char* data = mRender->GetData();
string datacode = mB64Encoder.encode(data, size);
dataElement.AddValue(datacode);
return true;
}
bool
GyroRatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
Predicate &predicate = predList->AddPredicate();
predicate.name = "GYR";
predicate.parameter.Clear();
ParameterList &nameElement = predicate.parameter.AddList();
nameElement.AddValue(std::string("n"));
nameElement.AddValue(GetName());
Matrix invRot = mBody->GetRotation();
invRot.InvertRotationMatrix();
Vector3f rate = invRot * mBody->GetAngularVelocity();
ParameterList &ratesElement = predicate.parameter.AddList();
ratesElement.AddValue(std::string("rt"));
ratesElement.AddValue(gRadToDeg(rate.x()));
ratesElement.AddValue(gRadToDeg(rate.y()));
ratesElement.AddValue(gRadToDeg(rate.z()));
// What should be done when yrotate is around 90? in that case, the parameters of the atan2 are 0!
// const dReal* q = dBodyGetQuaternion(mBody->GetODEBody());
// float xrotate = gArcTan2(2*(q[0]*q[1]+q[2]*q[3]), 1-2*(q[1]*q[1]+q[2]*q[2]));
// float yrotate = gArcSin(2*(q[0]*q[2] - q[3]*q[1]));
// float zrotate = gArcTan2(2*(q[0]*q[3] + q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3]));
//
// ParameterList & anglesElement = predicate.parameter.AddList();
// anglesElement.AddValue(std::string("ang"));
// anglesElement.AddValue(gRadToDeg(xrotate));
// anglesElement.AddValue(gRadToDeg(yrotate));
// anglesElement.AddValue(gRadToDeg(zrotate));
return true;
}
bool
VisionPerceptor::StaticAxisPercept(boost::shared_ptr<PredicateList> predList)
{
Predicate& predicate = predList->AddPredicate();
predicate.name = mPredicateName;
predicate.parameter.Clear();
TTeamIndex ti = mAgentState->GetTeamIndex();
salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
TObjectList visibleObjects;
SetupVisibleObjects(visibleObjects);
for (std::list<ObjectData>::iterator i = visibleObjects.begin();
i != visibleObjects.end(); ++i)
{
ObjectData& od = (*i);
od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
if (mAddNoise)
{
od.mRelPos += mError;
}
if (
(od.mRelPos.Length() <= 0.1) ||
(CheckOcclusion(myPos,od))
)
{
// object is occluded or too close
continue;
}
// theta is the angle in the X-Y (horizontal) plane
od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0]));
// latitude
od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist));
// make some noise
ApplyNoise(od);
// generate a sense entry
AddSense(predicate,od);
}
if (mSenseMyPos)
{
Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("mypos"));
element.AddValue(sensedMyPos[0]);
element.AddValue(sensedMyPos[1]);
element.AddValue(sensedMyPos[2]);
}
return true;
}
bool
AgentStatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
if (mAgentState.get() == 0)
{
return false;
}
--mSenses;
if (mSenses > 0)
{
return false;
}
mSenses = mPerceptRate;
Predicate& predicate = predList->AddPredicate();
predicate.name = "AgentState";
predicate.parameter.Clear();
boost::shared_ptr<BaseNode> parent =
dynamic_pointer_cast<BaseNode>(GetParent().lock());
if (parent.get() == 0)
{
GetLog()->Warning() << "WARNING: (AgentStatePerceptor) "
<< "parent node is not derived from BaseNode\n";
} else {
boost::shared_ptr<RestrictedVisionPerceptor> rvp =
parent->FindChildSupportingClass<RestrictedVisionPerceptor>(false);
if (rvp.get() == 0)
{
GetLog()->Warning() << "WARNING: (AgentStatePerceptor) "
<< "cannot find RestrictedVisionPerceptor instance\n";
} else {
// pan tilt information
ParameterList& pantiltElement = predicate.parameter.AddList();
pantiltElement.AddValue(std::string("pan_tilt"));
pantiltElement.AddValue(gRound(rvp->GetPan()));
pantiltElement.AddValue(gRound(rvp->GetTilt()));
}
}
// battery
ParameterList& batteryElement = predicate.parameter.AddList();
batteryElement.AddValue(std::string("battery"));
batteryElement.AddValue(mAgentState->GetBattery());
// temperature
ParameterList& tempElement = predicate.parameter.AddList();
tempElement.AddValue(std::string("temp"));
tempElement.AddValue(mAgentState->GetTemperature());
return true;
}
bool ForceResistancePerceptor::Percept(
boost::shared_ptr<oxygen::PredicateList> predList)
{
if (mContactList.empty())
{
mLastForce.Zero();
mLastCenter.Zero();
return false;
}
Vector3f force(0,0,0);
Vector3f center(0,0,0);
float sumLength = 0;
for (TContactList::const_iterator i = mContactList.begin();
i!= mContactList.end();
++i)
{
Vector3f forceVec(i->second.f1[0], i->second.f1[1], i->second.f1[2]);
force += forceVec;
float forcevalue = forceVec.Length();
center += Vector3f(i->first.pos[0], i->first.pos[1], i->first.pos[2])
* forcevalue;
sumLength += forcevalue;
}
if (sumLength != 0) // sumLength will be zero when the body is disabled
{
Matrix invRot = mBody->GetLocalTransform();
invRot.InvertRotationMatrix();
mLastCenter = invRot * (center / sumLength);
mLastForce = invRot.Rotate(force);
}
Predicate& predicate = predList->AddPredicate();
predicate.name = "FRP";
predicate.parameter.Clear();
ParameterList& nameElement = predicate.parameter.AddList();
nameElement.AddValue(std::string("n"));
nameElement.AddValue(GetName());
ParameterList& posElement = predicate.parameter.AddList();
posElement.AddValue(std::string("c"));
posElement.AddValue(mLastCenter.x());
posElement.AddValue(mLastCenter.y());
posElement.AddValue(mLastCenter.z());
ParameterList& forceElement = predicate.parameter.AddList();
forceElement.AddValue(std::string("f"));
forceElement.AddValue(mLastForce.x());
forceElement.AddValue(mLastForce.y());
forceElement.AddValue(mLastForce.z());
return true;
}
bool
GameStatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
if (
(mGameState.get() == 0) ||
(mAgentState.get() == 0)
)
{
return false;
}
Predicate& predicate = predList->AddPredicate();
predicate.name = "GS";
predicate.parameter.Clear();
// with the first GameState percept after the player is assigned
// to a team it receives info about it's team and unum assignment
// along with outher soccer parameters
if (
(mFirstPercept) &&
(mAgentState->GetTeamIndex() != TI_NONE)
)
{
mFirstPercept = false;
InsertInitialPercept(predicate);
}
if (mReportScore) {
// score left
ParameterList& slElement = predicate.parameter.AddList();
slElement.AddValue(string("sl"));
slElement.AddValue(mGameState->GetScore(TI_LEFT));
// score right
ParameterList& srElement = predicate.parameter.AddList();
srElement.AddValue(string("sr"));
srElement.AddValue(mGameState->GetScore(TI_RIGHT));
}
// time
ParameterList& timeElement = predicate.parameter.AddList();
timeElement.AddValue(string("t"));
timeElement.AddValue(mGameState->GetTime());
// playmode
ParameterList& pmElement = predicate.parameter.AddList();
pmElement.AddValue(string("pm"));
pmElement.AddValue(SoccerBase::PlayMode2Str(mGameState->GetPlayMode()));
return true;
}
bool ImagePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
if ( !Render() )
return false;
Predicate &predicate = predList->AddPredicate();
predicate.name = "Image";
predicate.parameter.Clear();
// ParameterList &sizeElement = predicate.parameter.AddList();
// sizeElement.AddValue(std::string("s"));
// sizeElement.AddValue(mCamera->GetViewportWidth());
// sizeElement.AddValue(mCamera->GetViewportHeight());
predicate.parameter.AddValue(mData);
return true;
}
bool Accelerometer::Percept(boost::shared_ptr<PredicateList> predList)
{
Predicate &predicate = predList->AddPredicate();
predicate.name = "ACC";
predicate.parameter.Clear();
ParameterList &nameElement = predicate.parameter.AddList();
nameElement.AddValue(std::string("n"));
nameElement.AddValue(GetName());
ParameterList &ratesElement = predicate.parameter.AddList();
ratesElement.AddValue(std::string("a"));
ratesElement.AddValue(mAcc.x());
ratesElement.AddValue(mAcc.y());
ratesElement.AddValue(mAcc.z());
return true;
}
bool
GameStatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
if (
(mGameState.get() == 0) ||
(mAgentState.get() == 0)
)
{
return false;
}
Predicate& predicate = predList->AddPredicate();
predicate.name = "GS";
predicate.parameter.Clear();
// with the first GameState percept after the player is assigned
// to a team it receives info about it's team and unum assignment
// along with outher soccer parameters
if (
(mFirstPercept) &&
(mAgentState->GetTeamIndex() != TI_NONE)
)
{
mFirstPercept = false;
InsertInitialPercept(predicate);
}
// time
ParameterList& timeElement = predicate.parameter.AddList();
timeElement.AddValue(string("t"));
timeElement.AddValue(mGameState->GetTime());
// playmode
ParameterList& pmElement = predicate.parameter.AddList();
pmElement.AddValue(string("pm"));
if (mAgentState->IsPenalized())
pmElement.AddValue(string("penalized"));
else
pmElement.AddValue(mGameState->GetPlayModeStr());
return true;
}
bool HingePerceptor::Percept(boost::shared_ptr<oxygen::PredicateList> predList)
{
if (mJoint.get() == 0)
{
return false;
}
Predicate& predicate = predList->AddPredicate();
predicate.name = "HJ";
predicate.parameter.Clear();
ParameterList& nameElement = predicate.parameter.AddList();
nameElement.AddValue(string("n"));
nameElement.AddValue(GetName());
InsertAxisAngle(predicate);
//InsertAxisRate(predicate);
//InsertAxisTorque(predicate);
return true;
}
bool TimePerceptor::Percept(boost::shared_ptr<oxygen::PredicateList> predList)
{
if (mSimulationServer.get() == 0)
{
return false;
}
Predicate& predicate = predList->AddPredicate();
predicate.name = "time";
predicate.parameter.Clear();
ParameterList& nowElement = predicate.parameter.AddList();
nowElement.AddValue(string("now"));
nowElement.AddValue(mSimulationServer->GetTime());
// ParameterList& stepElement = predicate.parameter.AddList();
// stepElement.AddValue(string("st"));
// stepElement.AddValue(mSimulationServer->GetSimStep());
return true;
}
bool
GPS::Percept(boost::shared_ptr<PredicateList> predList)
{
Predicate &predicate = predList->AddPredicate();
predicate.name = "GPS";
predicate.parameter.Clear();
ParameterList &nameElement = predicate.parameter.AddList();
nameElement.AddValue(std::string("n"));
nameElement.AddValue(GetName());
const Matrix& tf = mTransformParent->GetWorldTransform();
ParameterList &tfElement = predicate.parameter.AddList();
tfElement.AddValue(std::string("tf"));
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
tfElement.AddValue(tf(i, j));
}
}
return true;
}
bool
GameTimePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
if (mGameState.get() == 0)
{
return false;
}
Predicate& predicate = predList->AddPredicate();
predicate.name = "time";
predicate.parameter.Clear();
// with the first GameState percept after the player is assigned
// to a team it receives info about it's team and unum assignment
// along with outher soccer parameters
// time
ParameterList& timeElement = predicate.parameter.AddList();
timeElement.AddValue(string("now"));
timeElement.AddValue(mGameState->GetTime());
return true;
}
bool
VisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
{
Predicate& predicate = predList->AddPredicate();
predicate.name = mPredicateName;
predicate.parameter.Clear();
// get the transformation matrix describing the current orientation
const Matrix& mat = mTransformParent->GetWorldTransform();
TObjectList visibleObjects;
SetupVisibleObjects(visibleObjects);
for (std::list<ObjectData>::iterator i = visibleObjects.begin();
i != visibleObjects.end(); ++i)
{
ObjectData& od = (*i);
if (mAddNoise)
{
od.mRelPos += mError;
}
if (od.mRelPos.Length() <= 0.1)
{
// object is too close
continue;
}
// determine position relative to the local reference frame
Vector3f localRelPos = mat.InverseRotate(od.mRelPos);
// theta is the angle in horizontal plane, with fwAngle as 0 degree
od.mTheta = gNormalizeDeg (gRadToDeg(gNormalizeRad(
gArcTan2(localRelPos[1],localRelPos[0])
)) -90 );
// latitude with fwPhi as 0 degreee
od.mPhi = gRadToDeg(gNormalizeRad(
gArcTan2( localRelPos[2], Vector2f(localRelPos[0], localRelPos[1]).Length())
));
// make some noise
ApplyNoise(od);
// generate a sense entry
AddSense(predicate,od);
}
if (mSenseMyPos)
{
TTeamIndex ti = mAgentState->GetTeamIndex();
salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("mypos"));
element.AddValue(sensedMyPos[0]);
element.AddValue(sensedMyPos[1]);
element.AddValue(sensedMyPos[2]);
}
return true;
}
bool
HearPerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
if (
(mAgentState.get() == 0) ||
(mGameState.get() == 0)
)
{
return false;
}
bool result = false;
string message;
string team;
float direction;
if (mAgentState->GetSelfMessage(message))
{
string self = "self";
Predicate& predicate = predList->AddPredicate();
predicate.name = "hear";
predicate.parameter.Clear();
if (mLabelMessages) {
predicate.parameter.AddValue(mAgentState->GetPerceptName(ObjectState::PT_Player));
}
predicate.parameter.AddValue(mGameState->GetTime());
predicate.parameter.AddValue(self);
predicate.parameter.AddValue(message);
result = true;
}
if (mAgentState->GetMessage(message, team, direction, true))
{
Predicate& predicate = predList->AddPredicate();
predicate.name = "hear";
predicate.parameter.Clear();
if (mLabelMessages) {
predicate.parameter.AddValue(team);
}
predicate.parameter.AddValue(mGameState->GetTime());
predicate.parameter.AddValue(direction);
predicate.parameter.AddValue(message);
result = true;
}
if (mAgentState->GetMessage(message, team, direction, false))
{
Predicate& predicate = predList->AddPredicate();
predicate.name = "hear";
predicate.parameter.Clear();
if (mLabelMessages) {
predicate.parameter.AddValue(team);
}
predicate.parameter.AddValue(mGameState->GetTime());
predicate.parameter.AddValue(direction);
predicate.parameter.AddValue(message);
result = true;
}
return result;
}
bool
RestrictedVisionPerceptor::StaticAxisPercept(boost::shared_ptr<PredicateList> predList)
{
Predicate& predicate = predList->AddPredicate();
predicate.name = mPredicateName;
predicate.parameter.Clear();
TTeamIndex ti = mAgentState->GetTeamIndex();
salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
TNodeObjectsMap visibleNodes;
SetupVisibleNodes(visibleNodes);
for (TNodeObjectsMap::iterator i = visibleNodes.begin();
i != visibleNodes.end(); ++i)
{
boost::shared_ptr<BaseNode> node = (*i).first;
TObjectList& visibleObjects = (*i).second;
for (TObjectList::iterator j = visibleObjects.begin();
j != visibleObjects.end();)
{
ObjectData& od = (*j);
if (mAddNoise)
{
od.mRelPos += mError;
}
if (od.mRelPos.Length() <= 0.1 ||
(CheckOcclusion(myPos,od))
)
{
// object is too close
j = visibleObjects.erase(j);
continue;
}
// theta is the angle in the X-Y (horizontal) plane
assert(gAbs(GetPan()) <= 360);
od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0])) - GetPan();
od.mTheta = gNormalizeDeg(od.mTheta);
// latitude
assert(gAbs(GetTilt()) <= 360);
od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist)) - GetTilt();
od.mPhi = gNormalizeDeg(od.mPhi);
if (gAbs(od.mTheta) > mHViewCone ||
gAbs(od.mPhi) > mVViewCone
)
{
j = visibleObjects.erase(j);
continue;
}
// make some noise
ApplyNoise(od);
++j;
}
// generate a sense entry
AddSense(predicate, node, visibleObjects);
}
if (mSenseMyPos)
{
Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("mypos"));
element.AddValue(sensedMyPos[0]);
element.AddValue(sensedMyPos[1]);
element.AddValue(sensedMyPos[2]);
}
if (mSenseMyOrien)
{
// Orientation
TTeamIndex ti = mAgentState->GetTeamIndex();
Vector3f sensedMyUp = SoccerBase::FlipView(mTransformParent->GetWorldTransform().Up(), ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("myorien"));
element.AddValue(gRadToDeg(gArcTan2(sensedMyUp[1], sensedMyUp[0])));
}
if (mSenseBallPos)
{
TTeamIndex ti = mAgentState->GetTeamIndex();
boost::shared_ptr<Ball> ball;
SoccerBase::GetBall(*this, ball);
Vector3f sensedBallPos = SoccerBase::FlipView(ball->GetWorldTransform().Pos(), ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("ballpos"));
element.AddValue(sensedBallPos[0]);
element.AddValue(sensedBallPos[1]);
element.AddValue(sensedBallPos[2]);
}
if (mSenseLine)
{
SenseLine(predicate);
}
return true;
}
bool
RestrictedVisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
{
Predicate& predicate = predList->AddPredicate();
predicate.name = mPredicateName;
predicate.parameter.Clear();
const int hAngle_2 = mHViewCone >> 1;
const int vAngle_2 = mVViewCone >> 1;
// get the transformation matrix describing the current orientation
const Matrix& mat = mTransformParent->GetWorldTransform();
TNodeObjectsMap visibleNodes;
SetupVisibleNodes(visibleNodes);
for (TNodeObjectsMap::iterator i = visibleNodes.begin();
i != visibleNodes.end(); ++i)
{
boost::shared_ptr<BaseNode> node = (*i).first;
TObjectList& visibleObjects = (*i).second;
for (TObjectList::iterator j = visibleObjects.begin();
j != visibleObjects.end();)
{
ObjectData& od = (*j);
if (mAddNoise)
{
od.mRelPos += mError;
}
if (od.mRelPos.Length() <= 0.1)
{
// object is too close
j = visibleObjects.erase(j);
continue;
}
// determine position relative to the local reference frame
Vector3f localRelPos = mat.InverseRotate(od.mRelPos);
// theta is the angle in horizontal plane, with fwAngle as 0 degree
od.mTheta = gNormalizeDeg (gRadToDeg(gNormalizeRad(
gArcTan2(localRelPos[1],localRelPos[0])
)) -90 );
if (gAbs(od.mTheta) > hAngle_2)
{
// object is out of view range
// GetLog()->Debug() << "(RestrictedVisionPerceptor) Omitting "
// << od.mObj->GetPerceptName() << od.mObj->GetID()
// << ": h-angle = " << od.mTheta << ".\n" ;
j = visibleObjects.erase(j);
continue;
}
// latitude with fwPhi as 0 degreee
od.mPhi = gRadToDeg(gNormalizeRad(
gArcTan2(localRelPos[2],
Vector2f(localRelPos[0], localRelPos[1]).Length()
)
)
);
if (gAbs(od.mPhi) > vAngle_2)
{
j = visibleObjects.erase(j);
continue;
}
// make some noise
ApplyNoise(od);
++j;
}
// generate a sense entry
AddSense(predicate, node, visibleObjects);
}
if (mSenseMyPos)
{
TTeamIndex ti = mAgentState->GetTeamIndex();
salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("mypos"));
element.AddValue(sensedMyPos[0]);
element.AddValue(sensedMyPos[1]);
element.AddValue(sensedMyPos[2]);
}
if (mSenseMyOrien)
{
// Orientation
TTeamIndex ti = mAgentState->GetTeamIndex();
Vector3f sensedMyUp = SoccerBase::FlipView(mTransformParent->GetWorldTransform().Up(), ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("myorien"));
element.AddValue(gRadToDeg(gArcTan2(sensedMyUp[1], sensedMyUp[0])));
}
if (mSenseBallPos)
{
TTeamIndex ti = mAgentState->GetTeamIndex();
boost::shared_ptr<Ball> ball;
SoccerBase::GetBall(*this, ball);
Vector3f sensedBallPos = SoccerBase::FlipView(ball->GetWorldTransform().Pos(), ti);
ParameterList& element = predicate.parameter.AddList();
element.AddValue(std::string("ballpos"));
element.AddValue(sensedBallPos[0]);
element.AddValue(sensedBallPos[1]);
element.AddValue(sensedBallPos[2]);
}
if (mSenseLine)
{
SenseLine(predicate);
}
return true;
}