Go to Post it was beautifull just beautifull... then i took my earmuffs off. - Tytus Gerrish [more]
Home
Go Back   Chief Delphi > Technical > Programming
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Reply
Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 03-02-2017, 18:03
sraque's Avatar
sraque sraque is offline
Registered User
AKA: Steve Raque
FRC #2614 (MARS)
Team Role: Mentor
 
Join Date: Jun 2012
Rookie Year: 2009
Location: Morgantown, WV
Posts: 93
sraque is on a distinguished road
Path Planning and Trajectory Troubles

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.
Attached Files
File Type: csv AutoTestPlay.csv (6.0 KB, 39 views)
Reply With Quote
  #2   Spotlight this post!  
Unread 03-02-2017, 18:16
gerthworm's Avatar
gerthworm gerthworm is offline
Making the 1's and 0's
FRC #1736 (Robot Casserole)
Team Role: Mentor
 
Join Date: Jan 2015
Rookie Year: 2015
Location: Peoria, IL
Posts: 80
gerthworm has a spectacular aura aboutgerthworm has a spectacular aura about
Re: Path Planning and Trajectory Troubles

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.

Last edited by gerthworm : 03-02-2017 at 18:20. Reason: more explanation
Reply With Quote
  #3   Spotlight this post!  
Unread 03-02-2017, 20:26
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,141
Ether has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond repute
Re: Path Planning and Trajectory Troubles


You never mentioned what kind of drivetrain you are using.


Reply With Quote
  #4   Spotlight this post!  
Unread 03-02-2017, 21:48
sraque's Avatar
sraque sraque is offline
Registered User
AKA: Steve Raque
FRC #2614 (MARS)
Team Role: Mentor
 
Join Date: Jun 2012
Rookie Year: 2009
Location: Morgantown, WV
Posts: 93
sraque is on a distinguished road
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by Ether View Post

You never mentioned what kind of drivetrain you are using.


Six wheel drive.
Reply With Quote
  #5   Spotlight this post!  
Unread 03-02-2017, 21:54
sraque's Avatar
sraque sraque is offline
Registered User
AKA: Steve Raque
FRC #2614 (MARS)
Team Role: Mentor
 
Join Date: Jun 2012
Rookie Year: 2009
Location: Morgantown, WV
Posts: 93
sraque is on a distinguished road
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by gerthworm View Post
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.
In the talon MP, there is no feedback control on the velocity, only feedforward. The feedback control is on the position at every timestep.
Reply With Quote
  #6   Spotlight this post!  
Unread 04-02-2017, 13:18
sraque's Avatar
sraque sraque is offline
Registered User
AKA: Steve Raque
FRC #2614 (MARS)
Team Role: Mentor
 
Join Date: Jun 2012
Rookie Year: 2009
Location: Morgantown, WV
Posts: 93
sraque is on a distinguished road
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by gerthworm View Post
Also, you did mention something about total distance of each wheel being all that is necessary - this isn't 100% true - ...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.
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.
Attached Thumbnails
Click image for larger version

Name:	path capture.PNG
Views:	45
Size:	45.9 KB
ID:	21692  
Reply With Quote
  #7   Spotlight this post!  
Unread 04-02-2017, 13:22
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,141
Ether has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond repute
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by sraque View Post
Six wheel drive.
Is it dropped center?

Where is the center of mass?


Reply With Quote
  #8   Spotlight this post!  
Unread 04-02-2017, 14:01
sraque's Avatar
sraque sraque is offline
Registered User
AKA: Steve Raque
FRC #2614 (MARS)
Team Role: Mentor
 
Join Date: Jun 2012
Rookie Year: 2009
Location: Morgantown, WV
Posts: 93
sraque is on a distinguished road
Re: Path Planning and Trajectory Troubles

Yes, drop center. I don't know where the center of mass is.
Reply With Quote
  #9   Spotlight this post!  
Unread 04-02-2017, 15:24
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,141
Ether has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond repute
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by sraque View Post
I don't know where the center of mass is.
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.


Attached Thumbnails
Click image for larger version

Name:	CoM affects rotation.png
Views:	65
Size:	11.8 KB
ID:	21693  
Reply With Quote
  #10   Spotlight this post!  
Unread 04-02-2017, 15:39
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,141
Ether has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond repute
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by sraque View Post
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.
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?


Reply With Quote
  #11   Spotlight this post!  
Unread 04-02-2017, 15:45
Jared Russell's Avatar
Jared Russell Jared Russell is offline
Taking a year (mostly) off
FRC #0254 (The Cheesy Poofs), FRC #0341 (Miss Daisy)
Team Role: Engineer
 
Join Date: Nov 2002
Rookie Year: 2001
Location: San Francisco, CA
Posts: 3,082
Jared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond reputeJared Russell has a reputation beyond repute
Re: Path Planning and Trajectory Troubles

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.

1) Ignore it - model your robot as an ideal diff drive robot and only drive straight segments and turns in place.

2) 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.

3) 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.
Reply With Quote
  #12   Spotlight this post!  
Unread 04-02-2017, 15:52
antman antman is offline
Registered User
FRC #3238
Team Role: Mentor
 
Join Date: Mar 2016
Rookie Year: 2016
Location: WA
Posts: 13
antman is an unknown quantity at this point
Re: Path Planning and Trajectory Troubles

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:

Code:
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();
Attached Files
File Type: csv OutputFromPathfinder.csv (3.3 KB, 10 views)
Reply With Quote
  #13   Spotlight this post!  
Unread 04-02-2017, 15:57
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,141
Ether has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond repute
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by sraque View Post
Starting Point of (0, 0, 0) (X, Y, Robot Heading)
Intended Ending Point of (2.82, 1.63, 56) (meters, meters, degrees)
Is this a correct interpretation of what you're trying to do?


Attached Thumbnails
Click image for larger version

Name:	Cubic Centerline.png
Views:	48
Size:	11.1 KB
ID:	21704  

Last edited by Ether : 04-02-2017 at 19:25. Reason: corrected graph
Reply With Quote
  #14   Spotlight this post!  
Unread 04-02-2017, 18:11
sraque's Avatar
sraque sraque is offline
Registered User
AKA: Steve Raque
FRC #2614 (MARS)
Team Role: Mentor
 
Join Date: Jun 2012
Rookie Year: 2009
Location: Morgantown, WV
Posts: 93
sraque is on a distinguished road
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by Ether View Post
Is this a correct interpretation of what you're trying to do?


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.
Reply With Quote
  #15   Spotlight this post!  
Unread 04-02-2017, 19:47
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,141
Ether has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond reputeEther has a reputation beyond repute
Re: Path Planning and Trajectory Troubles

Quote:
Originally Posted by sraque View Post
Yes, if that endpoint is a 56 degree heading offset from the starting heading (straight ahead to the right).
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.


Attached Thumbnails
Click image for larger version

Name:	check_angle.png
Views:	40
Size:	4.7 KB
ID:	21706  
Reply With Quote
Reply


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump


All times are GMT -5. The time now is 18:29.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi