/* LICENSE: ========================================================================= CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7 Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso] School of Computer Science, Carnegie Mellon University All rights reserved. ========================================================================= */ #include "WorldModel.h" #include "WMStructs.h" #include "../Motion/MotionInterface.h" #include "../headers/CircBufPacket.h" #include "../headers/Util.h" #include "../headers/Sensors.h" #include "../Behaviors/KickLib.h" #include "../Main/RobotMain.h" #include using std::vector; static int wmodelThrottle = 0; static int trackerThrottle = 0; static int wmDebugThrottle = 0; vector2d stddev_helper(VisionInterface::VObject obj_info); int findMatch(Gaussian2 * opponent, int num_opponent, vector2d position, ulong time); bool inFieldBounds(vector2d pos); WorldModel::WorldModel() : ball_tracker(BallTracker::tMULTIPOS), teammate_ball_tracker(TeammateBallTracker::tMULTIPOS) { // Let's make sure the object is at least in a sane, known state // before init is called 'cuz it is possible for someone to touch // this object before that happens. goalColor = teamColor = myID = 0; lastUpdateModelTime = 0; wmOutStream = NULL; ball = Gaussian2(); self_position = Gaussian2(); self_heading.set(0,0); ball_on_chest_prob = 0; hist_cnt = 0; odo_position.set(0,0); odo_heading.set(1,0); // wmdebug_deque.push_back(WMDebug(100,100,1,0x4D,0x00,0x88)); last_motion_state = last_motion_state_type = 0; } void WorldModel::init(int gColor, int tColor, int ID, ulong time) { lastUpdateModelTime = time; goalColor = gColor; teamColor = tColor; myID = ID; wmOutStream = NULL; /* Initialize the world model to default values */ ball = Gaussian2(); ball.mean.set(100.0,0.0); self_position = Gaussian2(); self_heading.set(1.0,0.0); /* Since we don't know where anything is, make the standard deviations very large. */ ball.setsxsy(halfLength, halfLength, 0); /* clear the ball history */ hist_cnt = 0; for(int i = 0; i < BALL_HIST_LEN; i++){ ballHistory[i].time = 0; ballHistory[i].odo_frame.set(0.0, 0.0); ballHistory[i].saw_ball = false; } odo_heading.set(1.0, 0.0); odo_position.set(0.0, 0.0); } /* We want to allocate a buffer large enough to hold 10 messages. Each message currently encodes 12 objects (3 teammates, 4 opponents, the ball, two sides for each goal). Each object requires 5 fields (mean.x, mean.y, sMaj, sMin, thetaMaj) */ void WorldModel::initializeStreams(PacketStreamCollection *out_psc) { int stream_id; stream_id = out_psc->allocateStream(WORLD_MODEL_ENCODER_SIZE*10,10); wmOutStream = out_psc->getStream(stream_id); wmOutStream->type = SPOutEncoder::ModelObjLead; wmOutStream->binary = true; stream_id = out_psc->allocateStream(TRACKER_ENCODER_SIZE*10,10); trackerOutStream = out_psc->getStream(stream_id); trackerOutStream->type = SPOutEncoder::TrackerLead; trackerOutStream->binary = true; stream_id = out_psc->allocateStream(WMDEBUG_ENCODER_SIZE*10,10); wmDebugOutStream = out_psc->getStream(stream_id); wmDebugOutStream->type = SPOutEncoder::WMDebugLead; wmDebugOutStream->binary = true; } /* Call all the update functions. This ensures that functions are called in the correct order */ void WorldModel::updateModel(VisionInterface::ObjectInfo *obj_info, SensorData *sensor_data, TeamMsgMgr *msg_mgr, RobotPositionInfo const posn, VisionInterface::HeadVelocity *head_vel, ulong time) { doPosnUpdate(posn, time); doSensorUpdate(obj_info,head_vel,sensor_data, time); doStatusInfoUpdate(msg_mgr, time); doTimeUpdate(time); // All of our updates are done lastUpdateModelTime = time; } /* Iterate over the objects that are seen. If an object has a high enough confidence, merge its new reported location with the old location in the world model and reset the timestamp. Since we trust each robot's localization more than we trust the vision, don't bother to update if you see your teammates. We will get that info from the shared world model. */ void WorldModel::doSensorUpdate(VisionInterface::ObjectInfo *obj_info, VisionInterface::HeadVelocity *head_vel, SensorData* sensor_data, ulong time) { // Look for robots for(int i=0; iblue_robots[i].confidence > .5) { RobotObservation obs; obs.my_position = self_position; obs.robot_posn.set(obj_info->blue_robots[i].loc.x, obj_info->blue_robots[i].loc.y); obs.vision_conf = obj_info->blue_robots[i].confidence; // If a reading from the IR distance sensor is available, we should // use that instead of the distance from vision; it's much, much // more accurate. if(obj_info->blue_robots[i].confInFrontOfIR > .5) { // Should we use the near or far IR sensor? if(obs.robot_posn.length() < 500) { obs.robot_posn = obs.robot_posn.norm()* sensor_data->getFrame(0)->IRDistHeadNear; } else { obs.robot_posn = obs.robot_posn.norm()* sensor_data->getFrame(0)->IRDistHeadFar; } } // Convert to a global posn obs.robot_posn = obs.robot_posn.rotate(self_heading.angle()); obs.robot_posn += self_position.mean; obs.robot_color = TEAM_COLOR_BLUE; obs.obs_time = GetTime(); blue_robot_obs.push_front(obs); } // Handle red robots if(obj_info->red_robots[i].confidence > .5) { RobotObservation obs; obs.my_position = self_position; obs.robot_posn.set(obj_info->red_robots[i].loc.x, obj_info->red_robots[i].loc.y); obs.vision_conf = obj_info->red_robots[i].confidence; // Use IR reading if one is available. if(obj_info->red_robots[i].confInFrontOfIR > .5) { // Should we use the near or far IR sensor? if(obs.robot_posn.length() < 500) { obs.robot_posn = obs.robot_posn.norm()* sensor_data->getFrame(0)->IRDistHeadNear; } else { obs.robot_posn = obs.robot_posn.norm()* sensor_data->getFrame(0)->IRDistHeadFar; } } // Convert to a global posn obs.robot_posn = obs.robot_posn.rotate(self_heading.angle()); obs.robot_posn += self_position.mean; obs.robot_color = TEAM_COLOR_RED; obs.obs_time = GetTime(); red_robot_obs.push_front(obs); } } if (obj_info->ball.confidence > MINCONF_BALL) { vector2d ball_odo_pos = localToOdo(vector2d(obj_info->ball.loc.x, obj_info->ball.loc.y)); //===================================================================== // This is the old ball model vector2d std_dev = stddev_helper(obj_info->ball); Gaussian2 new_obj = Gaussian2(time,ball_odo_pos,std_dev.x,std_dev.y,0,true); ball = G2Multiply(ball, new_obj); ball.time = time; // Make sure our uncertainty does not converge past a certain point. if((ball.sMaj < CONVERGE_LIMIT) || (ball.sMin < CONVERGE_LIMIT)) { ball.setsMajsMin(max(CONVERGE_LIMIT, ball.sMaj), max(CONVERGE_LIMIT, ball.sMin), ball.thetaMaj); } //===================================================================== // Propagate with an observation and update the Kalman filter double ball_distance = GVector::distance(ball_odo_pos, odo_position); BallTrjInfo bti = WorldModel::getBallTrajectory(10); if (bti.chi2 > 20*20) { bti.velocity=vector2d(0.0,0.0); } ball_tracker.observe(time, ball_odo_pos, bti.velocity, obj_info->ball.confidence, ball_distance, head_vel->tilt, head_vel->pan, true); //===================================================================== // Store the ball history BallObservation bo; bo.position_local_frame = Gaussian2(time, vector2d(obj_info->ball.loc.x, obj_info->ball.loc.y), std_dev.x,std_dev.y,0,true); bo.position_odo_frame = Gaussian2(time, localToOdo(vector2d(obj_info->ball.loc.x, obj_info->ball.loc.y)), std_dev.x,std_dev.y,0,true); bo.position_global_frame = Gaussian2(time, LocalToGlobal(vector2d(obj_info->ball.loc.x, obj_info->ball.loc.y)), std_dev.x,std_dev.y,0,true); bo.obs_time = time; ball_obs.push_front(bo); //===================================================================== // Add the reading to the ball tracking histogram if(head_vel->tilt < 2.5 && head_vel->pan < 2.5 && last_motion_state_type != Motion::STATE_KICKING){ ballHistAdd(true, obj_info->ball.loc.x, obj_info->ball.loc.y, time, obj_info->ball.confidence); } else { ballHistAdd(false, 0.0, 0.0, time, 0.0); } } else { // Propagate the tracker as if we don't see the ball ball_tracker.propagate(time); ballHistAdd(false, 0.0, 0.0, time, 0.0); } #if 0 { ulong ID; if (ball_tracker.getID(ID,0)) { pprintf(TextOutputStream,"WM: Best Ball ID=%lu\n",ID); } } #endif //======================================================================== // Clean out the history cues ulong time_cutoff = GetTime() - max_obs_age; // Get rid of robot observations that are too old. while(blue_robot_obs.size() > 0 && blue_robot_obs.back().obs_time < time_cutoff) { blue_robot_obs.pop_back(); } while(red_robot_obs.size() > 0 && red_robot_obs.back().obs_time < time_cutoff) { red_robot_obs.pop_back(); } // Get rid of ball observations that are too old while(!ball_obs.empty() && ball_obs.back().obs_time < time_cutoff) { ball_obs.pop_back(); } //======================================================================== // Update the ball-on-chest detector using IR data doChestIRUpdate(sensor_data,time); /* Send to worldModel output to be encoded */ if(config.spoutConfig.dumpModelObj) { if (++wmodelThrottle >= config.spoutConfig.dumpModelObj) { if(wmOutStream!=NULL) { static uchar buf[WORLD_MODEL_ENCODER_SIZE]; int out_size=0; out_size=wm_encoder.encodeWorldModel(buf,this); wmOutStream->writeBinary(buf,out_size); } else { pprintf(TextOutputStream,"NWMOS"); } wmodelThrottle = 0; } } /* Send to worldModel tracker output to be encoded */ if(config.spoutConfig.dumpTracker) { if (++trackerThrottle >= config.spoutConfig.dumpTracker) { if(trackerOutStream!=NULL) { static uchar buf[TRACKER_ENCODER_SIZE]; int out_size=0; out_size=tracker_encoder.encodeTracker(buf,this); trackerOutStream->writeBinary(buf,out_size); } else { pprintf(TextOutputStream,"NTRKOS"); } trackerThrottle = 0; } } /* Send to worldModel debug output to be encoded */ if(config.spoutConfig.dumpWMDebug) { if (++wmDebugThrottle >= config.spoutConfig.dumpWMDebug) { if(wmDebugOutStream!=NULL) { static uchar buf[WMDEBUG_ENCODER_SIZE]; int out_size=0; out_size=wmdebug_encoder.encodeWMDebug(buf,this); wmDebugOutStream->writeBinary(buf,out_size); } else { pprintf(TextOutputStream,"NWMDOS"); } wmDebugThrottle = 0; } } clearWMDebug(); } void WorldModel::updateModelTeamMsg(int sender_id, const StatusMsgEntry &msg) { teammate_msgs[sender_id].push_front(msg); // Handle updates to the teammate ball tracker if (1==msg.shared_info.valid_ball_posn) { // #$# // We have a potential problem with the timestamp here // We have to propagate the uncertainty in the observations // but we have to take the observation information into account teammate_ball_tracker.observe(GetTime(),//msg.timestamp, msg.shared_info.ball_position, true); } // Check to see if any of our teammates have expired messages. // Concise. map >::iterator it; it = teammate_msgs.begin(); while(it!=teammate_msgs.end()) { deque &tm_deque = (*it).second; ulong time_cutoff = GetTime() - max_obs_age; while(tm_deque.size() > 0 && tm_deque.back().timestamp < time_cutoff) tm_deque.pop_back(); it++; } } /* See if any of your teammates have broadcast their location or the location of the ball. Store your teammates' location (we trust them more than we trust our own vision of them). What about the ball? */ void WorldModel::doStatusInfoUpdate(TeamMsgMgr *msg_mgr, ulong time) { #if 0 // We don't want to keep incorporating the same shared information // over and over again. So we track the last estimate from a teammate // that we used and do not use it a second time. static vector2d last_tm_est(0.0, 0.0); /* There are 8 world models stored in the shared world model - one for each possible robot. We have a list of which robots we are receiving messages from, so poll those to see if they contain valid player positions. */ for (int i = 1; i < 4; i++) { teammate[i - 1] = msg_mgr->getTeammatePosn(i); } /* Now look at the ball position in the Shared World model. If we haven't already seen the ball in LOST_BALL_TIME, accept the value from the shared world model, plus some error because the shared world model info is old */ if (time - ball.time > LOST_BALL_TIME) { Gaussian2 new_ball_pos = msg_mgr->getBestSharedBallPosn(); if (new_ball_pos.mean.length() != 0 && new_ball_pos.mean!=last_tm_est) { new_ball_pos = Gaussian2(new_ball_pos.time, new_ball_pos.mean, new_ball_pos.sx + SHARED_BALL_ERROR, new_ball_pos.sy + SHARED_BALL_ERROR, new_ball_pos.psxsy, true); ball = G2Multiply(ball, new_ball_pos); ball.time = new_ball_pos.time; last_tm_est = new_ball_pos.mean; } } #endif } /* Grow the error for all objects that have not been updated yet this timestep */ void WorldModel::doTimeUpdate(ulong time) { if (ball.time != time) { ball = Gaussian2(ball.time, ball.mean, ball.sx + SMALL_ERROR, ball.sy + SMALL_ERROR, ball.psxsy, true); } teammate_ball_tracker.propagate(time); } /* Store the robot's position from the Localization data structure */ void WorldModel::doPosnUpdate(RobotPositionInfo const posn, ulong const time) { self_position = posn.position; self_heading = posn.heading.mean; } /* We update our position in the odometry only coordinate frame based on the latest motion update. */ void WorldModel::updateModelMotion(const Motion::MotionLocalizationUpdate *move) { int ball_idx; double delta_t = move->heading_delta.angle(); double new_theta = odo_heading.angle() + delta_t; odo_heading.set(cos(new_theta), sin(new_theta)); /* Now compute the position part of the update. */ /* Retrieve the movement in the local frame and rotate by heading */ vector2d delta_local = move->pos_delta; vector2d delta_odo_frame = delta_local.rotate(odo_heading.angle()); /* New position is previous position plus differential from move */ odo_position = odo_position + delta_odo_frame; // We update the ball position based on whether or not we think // that we've kicked it. // pprintf(TextOutputStream,"SK=%d lmst=%d mst=%d lms=%d\n",Motion::STATE_KICKING,last_motion_state_type,move->state_type,last_motion_state); if(last_motion_state_type==Motion::STATE_KICKING && move->state_type!=Motion::STATE_KICKING) { // If we kicked the ball, we will place it at a set location // relative to the robot. [Hopefully] the robot will see the // true location of the ball when it looks for it at the // guestimate location. vector2d local_delta = vector2d(0.0, 0.0); switch(last_motion_state) { case Motion::MOTION_KICK_DIVE: local_delta.set(300.0, 0.0); break; case Motion::MOTION_KICK_BUMP: local_delta.set(100.0, 0.0); break; case Motion::MOTION_KICK_FOREWARD: local_delta.set(300.0, 0.0); break; case Motion::MOTION_KICK_HEAD_L: local_delta.set(64.0, 176.0); break; case Motion::MOTION_KICK_HEAD_R: local_delta.set(64.0, -176.0); break; case Motion::MOTION_KICK_HEAD_SOFT_L: local_delta.set(64.0, 176.0); break; case Motion::MOTION_KICK_HEAD_SOFT_R: local_delta.set(64.0, -176.0); break; case Motion::MOTION_KICK_HEAD_HARD_L: local_delta.set(85.0, 234.0); break; case Motion::MOTION_KICK_HEAD_HARD_R: local_delta.set(85.0, -234.0); break; case Motion::MOTION_KICK_SWING_L: local_delta.set(400.0, 600.0); break; case Motion::MOTION_KICK_SWING_R: local_delta.set(400.0, -600.0); break; } // Do we know how to reposition the ball based on the kick? if(local_delta.length() > 0) { ball.mean = localToOdo(local_delta); // We just fudged our ball position. Let's add some uncertainty // 'cuz we really have no idea. ball.setsMajsMin(ball.sMaj + 500, ball.sMin + 500, ball.thetaMaj); } // Adjust the most likely tracker with the new ball estimate if (ball_tracker_closest(ball_idx)) { if (0 != ball_idx) { pprintf(TextOutputStream,"WM: KICK WARNING! NEAREST BALL IS NOT MOST LIKELY!\n"); } // This uses kickLib now float angle,dist; if (getKickAngleDist(last_motion_state,angle,dist)) { //pprintf(TextOutputStream,"WM: Kicklib kick ID:%d angle:%f dist:%f\n",last_motion_state,angle,dist/2.0); vector2d new_pos(dist/2.0,0); new_pos=new_pos.rotate(angle); new_pos=localToOdo(new_pos); ball_tracker.perturb(GetTime(),new_pos,ball_idx); } else { pprintf(TextOutputStream,"WM: Kicklib UNKNOWN kick ID:%d\n",last_motion_state); } } } last_motion_state = move->state; last_motion_state_type = move->state_type; // If the ball bumps against our chest, we need to take into account // that we're pushing it. Okay, this is going to get weird. Instead // of tracking the line of our chest moving through space and seeing // if that intersects the ball, we're going to assume that our motion // is linear along the path from our old position to our new position. // We also assume that our heading doesn't change while moving. // Then we're going to pretend the ball is moving and not us, and see // if the ray of the ball's movement intersects with the line segment // defining our chest. (Perhaps I should lay off the crack) // Get a relative movement vector. In practice, this should // be darn close to straight vector2d my_rel_delta = delta_local; // Get the ball's relative starting position before the move vector2d ball_rel_start; vector2d ball_rel_end; vector2d chest_rel_left; vector2d chest_rel_right; vector2d new_ball_posn; if (USE_OLD_BALL_ESTIMATE) { ball_rel_start = odoToLocal(ball.mean); // Now compute where the ball ends up in relative coorindates ball_rel_end = ball_rel_start - my_rel_delta; // Define where our chest starts and ends chest_rel_left = vector2d(BALL_ON_CHEST_X_MIN, BALL_ON_CHEST_Y_MAX); chest_rel_right = vector2d(BALL_ON_CHEST_X_MIN, -BALL_ON_CHEST_Y_MAX); if(GVector::segment_intersect(ball_rel_start, ball_rel_end, chest_rel_left, chest_rel_right)) { // Center the ball against our chest at our new position. new_ball_posn = vector2d(BALL_ON_CHEST_X_MIN,0); new_ball_posn = localToOdo(new_ball_posn); ball.mean = new_ball_posn; // We should increase our uncertainty too. But I haven't // thought about a good way yet. ball.setsMajsMin(ball.sMaj + 100, ball.sMin + 100, ball.thetaMaj); } } else { if (ball_tracker_closest(ball_idx)) { Gaussian2 _pos; if (ball_tracker_observation(_pos,ball_idx)) { ball_rel_start = _pos.mean; // Now compute where the ball ends up in relative coorindates ball_rel_end = ball_rel_start - my_rel_delta; // Define where our chest starts and ends chest_rel_left = vector2d(BALL_ON_CHEST_X_MIN, BALL_ON_CHEST_Y_MAX); chest_rel_right = vector2d(BALL_ON_CHEST_X_MIN, -BALL_ON_CHEST_Y_MAX); if(GVector::segment_intersect(ball_rel_start, ball_rel_end, chest_rel_left, chest_rel_right)) { // Adjust the most likely tracker with the new ball estimate //pprintf(TextOutputStream,"WM: Dribbling\n"); new_ball_posn = localToOdo(vector2d(BALL_ON_CHEST_X_MIN, _pos.mean.y)); ball_tracker.perturb(GetTime(),new_ball_posn,ball_idx); } } } } } /* End updateModelMotion() */ /* This function returns a standard deviation for the location of the given object that will (in the future) be based on the distance from the object to the robot and the confidence. Right now, it returns a dummy value. */ vector2d stddev_helper(VisionInterface::VObject obj_info) { double dev = 10.0; // We tried to actually do something intelligent here. It made the // world model lag-a-licious, so we're undoing it at RoboCup. // Silly CMU, smarts aren't for robots... // dev = 30.0 + obj_info.distance/50.0 + (1.0-obj_info.confidence)*50.0; return vector2d(dev,dev); } /* Convert an object that the robot has seen (in its local coordinate frame) into global coordinates. */ vector2d WorldModel::LocalToGlobal(vector2d object) { return LocalToGlobal(object, self_position.mean, self_heading); } vector2d WorldModel::LocalToGlobal(vector2d object, vector2d my_posn, vector2d heading) { object = object.rotate(heading.angle()); object += my_posn; return object; } /* Convert an object from global to local coordinates. */ vector2d WorldModel::GlobalToLocal(vector2d object) { return GlobalToLocal(object, self_position.mean, self_heading); } vector2d WorldModel::GlobalToLocal(vector2d object, vector2d my_posn, vector2d heading) { object -= my_posn; object = object.rotate(-heading.angle()); return object; } /* Find the index of the opponent closest to a given position. If the closest opponent is within a reasonable distance (for now, 1/4 of the field length) from the new observation, return its index. Otherwise, return the index of the oldest opponent */ int findMatch(Gaussian2 * opponent, int num_opponent, vector2d position, ulong time){ double best_dist = 2*halfLength; int best_match = -1; for (int i = 0; i < num_opponent; i++) { double dist = sqrt((opponent[i].mean.x - position.x)*(opponent[i].mean.x - position.x) + (opponent[i].mean.y - position.y)*(opponent[i].mean.y - position.y)); if (dist < best_dist) { best_dist = dist; best_match = i; } } if (best_dist < halfLength/2.0) { return best_match; } else { ulong oldest = 0; for (int i = 0; i < num_opponent; i++) { ulong temp_time = time - opponent[i].time; if (temp_time > oldest) { oldest = temp_time; best_match = i; } } return best_match; } } // Fill in robot position, ball position, and recent // robot observations. void WorldModel::fillInStatusMsg(StatusMsgEntry &entry) { entry.shared_info.my_position = self_position; entry.shared_info.my_heading = self_heading; if(!ball_tracker_observation_global(entry.shared_info.ball_position,0)) { entry.shared_info.ball_position = Gaussian2(); entry.shared_info.valid_ball_posn = 0; } else { entry.shared_info.valid_ball_posn = 1; } // Make sure observations get initialized mzero(entry.robot_obs); // We need to be careful; since we send the age of the observation // as the timestamp a timestamp of zero means a super-recent // observation. Using GetTime() as the age works much better. for(int i=0; i= 0; i++) { entry.robot_obs[i] = red_robot_obs[offset]; // We send the age of the observation in the shared message. entry.robot_obs[i].obs_time = GetTime() - red_robot_obs[offset].obs_time; offset--; } } vector2d WorldModel::my_position() { return self_position.mean; } Gaussian2 WorldModel::my_position_estimate() { return self_position; } vector2d WorldModel::my_heading() { return self_heading; } vector2d WorldModel::cyan_goal() { return GlobalToLocal(vector2d(halfLength,0)); } vector2d WorldModel::yellow_goal() { return GlobalToLocal(vector2d(-halfLength,0)); } vector2d WorldModel::score_goal() { if(goalColor==DEFEND_CYAN_GOAL) return yellow_goal(); else return cyan_goal(); } vector2d WorldModel::defend_goal() { if(goalColor==DEFEND_CYAN_GOAL) return cyan_goal(); else return yellow_goal(); } vector2d WorldModel::ball_posn() { return odoToLocal(ball.mean); } vector2d WorldModel::ball_posn_global() { return odoToGlobal(ball.mean); } Gaussian2 WorldModel::ball_estimate() { return Gaussian2(ball.time, odoToLocal(ball.mean), ball.sMaj, ball.sMin, ball.thetaMaj - self_heading.angle(), false); } Gaussian2 WorldModel::ball_estimate_global() { return Gaussian2(ball.time, odoToGlobal(ball.mean), ball.sMaj, ball.sMin, ball.thetaMaj - odo_heading.angle(), false); } //=========================================================================== // How many hypotheses are there? int WorldModel::ball_tracker_hypothesis(bool use_old_ball_estimate) { if (use_old_ball_estimate) { return 1; } else { return ball_tracker.numHypotheses(); } } // What is the ball tracker's best ball estimate bool WorldModel::ball_tracker_observation(Gaussian2 & pos, int hypothesis, bool use_old_ball_estimate) { if (use_old_ball_estimate) { // pos = Gaussian2(ball.time, odoToLocal(ball.mean), ball.sMaj, ball.sMin, // ball.thetaMaj - self_heading.angle(), false); pos = ball_estimate(); return true; }else{ // Get the position estimate now (dt=0) Gaussian2 temp; if (ball_tracker.getPosition(temp,hypothesis)) { pos = Gaussian2(temp.time, odoToLocal(temp.mean), temp.sMaj, temp.sMin, temp.thetaMaj - self_heading.angle(), false); return true; }else{ return false; } } } // What is the ball tracker's best ball estimate bool WorldModel::ball_tracker_observation_global(Gaussian2 & pos, int hypothesis, bool use_old_ball_estimate) { // Get the position estimate now (dt=0) if (use_old_ball_estimate) { // pos = Gaussian2(ball.time, odoToGlobal(ball.mean), ball.sMaj, // ball.sMin, ball.thetaMaj - odo_heading.angle(), false); pos = ball_estimate_global(); return true; }else{ Gaussian2 temp; if (ball_tracker.getPosition(temp,hypothesis)) { pos = Gaussian2(temp.time, odoToGlobal(temp.mean), temp.sMaj, temp.sMin, temp.thetaMaj - odo_heading.angle(), false); return true; } else { return false; } } } bool WorldModel::ball_tracker_observation_from_ID(Gaussian2 & pos, ulong ID, bool use_old_ball_estimate) { int hypothesis; if (!get_ball_tracker_hypothesis_from_ID(hypothesis,ID,use_old_ball_estimate)) { return false; } return ball_tracker_observation(pos,hypothesis,use_old_ball_estimate); } bool WorldModel::ball_tracker_observation_global_from_ID(Gaussian2 & pos, ulong ID, bool use_old_ball_estimate) { int hypothesis; if (!get_ball_tracker_hypothesis_from_ID(hypothesis,ID,use_old_ball_estimate)) { return false; } return ball_tracker_observation_global(pos,hypothesis,use_old_ball_estimate); } bool WorldModel::get_ball_tracker_hypothesis_from_ID(int & hypothesis, ulong ID, bool use_old_ball_estimate) { if (use_old_ball_estimate) { hypothesis = 0; return true; } else { return ball_tracker.getHypothesisFromID(hypothesis,ID); } } bool WorldModel::get_ball_tracker_ID_from_hypothesis(ulong & ID, int hypothesis, bool use_old_ball_estimate){ if (use_old_ball_estimate) { ID = 0; return true; } else { return ball_tracker.getIDFromHypothesis(ID,hypothesis); } } // Return the index of the closest ball to us bool WorldModel::ball_tracker_closest(int & idx, bool use_old_ball_estimate) { if (use_old_ball_estimate) { idx = 0; return true; } else { Gaussian2 ball_pos; if (!ball_tracker_observation(ball_pos,0)) { return false; } int min_idx = 0; double min_dist = GVector::distance(ball_pos.mean,odo_position); for (int i=1;i (halfLength + goalDepth)) return false; if (pos.y < -halfWidth || pos.y > halfWidth) return false; return true; } /* Adds information about the ball's location to the ball history in a circular buffer fashion. */ void WorldModel::ballHistAdd(bool see_ball, double loc_x, double loc_y, ulong time, double confidence) { hist_cnt = (hist_cnt + 1) % BALL_HIST_LEN; // Generate a point in our odometry only coordinate frame // from the local coordinates vector2d odo_point = localToOdo(vector2d(loc_x, loc_y)); ballHistory[hist_cnt].odo_frame = odo_point; ballHistory[hist_cnt].time = time; ballHistory[hist_cnt].saw_ball = see_ball; ballHistory[hist_cnt].confidence = confidence; } int WorldModel::validBallsInHist() { int retval = 0; for(int i=0; i WorldModel::getBallIntercept(int min_hist_len) { BallTrjInfo trj = getBallTrajectory(min_hist_len); std::pair retval; // CPM FIXME: inefficient. actually solve the equation for (double time = 0.0; time < 5.0; time += 0.1) { vector2d estimate(time * trj.velocity.x + trj.intercept.x, time * trj.velocity.y + trj.intercept.y); estimate = estimate - odo_position; estimate = estimate.rotate(-odo_heading.angle()); if (estimate.x < 0) { retval.first = estimate; retval.second = time; return retval; } } retval.first = vector2d(0.0, 0.0); retval.second = -1.0; return retval; } double gammq(double a, double x) { return 0.0; // CPM FIXME implement } /* Original function from Numerical Recipes in C, sec. 15.2. Unfortunately, the license of Numerical Recipes prevents us from including that code in our source release, so you'll either have to type it in yourself or supply your own linear regression algorithm. */ void fit(double x[], double y[], int ndata, double sigma[], int mwt, double *a, double *b, double *sigmaa, double *sigmab, double *chi2, double *q) { // Start solving *b = 0.0; *a = y[0]; // Okay, the rest is just to test how good the fit is... *chi2 = 0.0; *q = 1.0; *sigmaa = 0.0; *sigmab = 0.0; *q = 0.0; } BallTrjInfo WorldModel::getBallTrajectory(int min_hist_len, int max_hist_len) { BallTrjInfo trj; trj.approach_vel = 0; trj.perp_vel = 0; trj.perp_offset = 0; trj.velocity.set(0.0, 0.0); trj.intercept.set(0.0, 0.0); // We're doing a least squares fit twice. We need // x(t) = m*t + b and // y(t) = m'*t + b' // We'll store (m, m') in trj.velocity and (b, b') in trj.intercept. // Accumulators for solving least squares. Stats for people // who hate stats has the formula. The derivation is easy too. double sum_t = 0; double sum_t_sq = 0; double sum_x = 0; double sum_x_sq = 0; double sum_xt = 0; double sum_y = 0; double sum_y_sq = 0; double sum_yt = 0; double num_added = 0; int num_added_int = 0; double dx_dt, offset_x; double dy_dt, offset_y; // We subtract the time of the most recent model update and // use that as "0 = now". And we convert to seconds as we go. // CPM: or use weighted regression // CPM: Jim suggests robust regression (see ML slides) // FIXME: dumb var names double tary[BALL_HIST_LEN]; // CPM double xary[BALL_HIST_LEN]; // CPM double yary[BALL_HIST_LEN]; // CPM double sigma[BALL_HIST_LEN]; // CPM double xa, xb, ya, yb, sigmaa, sigmab, chi2x, chi2y, q; // CPM for(int j = 0; j < max_hist_len; j++) { // most recent entry is in ballHistory[hist_cnt]. Consider them in // reverse order. int i = hist_cnt - j; if (i < 0) { i += BALL_HIST_LEN; } if(ballHistory[i].saw_ball) { // Be careful to use signed longs in the calculation because // negative results are possible. double t = ((double)(((long)ballHistory[i].time) - ((long)lastUpdateModelTime))) / 1000000.0; vector2d newpoint; newpoint.set(ballHistory[i].odo_frame.x, ballHistory[i].odo_frame.y); tary[num_added_int] = t; xary[num_added_int] = newpoint.x; yary[num_added_int] = newpoint.y; sigma[num_added_int] = sqrt(1.0 / ballHistory[i].confidence); sum_t += t; sum_t_sq += t*t; double x = ballHistory[i].odo_frame.x; sum_x += x; sum_x_sq += x*x; double y = ballHistory[i].odo_frame.y; sum_y += y; sum_y_sq += y*y; sum_xt += x*t; sum_yt += y*t; num_added++; num_added_int++; } } //pprintf(TextOutputStream, "Num available: %d\n", num_added_int); double best = 1e9; int besti = 0; double bestxa, bestxb, bestya, bestyb; bestxa = bestxb = bestya = bestyb = 0.0; for (int i = min_hist_len; i <= num_added_int; i++) { // FIXME CPM: maybe not every choice... fit(tary, xary, i, sigma, 1, &xa, &xb, &sigmaa, &sigmab, &chi2x, &q); // CPM fit(tary, yary, i, sigma, 1, &ya, &yb, &sigmaa, &sigmab, &chi2y, &q); // CPM double chi2normalized = (chi2x + chi2y) / i; if (chi2normalized < best) { vector2d velocity; velocity.set(xb, yb); if (velocity.length() < RIDICULOUS_SPEED) { best = chi2normalized; besti = i; bestxa = xa; bestxb = xb; bestya = ya; bestyb = yb; } } } // Only do the regression if we've seen ball for min_hist_len of the // last BALL_HIST_LEN frames. Otherwise we assume the ball isn't // moving. (FIXME: this may not be a good assumption) if(num_added >= min_hist_len) { //pprintf(TextOutputStream, "best: i=%d chi2=%f: (%f,%f) (%f,%f)\n", besti, best, bestxb, bestyb, bestxa, bestya); dx_dt = (sum_xt - (sum_t*sum_x/num_added)) / (sum_t_sq - (sum_t*sum_t/num_added)); offset_x = (sum_x - dx_dt*sum_t)/num_added; dy_dt = (sum_yt - (sum_t*sum_y/num_added)) / (sum_t_sq - (sum_t*sum_t/num_added)); offset_y = (sum_y - dy_dt*sum_t)/num_added; trj.chi2 = best; trj.velocity.set(xb, yb); // CPM new trj.intercept.set(xa, ya); // CPM new //trj.velocity.set(dx_dt, dy_dt); // CPM original // trj.intercept is also our best estimate of the ball's // current position in odometry coordinates. //trj.intercept.set(offset_x, offset_y); // CPM original /* Calculate approach rate and other derived information */ double ball_distance = (odo_position - trj.intercept).length(); vector2d my_front = odo_heading * -1.0; trj.approach_vel = my_front.dot(trj.velocity); // calc perp_vel vector2d perp_to_me = my_front; perp_to_me.set(perp_to_me.y, -perp_to_me.x); trj.perp_vel = perp_to_me.dot(trj.velocity); // calc perp_offset (y-offset at which ball rolls past us) if(trj.approach_vel > 0) { trj.perp_offset = trj.perp_vel*ball_distance/trj.approach_vel; } else { trj.perp_offset = 0; } // We need to calculate the mean squared error of our prediction: trj.sq_error = 0.0; for(int i=0; igetFrame(i)->IRDistChest; } mean_dist /= HistorySize; // the smaller squared difference is the more likely state // ball_on_chest_prob = (on_chest_sum < not_on_sum)? 1.0 : 0.0; // calculate mean measurements and evaluate as gaussian distributions double gauss_on = gaussian_prob(mean_dist - OnDistMean,StdDev); double gauss_off = gaussian_prob(mean_dist - OffDistMean,StdDev); ball_on_chest_prob = gauss_on / (gauss_on + gauss_off); } vector2d WorldModel::localToOdo(vector2d local_posn) { // Rotate the point to account for our rotation local_posn = local_posn.rotate(odo_heading.angle()); // Add our position in the odometry coordinate frame local_posn += odo_position; return local_posn; } vector2d WorldModel::odoToLocal(vector2d odo_posn) { // subtract off our offset odo_posn -= odo_position; // now undo the rotation for our heading odo_posn = odo_posn.rotate(-odo_heading.angle()); return odo_posn; } vector2d WorldModel::odoToGlobal(vector2d odo_posn) { vector2d local_posn = odoToLocal(odo_posn); local_posn = local_posn.rotate(self_heading.angle()); local_posn += self_position.mean; return local_posn; } vector2d WorldModel::globalToOdo(vector2d global_posn) { // let's do global to local first vector2d local_posn = global_posn - self_position.mean; local_posn = local_posn.rotate(-self_heading.angle()); return localToOdo(local_posn); } //------------------------------------------------------------------------- bool WorldModel::ball_mean(vector2d & pos, ulong dt) { Gaussian2 gpos; if (ball_observation(gpos,dt)) { pos=gpos.mean; return true; } else { return false; } } bool WorldModel::ball_observation(Gaussian2 & pos, ulong dt) { for (uint i=0;i= ball_obs[i].obs_time) { pos = ball_obs[i].position_local_frame; return true; } } return false; } // Provide interfaces for the red and blue robot observations that // are indexed by time bool WorldModel::red_robot_mean(vector2d & pos, ulong dt) { Gaussian2 gpos; if (red_robot_observation(gpos,dt)) { pos=gpos.mean; return true; } else { return false; } } bool WorldModel::blue_robot_mean(vector2d & pos, ulong dt) { Gaussian2 gpos; if (blue_robot_observation(gpos,dt)) { pos=gpos.mean; return true; } else { return false; } } bool WorldModel::red_robot_observation(Gaussian2 & pos, ulong dt) { for (uint i=0;i= red_robot_obs[i].obs_time) { // Get the covariance of the robot's position pos = red_robot_obs[i].my_position; // Get the mean of the observation pos.mean = red_robot_obs[i].robot_posn; return true; } } return false; } bool WorldModel::blue_robot_observation(Gaussian2 & pos, ulong dt) { for (uint i=0;i= blue_robot_obs[i].obs_time) { // Get the covariance of the robot's position pos = blue_robot_obs[i].my_position; // Get the mean of the observation pos.mean = blue_robot_obs[i].robot_posn; return true; } } return false; } //========================================================================== //========================================================================== //========================================================================== bool WorldModel::teammate_ball_mean(vector2d & pos, int teammate, ulong dt) { Gaussian2 gpos; if(teammate_ball_observation(gpos,teammate,dt)) { pos=gpos.mean; return true; } else { return false; } } bool WorldModel::teammate_ball_observation(Gaussian2 & pos, int teammate, ulong dt) { int idx=0; int cnt=0; map >::iterator it=teammate_msgs.begin(); while (it!=teammate_msgs.end()) { idx=(*it).first; if (teammate==cnt) break; cnt++; it++; } if (it==teammate_msgs.end()) return false; for (uint i=0;i= teammate_msgs[idx][i].timestamp) { if (1==teammate_msgs[idx][i].shared_info.valid_ball_posn) { pos = teammate_msgs[idx][i].shared_info.ball_position; return true; } } } return false; } //========================================================================== //========================================================================== //========================================================================== bool WorldModel::teammate_pos_mean(vector2d & pos, int teammate, ulong dt) { Gaussian2 gpos; if(teammate_pos_observation(gpos,teammate,dt)) { pos=gpos.mean; return true; } else { return false; } } bool WorldModel::teammate_pos_observation(Gaussian2 & pos, int teammate, ulong dt) { int idx=0; int cnt=0; map >::iterator it=teammate_msgs.begin(); while (it!=teammate_msgs.end()) { idx=(*it).first; if (teammate==cnt) break; cnt++; it++; } if (it==teammate_msgs.end()) return false; for (uint i=0;i= teammate_msgs[idx][i].timestamp) { pos = teammate_msgs[idx][i].shared_info.my_position; return true; } } return false; } //========================================================================== //========================================================================== //========================================================================== // Provide interfaces for the teammate red and blue robot observations // that are indexed by time bool WorldModel::teammate_red_robot_mean(vector2d & pos, int teammate, ulong dt) { Gaussian2 gpos; if(teammate_red_robot_observation(gpos,teammate,dt)) { pos=gpos.mean; return true; } else { return false; } } bool WorldModel::teammate_blue_robot_mean(vector2d & pos, int teammate, ulong dt) { Gaussian2 gpos; if(teammate_blue_robot_observation(gpos,teammate,dt)) { pos=gpos.mean; return true; } else { return false; } } bool WorldModel::teammate_red_robot_observation(Gaussian2 & pos, int teammate, ulong dt) { int idx=0; int cnt=0; map >::iterator it=teammate_msgs.begin(); while (it!=teammate_msgs.end()) { idx=(*it).first; if (teammate==cnt) break; cnt++; it++; } if (it==teammate_msgs.end()) return false; // Iterate through our stored teammate messages for (uint i=0;i= teammate_msgs[idx][i].robot_obs[j].obs_time) { if (TEAM_COLOR_RED == teammate_msgs[idx][i].robot_obs[j].robot_color) { // Get the covariance of the robot's position pos = teammate_msgs[idx][i].robot_obs[j].my_position; // Get the mean of the observation pos.mean = teammate_msgs[idx][i].robot_obs[j].robot_posn; return true; } else { // If the team color is NOT red, then what do we do? Do we // keep iterating through the time sequences? This won't be // very efficient... } } } } return false; } bool WorldModel::teammate_blue_robot_observation(Gaussian2 & pos, int teammate, ulong dt) { int idx=0; int cnt=0; map >::iterator it=teammate_msgs.begin(); while (it!=teammate_msgs.end()) { idx=(*it).first; if (teammate==cnt) break; cnt++; it++; } if (it==teammate_msgs.end()) return false; // Iterate through our stored teammate messages for (uint i=0;i= teammate_msgs[idx][i].robot_obs[j].obs_time) { if (TEAM_COLOR_BLUE == teammate_msgs[idx][i].robot_obs[j].robot_color) { // Get the covariance of the robot's position pos = teammate_msgs[idx][i].robot_obs[j].my_position; // Get the mean of the observation pos.mean = teammate_msgs[idx][i].robot_obs[j].robot_posn; return true; } else { // If the team color is NOT red, then what do we do? Do we // keep iterating through the time sequences? This won't be // very efficient... } } } } return false; } //========================================================================== // Returns true if pt is inside of the rectangle centered at // center and has width width_maj along the major axis and // width_min along the minor axis. (It doesn't actually matter // if width_min is larger or smaller than width_maj. We just // need to distinguish so we know how to rotate things) bool WorldModel::pointInRect(const vector2d pt, const vector2d rect_center, double width_maj, double width_min, double theta_maj) { vector2d major_axis, minor_axis; // Unit vector in direction of major axis major_axis.set(1.0, 0.0); major_axis = major_axis.rotate(theta_maj); // now get a unit vector along our minor axis. minor_axis = major_axis.rotate(M_PI/2.0); // Find our corners. I'm going to define "upper left" as if // the major axis were Y on a cartesian plane (i.e. in -x,+y for // upper left, +x+y for upper right, etc. vector2d upper_left = rect_center + major_axis*(width_maj/2.0) + minor_axis*(-width_min/2.0); vector2d upper_right = rect_center + major_axis*(width_maj/2.0) + minor_axis*(width_min/2.0); vector2d lower_left = rect_center + major_axis*(-width_maj/2.0) + minor_axis*(-width_min/2.0); vector2d lower_right = rect_center + major_axis*(-width_maj/2.0) + minor_axis*(width_min/2.0); // We are going to test if our point is inside of the rectangle // by seeing if lines from the point to each vertex intersect // the edges of the rectangle. The only way to avoid an intersection // is if the point is inside of the rect. // I think it's correct to assume that we only need to check two // edges for each pt->vertex segment. These two edges being the // edges that don't touch the vertex being connected. // check line to upper_left if(segment_intersect(pt, upper_left, lower_left, lower_right) || segment_intersect(pt, upper_left, lower_right, upper_right)) return false; // check line to upper_right if(segment_intersect(pt, upper_right, upper_left, lower_left) || segment_intersect(pt, upper_right, lower_left, lower_right)) return false; // check line to lower_left if(segment_intersect(pt, lower_left, upper_left, upper_right) || segment_intersect(pt, lower_left, upper_right, lower_right)) return false; // check line to lower_right if(segment_intersect(pt, lower_right, lower_left, upper_left) || segment_intersect(pt, lower_right, upper_left, upper_right)) return false; return true; } int WorldModel::robotObsInRect(int robot_color, bool care_about_color, vector2d center, double width_maj, double width_min, double theta_maj) { int num_inside = 0; // Look for robots that we've observed ourselves // Let's do red first if(robot_color==TEAM_COLOR_RED || !care_about_color) { for(unsigned int i=0; i >::iterator it; it = teammate_msgs.begin(); while(it!=teammate_msgs.end()) { deque &tm_deque = (*it).second; for(unsigned int i=0; i 1.0) retval = 1.0; return retval; } double WorldModel::teammateInRect(vector2d center, double width_maj, double width_min, double theta_maj) { int count; if(teamColor!=TEAM_COLOR_RED) { count = robotObsInRect(TEAM_COLOR_BLUE, true, center, width_maj, width_min, theta_maj); } else { count = robotObsInRect(TEAM_COLOR_RED, true, center, width_maj, width_min, theta_maj); } double retval = ((double)count)/5.0; if(retval > 1.0) retval = 1.0; return retval; } double WorldModel::robotInRect(vector2d center, double width_maj, double width_min, double theta_maj) { int count; count = robotObsInRect(TEAM_COLOR_BLUE, false, center, width_maj, width_min, theta_maj); double retval = ((double)count)/5.0; if(retval > 1.0) retval = 1.0; return retval; } //============================================================================ //============================================================================ //============================================================================ void WorldModel::sendWMDebug(float x, float y, uchar shape, uchar red,uchar green, uchar blue) { WMDebug wmd(x,y,shape,red,green,blue); wmdebug_deque.push_back(wmd); if (NUM_WMDEBUG < (int)wmdebug_deque.size()) { wmdebug_deque.pop_front(); } } int WorldModel::numWMDebug() { return wmdebug_deque.size(); } bool WorldModel::getWMDebug(WMDebug & wmd,int idx) { if ((idx < 0) || (idx >= (int)wmdebug_deque.size())) { return false; } wmd = wmdebug_deque[idx]; return true; } void WorldModel::clearWMDebug() { wmdebug_deque.clear(); }