Weird dips observed in observed robotVelocity

[jgkamat]

When plotting the robot velocity, I see these weird dips occuring. this is via the simulator, but this is much more pronounced on the field computer. This is probably messing with our motion control, so we should figure out why this is happening. It seems to happen fairly regular intervals, and happens even when we control the robot manualy (so I doubt it's path planning lag or something else).

On the field computer it jumps down to 0 (maybe not going fast enough?), but on my computer in the simulator it's visible, but much less exaggerated:

[jgkamat]

This seems to be related to a situation where we receive multiple vel packets in one processor frame. In this case, we do some weird stuff in RobotFilter.cpp. That should be a good place to track this down.

Ideally, we would handle it so that we have a smooth graph with little to no dips. The dips might be caused by the apparent 'lack of time' between the bunch of received packets.

[jgkamat]

Optimizing the path planner will also fix this (as will compiling with optimizations), but this still should be fixed since this situation can be caused by a bunch of cases.

[JNeiger]

It might be this. If obs->time isn't being sent right, the second prediction will be too close and be cut off.

Here is the caller of update.

[jgkamat]

Yes, albert said the same thing earlier.

It looks like (currently) we're setting the velocity to 0 when packets come in too fast (which seems very wrong). I'm not sure what the best thing to do would be though, I think we should take in all the packets, set the average velocity for the time we dropped out for, and keep going, but I'm not sure if that makes sense. We need to try out different things and see what works best.

[ashaw596]

If your debugging this, setting Velocity_Alpha to 1 makes it really obvious when its dropping. The velocity being set to 0 is not happening very much actually.

[jgkamat]

From what I can tell so far, this is caused by us getting two packets with the same exact position one after each other, causing velocity to drop to zero for a frame. The velocity_alpha is the reason we don't see a full drop all the time. The time seems to be correct (the frame time dosen't drop out).

The initial Min_Frame_Time thing seems to only run (for me) when the robots initialize, so I don't think thats the problem (on my machine) at this point.

For reference, this printout: std::cout << newVel << " " << (obs->pos - _estimate[s].pos) << " " << obs->pos << " " << _estimate[s].pos << std::endl; Gives me this occasionally:

Point(0.182486, -2.09184) Point(0.00292798, -0.0335636) Point(0.00598637, 1.88643) Point(0.00305839, 1.91999)
Point(1.67693, -1.2706) Point(0.0269064, -0.0203868) Point(0.203742, 2.58787) Point(0.176836, 2.60826)
Point(0.232059, -0.702863) Point(0.00372338, -0.0112774) Point(1.36248, 2.4019) Point(1.35876, 2.41318)
Point(0.0606903, -0.500597) Point(0.000973776, -0.00803208) Point(-0.188283, 1.93807) Point(-0.189257, 1.9461)
Point(1.48594e-05, -1.48594e-05) Point(2.38419e-07, -2.38419e-07) Point(1.42737, 1.94338) Point(1.42737, 1.94338)
Point(1.05432, -0.835943) Point(0.0169165, -0.0134127) Point(0.94585, 1.74999) Point(0.928934, 1.7634)
Point(0, 0) Point(0, 0) Point(-4.37114e-08, 5.5) Point(-4.37114e-08, 5.5)
Point(0, 0) Point(0, 0) Point(-1, 5.5) Point(-1, 5.5)
Point(0, 0) Point(0, 0) Point(1, 5.5) Point(1, 5.5)
Point(0, 0) Point(0, 0) Point(-1, 6.5) Point(-1, 6.5)
Point(0, 0) Point(0, 0) Point(1, 6.5) Point(1, 6.5)
Point(0, 0) Point(0, 0) Point(-8.74228e-08, 6.5) Point(-8.74228e-08, 6.5)
Point(0.131675, -2.15221) Point(0.002118, -0.0346184) Point(0.00810437, 1.85181) Point(0.00598637, 1.88643)
Point(1.69514, -1.28439) Point(0.0272663, -0.0206594) Point(0.231008, 2.56721) Point(0.203742, 2.58787)
Point(0.229896, -0.696313) Point(0.00369787, -0.0112002) Point(1.36618, 2.3907) Point(1.36248, 2.4019)
Point(0.0827573, -0.469337) Point(0.00133115, -0.00754929) Point(-0.186952, 1.93052) Point(-0.188283, 1.93807)
Point(0, 0) Point(0, 0) Point(1.42737, 1.94338) Point(1.42737, 1.94338)
Point(1.05279, -0.834754) Point(0.0169342, -0.013427) Point(0.962785, 1.73656) Point(0.94585, 1.74999)
Point(0, 0) Point(0, 0) Point(-4.37114e-08, 5.5) Point(-4.37114e-08, 5.5)
Point(0, 0) Point(0, 0) Point(-1, 5.5) Point(-1, 5.5)
Point(0, 0) Point(0, 0) Point(1, 5.5) Point(1, 5.5)
Point(0, 0) Point(0, 0) Point(-1, 6.5) Point(-1, 6.5)
Point(0, 0) Point(0, 0) Point(1, 6.5) Point(1, 6.5)
Point(0, 0) Point(0, 0) Point(-8.74228e-08, 6.5) Point(-8.74228e-08, 6.5)
Point(0, 0) Point(0, 0) Point(0.00810437, 1.85181) Point(0.00810437, 1.85181)
Point(0, 0) Point(0, 0) Point(0.231008, 2.56721) Point(0.231008, 2.56721)
Point(0, 0) Point(0, 0) Point(1.36618, 2.3907) Point(1.36618, 2.3907)
Point(0, 0) Point(0, 0) Point(-0.186952, 1.93052) Point(-0.186952, 1.93052)
Point(0, 0) Point(0, 0) Point(1.42737, 1.94338) Point(1.42737, 1.94338)
Point(0, 0) Point(0, 0) Point(0.962785, 1.73656) Point(0.962785, 1.73656)
Point(0, 0) Point(0, 0) Point(-4.37114e-08, 5.5) Point(-4.37114e-08, 5.5)
Point(0, 0) Point(0, 0) Point(-1, 5.5) Point(-1, 5.5)
Point(0, 0) Point(0, 0) Point(1, 5.5) Point(1, 5.5)
Point(0, 0) Point(0, 0) Point(-1, 6.5) Point(-1, 6.5)
Point(0, 0) Point(0, 0) Point(1, 6.5) Point(1, 6.5)

[jgkamat]

After more tests, I'm pretty sure we're getting duplicate packets from the vision system (at least for position). Not literally duplicate packets, but duplicating position. Time in seconds seems fine. On the simulator I don't see this, so this isn't a sim problem. I'll start looking at where we handle the incoming packets.

[jgkamat]

I've tracked this all the way up to visionreceiver, with this print statement:

cout << packet->wrapper.detection().robots_yellow()[2].y() << " " << packet->wrapper.detection().t_capture() << endl;

Next, I'm probably going to try and log the output of the viewer program and see if I can repro there. As before, the frame number increases, as does the recorded timestamp, but the position is not updated.

This might be a weird bug in our UDP receiving code.

[ashaw596]

The problem is actually in the simulator. The Vision and physics engine runs on two different threads. They both try to run at 60 fps about. Every now and the vision runs multiple times before the physics engine updates. Upping the physics engine to a higher fps ( by changing the sleep to 1000 instead of 16000 on line 107 of SimulatorGLUTThread) mostly fixes it. (But we really should make a better RobotFilter. This algorithm is horrible.)

[ashaw596]

There is still the problem with actual vision though. I'd guess its just that out RobotFilter is not very good and the skip/dropped packets are causing something.

[jgkamat]

We see this in ssl-vision as well (assuming the drop problem on the field computer is the same as this one), so we have to do something to improve RobotFilter anyway, RobotFilter should be smart enough to realize that some packets get dropped.

I'm curious to whether this happens in grSim as well.

[jgkamat]

Another interesting thing to note is that the ball dosen't have this behaivor, maybe we can take the filtering the ball uses and apply it to robots as well (I think it's a kalman filter).

[ashaw596]

Hmm. The Ball Filter isn't much better. Not sure why it wouldn't be an issue for the ball. We do still have this implementation in our repo for some reason. https://github.com/RoboJackets/robocup-software/tree/master/old/modeling-old/kalman

[joshhting]

Currently looking into it. This behavior is even more pronounced in grSim:

[jgkamat]

@joshhting from previous discussion, we found the root of the problem.

The simulator and ssl vision occasionally sends two distinct frames with the exact same position. Ideally we would use a kalman filter to correct for this.

I don't think that grSim has a bigger problem, we just zoomed the graph out by default so it looks like it's worse. It was this bad before, I think 😄.

Right now we use a 'Velocity_Alpha' to correct for this, which you can try to tweak. AFAIK, this 'Velocity_Alpha' is the hacky solution, and I don't think there's a less hacky, solution that's not a kalman filter. Let me know if you get stuck though, I /think/. I understand this problem pretty clearly 😄

[joshhting]

For the two frame with the same position, is position the only thing that remains constant between them? They have different timestamps and everything else?

[jgkamat]

@joshhting as far as I remember, yes, they had different timestamps, ID's, etc.

Maybe we could do something like

(if velocity-greaterthan-x and last-two-frames-equal and last-four-frames-not-equal
keep previous velocity)

although that seems pretty hacky, it would solve this one case (which is a pretty big problem).

[joshhting]

What is the purpose of the min_frame_time part of https://github.com/RoboJackets/robocup-software/blob/master/soccer/modeling/RobotFilter.cpp#L25 ? I deleted that part so the line just read if (reset) { and I got a MUCH smoother velocity observation:

[joshhting]

Also note that in this picture, there is a very consistent behavior with repeatedlineup where when the robots are moving from left to right, it's fine, but when it moves from right to left, there is still one velocity dip that occurs at roughly the same place every single time. Not ideal, but much less disruptive to motion control than what we had before.

[jgkamat]

Yah, I noticed that before. That's probably a different bug, but this (if it dosen't affect anything else) is much better than before :smile:

[joshhting]

Yeah I'm still worried that removing that line of code will break things with real robots and vision

[ashaw596]

So I took a longer look at this on the plane and I really didn't like how we were doing it so I tried rewriting it. I think it works alot better, but I've only been running on sim. #998

[jgkamat]

This was mostly fixed at comp, through albert's PR.