OCCRA
Go to Post Silly FIRST mentor -- actually spending brain power caring about a sporting event occuring during build season! - Aidan F. Browne [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

 
Closed Thread
Thread Tools Rate Thread Display Modes
  #16   Spotlight this post!  
Unread 03-31-2012, 12:13 AM
theNerd's Avatar
theNerd theNerd is offline
Registered User
FRC #3329 (Cam Bots)
Team Role: Programmer
 
Join Date: Jan 2011
Rookie Year: 2110
Location: St. Marys
Posts: 51
theNerd is an unknown quantity at this point
Re: Curve Driving Generator

Quote:
Originally Posted by theNerd View Post
Lets not discuss why we would or would not do this but instead, focus on how. Thanks!
Please re-read my original message
  #17   Spotlight this post!  
Unread 03-31-2012, 12:17 AM
theNerd's Avatar
theNerd theNerd is offline
Registered User
FRC #3329 (Cam Bots)
Team Role: Programmer
 
Join Date: Jan 2011
Rookie Year: 2110
Location: St. Marys
Posts: 51
theNerd is an unknown quantity at this point
Re: Curve Driving Generator

Quote:
Originally Posted by Ether View Post
You don't have to use matrix math if you don't want to. You can use plain ole algebra.

Attached PDF has the cookbook formulas for computing a, b, c, d.

a=-(-2*y2+2*y1+(m2+m1)*x2+(-m2-m1)*x1)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

b=(-3*x2*y2+x1*((m2-m1)*x2-3*y2)+(3*x2+3*x1)*y1+(m2+2*m1)*x2^2+(-2*m2-m1)*x1^2)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

c=-(x1*((2*m2+m1)*x2^2-6*x2*y2)+6*x1*x2*y1+m1*x2^3+(-m2-2*m1)*x1^2*x2-m2*x1^3)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

d=(x1^2*((m2-m1)*x2^2-3*x2*y2)+x1^3*(y2-m2*x2)+(3*x1*x2^2-x2^3)*y1+m1*x1*x2^3)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);


Quote:
Originally Posted by Ether View Post
Pick a coordinate system whose origin is the starting point and whose positive X axis aligns with the starting direction of motion.

Compute the ending point (X2,Y2) and slope m2 in that coordinate system.

Then the cubic becomes simply Y = aX3 + bX2

and the formulas for a and b are simply

a = (m2X2 - 2Y2)/X23

b = (3Y2 - m2X2)/X22
As soon as I looked at the algebra method I wanted to run away from the problem. But I'm assuming that making the initial robot position always equal to the x axis simplified the problem? I'm sorry if its tedious but, could you show me how you got that formula for "Y = " ? thanks alot for your help!
  #18   Spotlight this post!  
Unread 03-31-2012, 12:56 AM
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,724
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: Curve Driving Generator

Quote:
Originally Posted by theNerd View Post
I'm sorry if its tedious but, could you show me how you got that formula for "Y = " ? thanks alot for your help!
If you set x1=0, y1=0, and m1=0 in these formulas:

a=-(-2*y2+2*y1+(m2+m1)*x2+(-m2-m1)*x1)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

b=(-3*x2*y2+x1*((m2-m1)*x2-3*y2)+(3*x2+3*x1)*y1+(m2+2*m1)*x2^2+(-2*m2-m1)*x1^2)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

c=-(x1*((2*m2+m1)*x2^2-6*x2*y2)+6*x1*x2*y1+m1*x2^3+(-m2-2*m1)*x1^2*x2-m2*x1^3)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);

d=(x1^2*((m2-m1)*x2^2-3*x2*y2)+x1^3*(y2-m2*x2)+(3*x1*x2^2-x2^3)*y1+m1*x1*x2^3)/(-x2^3+3*x1*x2^2-3*x1^2*x2+x1^3);


... then you get:

a = (m2X2 - 2Y2)/X23

b = (3Y2 - m2X2)/X22

c = 0

d = 0

... and since c=0 and d=0, the cubic Y = aX3 + bX2 + cX + d is reduced to:

Y = aX3 + bX2


Quote:
I'm assuming that making the initial robot position always equal to the x axis simplified the problem?
Picking a coordinate system such that initial robot heading is aligned with +X axis (m1=0) and initial position is at origin (x1=0 and y1=0) simplified the problem.


Last edited by Ether : 03-31-2012 at 01:08 AM.
  #19   Spotlight this post!  
Unread 03-31-2012, 12:29 PM
theNerd's Avatar
theNerd theNerd is offline
Registered User
FRC #3329 (Cam Bots)
Team Role: Programmer
 
Join Date: Jan 2011
Rookie Year: 2110
Location: St. Marys
Posts: 51
theNerd is an unknown quantity at this point
Re: Curve Driving Generator

The polynomial idea works..... theoretically... However, from experience does this solution fit if we used 2nd degree polynomials instead of cubic? Also, for this type of problem is it easier to do matrix math instead of algebra?
  #20   Spotlight this post!  
Unread 03-31-2012, 12:49 PM
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,724
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: Curve Driving Generator

Quote:
Originally Posted by theNerd View Post
does this solution fit if we used 2nd degree polynomials instead of cubic?
You cannot fit a 2nd degree polynomial to 2 arbitrary points in the XY plane if the tangents at both points are specified.

A 2nd degree polynomial has only 3 adjustable parameters, and there are 4 equations to be satisfied:

Y1 = f(X1)

Y2 = f(X2)

Tangent1 = f'(X1)

Tangent2 = f'(X2)


  #21   Spotlight this post!  
Unread 03-31-2012, 08:37 PM
dcherba dcherba is offline
Registered User
FRC #3234 (Red Arrow Robotics)
Team Role: Programmer
 
Join Date: Dec 2009
Rookie Year: 2000
Location: ada, mi
Posts: 32
dcherba has a spectacular aura aboutdcherba has a spectacular aura aboutdcherba has a spectacular aura about
Re: Curve Driving Generator

The real problem you have is one of reference frame. The curve it self is not important as it represents some path on the playing field frame of reference. The incremental control of the robot is based on its frame of reference to the center of the robot for example.
This is a common problem for aircraft, satellites and just about any navigation problem.
The solution is to use a series of translations and rotations. The best way to do that is with quaternions as they do the rotation without ambiquity.

http://en.wikipedia.org/wiki/Quatern...atial_rotation
has a good primer on the subject. This is commonly used for computer graphics and simulations.

Have fun
__________________
Dave Cherba
Mentor Team 3234
WZ8T
  #22   Spotlight this post!  
Unread 03-31-2012, 10:09 PM
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,724
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: Curve Driving Generator


This is not a 3D problem. It's a problem in a 2D flat plane, and a skid-steer vehicle has only 2 degrees of freedom (rotation and fwd/reverse translation) in that plane.

To execute a smooth* path from an arbitrary point A (starting with heading angle Alpha) to point B (ending with heading angle Beta) the vehicle's instantaneous translation and/or rotation rates must continuously change over time in order to follow that path and arrive at the destination with the proper heading.

So the problem is:

1) given the starting and ending points and headings, what criteria are to be used to define the desired path? smoothest* curve? Shortest distance* ? Shortest time? radius of curvature* never less than some specified minimum value?

2) how to compute the curve defined by those specifications

3) how to use the curve to determine the required instantaneous vehicle rotation and translation at any point along the curve

4) how to use the answer to #3 to determine the left and right wheel speeds (for skid steer) at each point along the curve

5) is the path to be computed once (at the start) and then executed to completion without feedback, or is the path periodically re-computed to minimize accumulated drift errors

For a subset of the possible starting and ending points and headings, it is possible, as explained in earlier posts in this thread, to compute a cubic polynomial which defines a smooth path. For a given vehicle forward speed, this polynomial can be used to determine the required instantaneous vehicle rotation rate at each point along the path.

In another earlier post, dbeckwith suggested a Bezier curve. I believe the instantaneous vehicle rotation rate for a given vehicle forward speed can also be computed from the parametric Bezier equations, but I haven't actually done it yet.

The bottom line here is that the OP stated a problem without much context and asked that the discussion be limited to "how" and not "why". It makes for an interesting academic discussion, but I suspect that if we had greater insight into what was trying to be accomplished the solution might be different.



*notes:

smooth path: perhaps for esthetic reasons? the OP did not specify why

shortest distance: rotate-in-place at point A, drive straight to point B, rotate-in-place. probably not what the OP would consider a "smooth" curve

limited minimum radius of curvature: many skid-steer vehicles don't like a small turning radius. they hop and jerk.



Last edited by Ether : 03-31-2012 at 10:11 PM.
  #23   Spotlight this post!  
Unread 04-01-2012, 02:11 PM
theNerd's Avatar
theNerd theNerd is offline
Registered User
FRC #3329 (Cam Bots)
Team Role: Programmer
 
Join Date: Jan 2011
Rookie Year: 2110
Location: St. Marys
Posts: 51
theNerd is an unknown quantity at this point
Re: Curve Driving Generator

Quote:
Originally Posted by Ether View Post
[smooth path: perhaps for esthetic reasons? the OP did not specify why
Sorry for the confusion guys,

The reason for this problem is in aiding the drive in autonomous and teleop - more so autonomous. Also, this is just an exercise for the future programmers of our team. I had noticed that although the rotate in place is the "shortest distance" method it is not always the fastest - especially for a very smooth control - as that the robot must slow down, rotate, and then speed back up. While if the robot drove in a curve it would never need to stop, only change wheel speeds. So then, I'm looking for the fastest route possible. (The reason I did not want to discuss why is because most times I post a thread there always seems to be a discussion as to why not or why and completely turn away from the answer I wanted. I apologize for not being more specific).

I am planning on integrating this with distance PID loops to control the drive. Essentially this is an add-on to a drive system I am creating that pulls from a list of coordinates that I want to go to, sets the left and right wheel distances that the robot must travel to get to that point, and then feeds those distances into a PID loop. Once the error in the pid loop reaches around 0-5% it then pulls the next coordinate to go to. I also, figured that a coordinate system relative to the robots current position would be more beneficial than a coordinate system that is fixed for what I am trying to do.

The equations that the robot uses to determine left and right distances use the arc length of the path, and the angle through which the robot will turn while it is driving along that path. If the angle is 0 then it drives in a straight line, if the distance is 0 then it rotates in place. The equations are thus:

double rightDistance = ((wheelWidth*angle) + (2*distance))/2;
double leftDistance = ((2*distance) - (wheelWidth*angle))/2;

The advantage of this is that I only need one set of equations to drive curves, or perform the traditional rotate and drive.

I am also planning on working with trapezoidal motion profiling in this. I am using these equations:

x = x0 +dx(P(u));
P(u) = -2u^3+3u^2;
u = currentTime - startTime/delta_time;

Where x is the distance travelled in 1 dimension and dx is delta_x and x0 is the startingX --> which is normally 0. P(u) is a function with these properties: f(0)=0, f'(0) =0; f(1)=1, f'(1)=0 and P(u)=[0,1] and u=[0,1].
There is a second set of equations I plan on replacing the above with that include max acceleration and max velocity, I just haven't yet because I'm not sure how to go about testing for those due to a very very small workspace - and running the robot on blocks is inaccurate for our heavy bot.

Last edited by theNerd : 04-01-2012 at 02:49 PM.
  #24   Spotlight this post!  
Unread 04-01-2012, 06:22 PM
MichaelBick MichaelBick is offline
Registered User
FRC #1836 (MilkenKnights)
Team Role: Alumni
 
Join Date: Oct 2011
Rookie Year: 2010
Location: Los Angeles
Posts: 756
MichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant futureMichaelBick has a brilliant future
Re: Curve Driving Generator

254 has something like this build into their drive code, though as I said before, I believe it also integrates the current speed of the robot(faster speed, bigger arc, slower speed, smaller arc). I would check their code, as they have made it public, and it is very refined.
__________________
Team 1836 - The Milken Knights
2013 LA Regional Champions with 1717 and 973
2012 LA Regional Finalists with 294 and 973
To follow Team 1836 on Facebook, go to http://www.facebook.com/MilkenKnights
To go to our website, go to http://milkenknights.com/index.html
  #25   Spotlight this post!  
Unread 04-01-2012, 07:12 PM
Tom Line's Avatar
Tom Line Tom Line is offline
Danger: Raptor Crossing
FRC #1718 (The Fighting Pi)
Team Role: Mentor
 
Join Date: Jan 2007
Rookie Year: 1999
Location: Armada, Michigan
Posts: 2,825
Tom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond reputeTom Line has a reputation beyond repute
Re: Curve Driving Generator

Is this for teleop or autonmous?

In autonmous, getting a robot to drive an accurate curve is fairly early.

Have a PID loop for distance connected to each side of your drive train, and another PID loop based on a desired angle and the gyro that is added into to your final drivetrain output values. The robot will then drive curves to reach the endpoints, assuming gains are set correctly.
  #26   Spotlight this post!  
Unread 04-02-2012, 12:59 AM
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,724
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: Curve Driving Generator


Food for thought

See attached graph.

There's a Segway with a 2-foot wheelbase sitting at the origin.

Its heading is aligned with the +X axis.

I drive the Segway so that the center of its axle follows the black Bezier curve.

At the end of the path, the Segway's heading is parallel to the X axis and its coordinates are x=10 y=3.

The red curve shows the track of its left wheel, and the green curve shows the track of its right wheel.

The red, black, and green curves all have the SAME LENGTH (~10.6807 feet), but they are three DIFFERENT SHAPES.

If I were to command the left and right sides of a skid-steer vehicle to each travel exactly 10.6807 feet, the chances that the vehicle would follow the black curve are pretty slim. In fact, if the left and right sides were controlled identically and perfectly, it would travel in a straight line and end up on the X axis with coordinates x=10 y=0.


Attached Thumbnails
Click image for larger version

Name:	tracksGraph.png
Views:	173
Size:	8.1 KB
ID:	12505  Click image for larger version

Name:	shapes.png
Views:	154
Size:	7.3 KB
ID:	12507  
Attached Files
File Type: pdf tracks equations.pdf (16.1 KB, 84 views)
  #27   Spotlight this post!  
Unread 04-02-2012, 01:06 AM
Andrew Lawrence
 
Posts: n/a
Re: Curve Driving Generator

Wow. All I have to say is Ether = Genius.

I'll email my programmers this thread, since it will put them to good use for the next 9 months or so.
  #28   Spotlight this post!  
Unread 04-02-2012, 09:19 AM
dbeckwith's Avatar
dbeckwith dbeckwith is offline
Lead Programmer
AKA: Daniel Beckwith
FRC #3205 (The Patriots)
Team Role: Programmer
 
Join Date: Jan 2010
Rookie Year: 2009
Location: USA
Posts: 84
dbeckwith is an unknown quantity at this point
Re: Curve Driving Generator

Quote:
Originally Posted by Ether View Post
In another earlier post, dbeckwith suggested a Bezier curve. I believe the instantaneous vehicle rotation rate for a given vehicle forward speed can also be computed from the parametric Bezier equations, but I haven't actually done it yet.
The reason I suggested them is because they are parametric, so I thought they might be easier to deal with, and also you can control the shape of the curve in without changing the tangents at the endpoints easily, which I'm not sure you can do with cubic curves.

This is all really cool by the way, I'd love to try to implement this in our robot, even if it isn't entirely practical or needed, just because it's such an interesting problem. Another layer of complexity to add with our robot though is that we use Mecanum drive, so basically we could have the robot drive along the curve with the X and Y coordinates being affected by forward and strafing motion, and the heading just constantly changing by itself from the first tangent to the last.
I think that actually brings up another problem though, which I've been wondering about for a while: how do you get a robot with Mecanum drive to drive completely straight while turning at a constant rate (as in, the overall motion of the robot is in a straight line, but it is constantly rotating)? This might be going way beyond what the OP was looking for, but it was just something I was thinking about since we have Mecanums.
__________________
q = (2*b) | ~(2*b);

if (life.getLemons() != null) this.lemonade = new Drink(life.getLemons());
else throw new NoLemonsException("What now?");


  #29   Spotlight this post!  
Unread 04-02-2012, 09:30 AM
Ether's Avatar
Ether Ether is offline
systems engineer (retired)
no team
 
Join Date: Nov 2009
Rookie Year: 1969
Location: US
Posts: 8,724
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: Curve Driving Generator

Quote:
Originally Posted by dbeckwith View Post
that actually brings up another problem though, which I've been wondering about for a while: how do you get a robot with Mecanum drive to drive completely straight while turning at a constant rate (as in, the overall motion of the robot is in a straight line, but it is constantly rotating)?
If you have a gyro, so that you know the heading of the robot, you can transform the driver's commands into "field-centric" commands1. So, for example, pushing forward on the joystick would mean "translate straight downfield, regardless of your current heading". So the robot could be rotating continuously as it travels a straight line.

In actual practice, it doesn't work perfectly, because of sensor accuracy and mechanical imperfections (pesky friction and free play etc), but with a well-designed robot built with fine craftsmanship, you can get pretty close.

1The equations to do this can be found here; or you can use the gyro input to the mecanum drive provided in WPILib


Last edited by Ether : 04-02-2012 at 09:40 AM. Reason: added footnote with link
  #30   Spotlight this post!  
Unread 04-02-2012, 02:40 PM
theNerd's Avatar
theNerd theNerd is offline
Registered User
FRC #3329 (Cam Bots)
Team Role: Programmer
 
Join Date: Jan 2011
Rookie Year: 2110
Location: St. Marys
Posts: 51
theNerd is an unknown quantity at this point
Re: Curve Driving Generator

Quote:
Originally Posted by Ether View Post

Food for thought

See attached graph.

There's a Segway with a 2-foot wheelbase sitting at the origin.

Its heading is aligned with the +X axis.

I drive the Segway so that the center of its axle follows the black Bezier curve.

At the end of the path, the Segway's heading is parallel to the X axis and its coordinates are x=10 y=3.

The red curve shows the track of its left wheel, and the green curve shows the track of its right wheel.

The red, black, and green curves all have the SAME LENGTH (~10.6807 feet), but they are three DIFFERENT SHAPES.

If I were to command the left and right sides of a skid-steer vehicle to each travel exactly 10.6807 feet, the chances that the vehicle would follow the black curve are pretty slim. In fact, if the left and right sides were controlled identically and perfectly, it would travel in a straight line and end up on the X axis with coordinates x=10 y=0.


Ok, so arc length would not bee what I am looking for but left and right wheel speeds? (Since the integrals of the three curves are different?)
Also, I noticed with the 3rd degree polynomial solution I can not compute a curve for a pi/2 rotation for the robot at the destination point.
How would I go about working with bezier curves as I have never worked with them in my life .
Closed Thread


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 07:44 AM.

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