void ItemSpawn::addItem(uint16_t offset, uint16_t categoryOffset) {
std::normal_distribution<double> normRand(4.0, 2.0);
double result;
while ((result = normRand(rand)) < 0);
Category &c = categories.at(categoryOffset);
items.emplace(offset, std::move(Item(c, result)));
c.items++;
}
void World::applyRelativeControl(const vector<Control*>& controls, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
}
//This is hack to easily determine the displacement of each control
void World::applyRelativeControl(const vector<Control*>& controls, vector<double>& displacements, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
vector<Vector3d> pre_positions;
if ((cursors.size() == 2) || (displacements.size() == 2)) {
pre_positions.resize(2);
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
pre_positions[i] = cursors[i]->end_eff->getPosition();
}
} else {
for (int i = 0; i < displacements.size(); i++) {
displacements[i] = -1;
}
}
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
if ((cursors.size() == 2) || (displacements.size() == 3)) {
Matrix<double,6,1> u_tran;
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
//cout << "norm of end effector " << i << ": " << (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm() << endl;
displacements[i] = (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm();
u_tran.segment(i*3,3) = (cursors[i]->end_eff->getPosition() - pre_positions[i]);
}
displacements[2] = u_tran.norm();
}
}
/* Drive during race. */
static void
drive(int index, tCarElt* car, tSituation *s)
{
total_tics[index]++;
char line[UDP_MSGLEN];
#ifdef __PRINT_RACE_RESULTS__
bestLap[index]=car->_bestLapTime;
damages[index]=car->_dammage;
totalTime[index]=car->_timeBehindLeader;
#endif
#ifdef __DISABLE_RESTART__
if (RESTARTING[index]==1)
{
clientAddressLength[index] = sizeof(clientAddress[index]);
// Set line to all zeroes
memset(line, 0x0, 101);
if (recvfrom(listenSocket[index], line, 100, 0,
(struct sockaddr *) &clientAddress[index],
&clientAddressLength[index]) < 0)
{
std::cerr << "Error: problem in receiving from the listen socket";
exit(1);
}
#ifdef __UDP_SERVER_VERBOSE__
// show the client's IP address
std::cout << " from " << inet_ntoa(clientAddress[index].sin_addr);
// show the client's port number.
std::cout << ":" << ntohs(clientAddress[index].sin_port) << "\n";
// Show the line
std::cout << " Received: " << line << "\n";
#endif
// compare received string with the ID
if (strncmp(line,UDP_ID,3)==0)
{
#ifdef __UDP_SERVER_VERBOSE__
std::cout << "IDENTIFIED" << std::endl;
#endif
// char line[UDP_MSGLEN];
sprintf(line,"***identified***");
// Sending the car state to the client
if (sendto(listenSocket[index], line, strlen(line) + 1, 0,
(struct sockaddr *) &clientAddress[index],
sizeof(clientAddress[index])) < 0)
std::cerr << "Error: cannot send identification message";
RESTARTING[index]=0;
}
}
#endif
// local variables for UDP
struct timeval timeVal;
fd_set readSet;
// computing distance to middle
float dist_to_middle = 2*car->_trkPos.toMiddle/(car->_trkPos.seg->width);
// computing the car angle wrt the track axis
float angle = RtTrackSideTgAngleL(&(car->_trkPos)) - car->_yaw;
NORM_PI_PI(angle); // normalize the angle between -PI and + PI
//Update focus sensors' angle
for (int i = 0; i < 5; ++i) {
focusSens[index]->setSensor(i,(car->_focusCmd)+i-2,200);
}
// update the value of track sensors only as long as the car is inside the track
float trackSensorOut[19];
float focusSensorOut[5];//ML
if (dist_to_middle<=1.0 && dist_to_middle >=-1.0 )
{
trackSens[index]->sensors_update();
for (int i = 0; i < 19; ++i)
{
trackSensorOut[i] = trackSens[index]->getSensorOut(i);
if (getNoisy())
trackSensorOut[i] *= normRand(1,__NOISE_STD__);
}
focusSens[index]->sensors_update();//ML
if ((car->_focusCD <= car->_curLapTime + car->_curTime)//ML Only send focus sensor reading if cooldown is over
&& (car->_focusCmd != 360))//ML Only send focus reading if requested by client
{//ML
for (int i = 0; i < 5; ++i)
{
focusSensorOut[i] = focusSens[index]->getSensorOut(i);
if (getNoisy())
focusSensorOut[i] *= normRand(1,__FOCUS_NOISE_STD__);
}
car->_focusCD = car->_curLapTime + car->_curTime + 1.0;//ML Add cooldown [seconds]
}//ML
else//ML
{//ML
for (int i = 0; i < 5; ++i)//ML
focusSensorOut[i] = -1;//ML During cooldown send invalid focus reading
}//ML
}
else
{
for (int i = 0; i < 19; ++i)
{
trackSensorOut[i] = -1;
}
for (int i = 0; i < 5; ++i)
{
focusSensorOut[i] = -1;
}
}
// update the value of opponent sensors
float oppSensorOut[36];
oppSens[index]->sensors_update(s);
for (int i = 0; i < 36; ++i)
{
oppSensorOut[i] = oppSens[index]->getObstacleSensorOut(i);
if (getNoisy())
oppSensorOut[i] *= normRand(1,__OPP_NOISE_STD__);
}
float wheelSpinVel[4];
for (int i=0; i<4; ++i)
{
wheelSpinVel[i] = car->_wheelSpinVel(i);
}
if (prevDist[index]<0)
{
prevDist[index] = car->race.distFromStartLine;
}
float curDistRaced = car->race.distFromStartLine - prevDist[index];
prevDist[index] = car->race.distFromStartLine;
if (curDistRaced>100)
{
curDistRaced -= curTrack->length;
}
if (curDistRaced<-100)
{
curDistRaced += curTrack->length;
}
distRaced[index] += curDistRaced;
float totdist = curTrack->length * (car->race.laps -1) + car->race.distFromStartLine;
// std::cerr << "totraced: " << totdist << std::endl;
/**********************************************************************
****************** Building state string *****************************
**********************************************************************/
string stateString;
stateString = SimpleParser::stringify("angle", angle);
stateString += SimpleParser::stringify("curLapTime", float(car->_curLapTime));
stateString += SimpleParser::stringify("damage", ( getDamageLimit() ? car->_dammage : car->_fakeDammage ) );
stateString += SimpleParser::stringify("distFromStart", car->race.distFromStartLine);
stateString += SimpleParser::stringify("totalDistFromStart", totdist);
stateString += SimpleParser::stringify("distRaced", distRaced[index]);
stateString += SimpleParser::stringify("fuel", car->_fuel);
stateString += SimpleParser::stringify("gear", car->_gear);
stateString += SimpleParser::stringify("lastLapTime", float(car->_lastLapTime));
stateString += SimpleParser::stringify("opponents", oppSensorOut, 36);
stateString += SimpleParser::stringify("racePos", car->race.pos);
stateString += SimpleParser::stringify("rpm", car->_enginerpm*10);
stateString += SimpleParser::stringify("speedX", float(car->_speed_x * 3.6));
stateString += SimpleParser::stringify("speedY", float(car->_speed_y * 3.6));
stateString += SimpleParser::stringify("speedZ", float(car->_speed_z * 3.6));
stateString += SimpleParser::stringify("track", trackSensorOut, 19);
stateString += SimpleParser::stringify("trackPos", dist_to_middle);
stateString += SimpleParser::stringify("wheelSpinVel", wheelSpinVel, 4);
stateString += SimpleParser::stringify("z", car->_pos_Z - RtTrackHeightL(&(car->_trkPos)));
stateString += SimpleParser::stringify("focus", focusSensorOut, 5);//ML
//GIUSE - VISION HERE!
// printf("size: %d\n",car->vision->imgsize);
if( getVision() ){
// std::cout << car->vision->imgsize << std::endl;
stateString += SimpleParser::stringify("img", car->vision->img, car->vision->imgsize);
}
// for(int i=0; i < car->vision->imgsize; i++){
// std::cout << (int)car->vision->img[i] << " ";
// }
// GIUSE - that's UGLY, can we stay coherent with either char* or string??
// char line[UDP_MSGLEN];
// memset(line, 0x0,UDP_MSGLEN ); //
// sprintf(line,"%s",stateString.c_str());
if (RESTARTING[index]==0)
{
#ifdef __UDP_SERVER_VERBOSE__
std::cout << "Sending: " << stateString.c_str() << std::endl;
std::cout << "Sending: " << stateString.c_str() << std::endl;
#endif
#ifdef __STEP_LIMIT__
if (total_tics[index]>__STEP_LIMIT__)
{
RESTARTING[index] = 1;
car->RESTART=1;
char fileName[200];
sprintf(fileName,"%s.txt",trackName);
printf("%s.txt\n",trackName);
FILE *f = fopen (fileName,"a");
printf("Dist_raced %lf\n",distRaced[index]);
fprintf(f,"Dist_raced %lf\n",distRaced[index]);
fclose(f);
return;
}
#endif
// Sending the car state to the client
// if (sendto(listenSocket[index], line, strlen(line) + 1, 0,
if (sendto(listenSocket[index], stateString.c_str(), stateString.length() + 1, 0,
(struct sockaddr *) &clientAddress[index],
sizeof(clientAddress[index])) < 0)
std::cerr << "Error: cannot send car state";
// Set timeout for client answer
FD_ZERO(&readSet);
FD_SET(listenSocket[index], &readSet);
timeVal.tv_sec = 0;
timeVal.tv_usec = UDP_TIMEOUT;
memset(line, 0x0,UDP_MSGLEN ); // GIUSE - BUG, THERE WAS A 1000 HARDCODED
if (select(listenSocket[index]+1, &readSet, NULL, NULL, &timeVal))
{
// Read the client controller action
// memset(line, 0x0,UDP_MSGLEN ); // Zero out the buffer. GIUSE - already done
int numRead = recv(listenSocket[index], line, UDP_MSGLEN, 0);
if (numRead < 0)
{
std::cerr << "Error, cannot get any response from the client!";
CLOSE(listenSocket[index]);
exit(1);
}
#ifdef __UDP_SERVER_VERBOSE__
std::cout << "Received: " << line << std::endl;
#endif
std::string lineStr(line);
CarControl carCtrl(lineStr);
if (carCtrl.getMeta()==RACE_RESTART)
{
RESTARTING[index] = 1;
#ifdef __DISABLE_RESTART__
// char line[UDP_MSGLEN];
memset(line, 0x0,UDP_MSGLEN );
sprintf(line,"***restart***");
// Sending the car state to the client
if (sendto(listenSocket[index], line, strlen(line) + 1, 0,
(struct sockaddr *) &clientAddress[index],
sizeof(clientAddress[index])) < 0)
std::cerr << "Error: cannot send restart message";
#else
car->RESTART=1;
#endif
}
// Set controls command and store them in variables
oldAccel[index] = car->_accelCmd = carCtrl.getAccel();
oldBrake[index] = car->_brakeCmd = carCtrl.getBrake();
oldGear[index] = car->_gearCmd = carCtrl.getGear();
oldSteer[index] = car->_steerCmd = carCtrl.getSteer();
oldClutch[index] = car->_clutchCmd = carCtrl.getClutch();
oldFocus[index] = car->_focusCmd = carCtrl.getFocus();//ML
}
else
{
//#ifdef __UDP_SERVER_VERBOSE__
std::cout << "Timeout for client answer\n";
//#endif
// If no new controls are availables uses old ones...
car->_accelCmd = oldAccel[index];
car->_brakeCmd = oldBrake[index];
car->_gearCmd = oldGear[index];
car->_steerCmd = oldSteer[index];
car->_clutchCmd = oldClutch[index];
car->_focusCmd = oldFocus[index];//ML
}
}
else
{
car->_accelCmd = oldAccel[index];
car->_brakeCmd = oldBrake[index];
car->_gearCmd = oldGear[index];
car->_steerCmd = oldSteer[index];
car->_clutchCmd = oldClutch[index];
car->_focusCmd = oldFocus[index];//ML
}
}
