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 } }