Path Planning and Trajectory Troubles

anon88055647 · February 3, 2017, 6:03pm

We are having some difficulties getting our robot to follow a desired path, and we are running out of options as to what could be the issue. Looking for any insights.

We have a software path planner that takes an input robot path and outputs a series of points over time for the velocity and position of the left and right wheels. We feed these two motion profiles to the left and right wheels via the Talon SRX motion profile feature.

When we do a straight path, the robot executes that path repeatably and accurately (± 1 cm for a path 4 m long). When we do a curved path, however, the robot ends up in the wrong spot. The encoders show that the wheels are accurately going the distances sent in the motion profiles (within a couple hundredths of a revolution).

A particular case:
Starting Point of (0, 0, 0) (X, Y, Robot Heading)
Intended Ending Point of (2.82, 1.63, 56) (meters, meters, degrees)
The robot has a track width (wheelbase width) of 0.48 meters.

The path planner calculates a total distance of 3.245 m for the left wheel and 3.714 m for the right wheel.

Actual ending point of (3.05, 1.22, about 48) we don’t have IMU on this bot so heading measurement is a bit crude with protractor on floor.

All measurements are of the center point of the robot.

We have a very mild acceleration and there is no evidence of wheel slip. As well, we ran this over and over and the robot goes to the same spot within a cm or two every time.

We are thinking that the path calculator may be the culprit, but I am open to any other suggestions. If someone has a path planner and would be willing to enter the same starting and ending points as above and compare to our output, that would be greatly appreciated. I think the total ending distance of each wheel is all that is necessary, but I have attached our CSV file of points to this post as well.

AutoTestPlay.csv (6.05 KB)

AutoTestPlay.csv (6.05 KB)

gerthworm · February 3, 2017, 6:16pm

If you haven’t done so yet, plot your commanded left/right velocities against your actual left/right velocities. Because position error will accumulate over time, the control loop needs to be quite tight in keeping the actual velocity close to the real velocity.

We did something similar last year, and definitely had lots of issues with turning at first - the wheel scrub was higher than expected, and fairly inconsistent. What allowed us to start nailing the paths was adding a gyro with a simple P feedback to help keep the robot pointed along the right heading.

Other thing to try would be to slow down the path traversal - does that make your end point any more accurate?

Also, you did mention something about total distance of each wheel being all that is necessary - this isn’t 100% true - Think if you ran just the right side of the drivetrain for the desired distance, then just the left side of the drivetrain. (you basically stay in one spot). To maintain a certain heading, the relative speed of the sides of the drivetrain at all times does matter. For this reason, the path planner would need to calculate velocities at all times, and the control algorithm needs to get the sides of the drivetrain to those velocities.

Ether · February 3, 2017, 8:26pm

*You never mentioned what kind of drivetrain you are using.

anon88055647 · February 3, 2017, 9:48pm

Six wheel drive.

anon88055647 · February 3, 2017, 9:54pm

In the talon MP, there is no feedback control on the velocity, only feedforward. The feedback control is on the position at every timestep.

anon88055647 · February 4, 2017, 1:18pm

I see your point exactly. Attached is a snapshot of the position and velocity graphs. For this particular run, ending coordinates were (2.96, 1.31, 49) I put an IMU on there today, so that measured heading is more reliable. I doubt we can reliably tune to better execution than this

I haven’t yet redone the path with lower max velocity…that is next.

I would still very much appreciate someone generating a path that has the same starting and ending coordinates and posting it. We will execute that path and see what happens. I would like to be able to take the path planner away as the culprit if I can. That would allow us to focus on execution and understand the limitations.

Ether · February 4, 2017, 1:22pm

Is it dropped center?

Where is the center of mass?

anon88055647 · February 4, 2017, 2:01pm

Yes, drop center. I don’t know where the center of mass is.

Ether · February 4, 2017, 3:24pm

The location of center of mass affects how a skid steer robot rotates.

Here’s an exaggerated sketch.

Red has CoM over rear wheels. Blue has CoM over front wheels.

*

Ether · February 4, 2017, 3:39pm

So if your left wheel distance is less than the right wheel distance, you must be turning left, yes?

So your heading of “0” for the starting point means the robot is pointing along the positive X axis (not the positive Y axis), yes?

Jared_Russell · February 4, 2017, 3:45pm

I suspect that your robot’s actual kinematics and dynamics don’t match the model your trajectory generator assumes.

An ideal differential drive robot only has two fixed wheels (plus friction-free casters, skids, or omniwheels for stability) and turns in place about the center of the axis of rotation of the wheels. This type of robot can turn without ever slipping its driven wheels because the axle is always tangent to the direction of motion. You can determine the necessary wheel speed/displacement by multiplying angular speed/displacement by half your wheelbase.

However, once you have more than two fixed wheels on the floor, your kinematics for steering change (here’s some light reading on the topic). For example, a 4 wheel drive robot turns about a point somewhere between the four wheels (where exactly is a function of traction and weight distribution). This means that any turning motion necessarily results in some wheel slip. However, if you know where the center of rotation is, you can deal with the fact that your axles are no longer tangent to the direction of motion and command a velocity/displacement that compensates.

A 6 wheel drive robot is even more interesting. If you have no wheel drop, your drivetrain is now a statically indeterminate system. If you have wheel drop, your drivetrain may behave like an “ideal” 2 wheel drive robot most of the time, but may act more like a front- or rear- biased 4 wheel drive due to dynamics as you accelerate, decelerate, or corner.

So what can you do about this? There are a few options.

Ignore it - model your robot as an ideal diff drive robot and only drive straight segments and turns in place.
Assume the center of rotation is fixed - either analytically or experimentally (which is usually what I do) come up with a best guess of “mean” center of rotation and wheel slip. As long as you don’t do crazy fast maneuvers (to limit the effect of superstructure dynamics on your drivetrain) this is often good enough for FRC. In your case this would simply manifest as slightly tweaked trajectories that compensate for wheel slip and center of rotation.
Measure it and compensate for it. You just need a way to measure robot motion without trusting your drivetrain encoders. A gyro is the most common way to do this; you can make the assumption that the average of your left and right encoder velocities/displacements is correct (e.g. that slip due to rotation is evenly distributed left to right) but then use the gyro for heading. However, this would preclude you from being able to do the whole thing on the Talons with a precomputed trajectory (at least for this year…). Alternatively, you can use passively sprung omniwheels with encoders to measure “true” robot velocity without slippage, but this can get complex mechanically.

antman · February 4, 2017, 3:52pm

The attached contains trajectories generated using Pathfinder. You didn’t specify your max velocity or max acceleration so I used the values in the Pathfinder readme (1.7 for max velocity, 2.0 for max acceleration, Pathfinder also wants max jerk as an input, kept this at 60.0 like in the readme).

Here’s the Java code snippet for generating the trajectories:

Waypoint] points = new Waypoint] {
new Waypoint(0, 0, 0),
new Waypoint(2.82, 1.63, Pathfinder.d2r(56))};

Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.04, 1.7, 2.0, 60.0);
Trajectory trajectory = Pathfinder.generate(points, config);
			
double wheelbase_width = 0.48;
TankModifier modifier = new TankModifier(trajectory);
modifier.modify(wheelbase_width);
Trajectory left  = modifier.getLeftTrajectory();
Trajectory right = modifier.getRightTrajectory();

```<br><br><a class='attachment' href='/uploads/default/original/3X/1/9/1906e55ab63734237b66e3a220885090f59151bb.csv'>OutputFromPathfinder.csv</a> (3.26 KB)<br><br><br><a class='attachment' href='/uploads/default/original/3X/1/9/1906e55ab63734237b66e3a220885090f59151bb.csv'>OutputFromPathfinder.csv</a> (3.26 KB)<br>

Ether · February 4, 2017, 3:57pm

Is this a correct interpretation of what you’re trying to do?

*

anon88055647 · February 4, 2017, 6:11pm

Yes, if that endpoint is a 56 degree heading offset from the starting heading (straight ahead to the right).

I also see the point about the center of rotation uncertainty because of the weight distribution. Though I answered before about not knowing, I meant numerically. Qualitatively, this little practice bot we are using has it in the front (battery is there). So it will be resting on middle and front wheelsets primarily.

So Jared, I think this feeds into what you explained, very helpful. I’ll use what Ether explained to then pursue option #2 you described.

Finally, thank you a bunch antman for the profile. I’ll be running it late tonight or tomorrow afternoon.

Thanks to all for the inputs and explanation.

Ether · February 4, 2017, 7:47pm

It is now

I posted the wrong attachment. It has been replaced with the correct one.

To check this, take the first derivative of the equation shown in the graph, evaluate it at x=2.82, and take the arc tangent. The answer is 56 degrees.

*

Ether · February 4, 2017, 11:34pm

*Right and Left wheel traces.

*

CRL Table (values).xls (31 KB)

CRL Table (values).xls (31 KB)

anon88055647 · February 5, 2017, 3:00pm

Here is what we found:

Our path planner is fine. We generated a path with the same inputs as antman, and our numbers are very close.
We executed antman’s path file. Similarly to our file, the robot did not end up exactly where we wished.
We have lowered max velocity (as gerthworm suggested) and increased max jerk (from what we had) and are seeing improved accuracy. We are also getting less dispersion.
We are continuing to experiment with various values and tweaking based on better understanding of the center of rotation (per Jarod and Ether).

Thanks again to all.