Curve Driving Generator [Archive]

theNerd

29-03-2012, 21:26

Ok, I've run into a great problem for all you math and programming guys. I've been working at it since December but I have made no progress. Any help would be so very appreciated. Here goes:

I am trying to create a code that will give the robot the ability to drive curves. Essentially I want to give the function a coordinate - namely a polar vector - and have it spit out the left and right speeds that the robot needs to travel to drive in a smooth curve. If there is any way possible to integrate a starting tangent line to the curve - as in say "I want a curve that crosses x and x+dx such that dy/dx @ x is equal to some number -say b" - that would be very advantageous in that I could link two curves together to have the robot not only drive in a smooth curve but also face a certain direction when it reaches its final point.

So far I have figured that we would need to consider a). the currents current speed, b). the robots current direction that it is facing and c). the arc length that the curve would need to be. I have also figured that the distance that right wheel would travel (Rd), the distance the left wheel would travel (Ld) and the speeds of the wheels (Rv and Lv) are related in this manner: Ld/Rd = Lv/Rv. ---> this relationship was based on assuming that the time for each wheel to travel a distance would be the same (and thus the velocities are different).

Note: I'm trying to keep the curves as simple as possible. For now I just want to deal with curves going to the 1st and 2nd quadrants - assuming the current robot's position is at the origin. I DO NOT want to deal with the robot driving to points to where it has to turn past (+-)PI/2 (90 degrees).

PS: Lets not discuss why we would or would not do this but instead, focus on how. Thanks!

PAR_WIG1350

29-03-2012, 23:13

Um... what kind of drive configuration are you using? If you are using swerve, I could probably help.

Ether

30-03-2012, 00:07

Hawiian Cadder

30-03-2012, 00:20

I worked on a control scheme for a vex robot that assumes the robot is always traveling in a curve with radius R. The difference in power between the sides of the skid steer then becomes K(1/R), thus as the radius of the curve goes to infinity the difference between wheel speeds approaches 0. R had a realistic minimum in my setup (navigating a black tape maze with a light sensor) and so the algorithm worked well. For FRC it would need modification for points where R approached 0.

To achieve smooth controll I would use a PID loop where the direct control was how far side to side the robot was off course. This would control the path, planned such that the arc would cross the intended path at some fraction of the remaining distance to travel. By controlling the average of the two wheel speeds in skid steer, as wheel as the radius of the turn one could achieve any possible path. This would require closed loop motor control, as well as some method to determine where the robot actually was, as opposed to where it needed to be.

Ether

30-03-2012, 00:28

If you have coordinates and tangents at each endpoint, you can fit a cubic polynomial to those constraints. The result will be a smooth curve which connects the endpoints and has the desired direction at each end.

Once you know the equation of the polynomial connecting the starting point and the destination, you can compute the radius of curvature (and thus the required rate of rotation) at any point along the curve.

ewhitman

30-03-2012, 01:12

Ether is right that the first thing you need to do is plan a path. A polynomial will work find for many situations - you need a 4th order polynomial (y= ax^3+bx^2+cx+d) to fit position and slope at both ends. However, polynomials can give you weird behavior in some situations and can't handle others - you can't get a semi-circle for example.

The math is more complex, but I would recommend looking into splines as a more versatile tool for path planning. Alternately, you could try composing your trajectories entirely of circular arcs and straight segments.

Once you have a path, you can compute the curvature of the path everywhere along it. From the path curvature and the speed, you can compute angular rotation rate (degrees/second). From the rotation rate and the speed, you can get your wheel velocities (using the geometry of your robot).

Ether

30-03-2012, 02:08

Once you know the equation of the cubic connecting the starting point and the destination, you can compute the radius of curvature (and thus the required rate of rotation) at any point along the curve.

Attached is an example for starting point = (0,0) with a slope of 1/6
and a finish point = (3,9) with a slope of 2.

The blue line is the path and the red line is the reciprocal of the radius at each point along the curve.

If you download and install Maxima, you can play with the endpoints and see the curve that results.

you need a 4th order polynomial (y= ax^3+bx^2+cx+d)

For the record, that's a third-order polynomial (cubic).

you can't get a semi-circle

semi-circle would be a turn of pi. The OP limited the turn to pi/2.

polynomials can give you weird behavior in some situations... The math is more complex, but I would recommend looking into splines as a more versatile tool for path planning.

If your starting and ending points are such that a single cubic results in a less-than-desirable curve, you can use intermediate waypoints to get to your destination (i.e. cubic splines). I suspect that in this application for many (most?) cases a single cubic will be satisfactory.

dbeckwith

30-03-2012, 08:28

I personally think that Bezier curves are the easiest to deal with, especially if you want a particular tangent on the endpoints. I would recommend reading this (http://www.math.ucla.edu/~baker/149.1.02w/handouts/bb_bezier.pdf).
Once you understand that, programming them in shouldn't be too difficult, you would just advance the t value as the robot goes along to get the next direction you need to drive. The only challenge I really see is converting the coordinates given by the Bezier curve into drive commands, but it shouldn't be too difficult.

theNerd

30-03-2012, 12:08

I personally think that Bezier curves are the easiest to deal with

I did some research last night and I was looking at that. However, I'm not sure as to how more efficient bezier curves are to a polynomial - only testing would tell of course.

@Ether: Your idea is great! I like its simplicity, however, I do have a question: What if I don't want the robot to move in a cubic curve? Can this method still be used to create a path that looks similar to x^(1/2) - except with a bit more curve. Also, what if the current heading angle was a slope of infinity? And, instead of using matrices (as my knowledge is extremely rustic and could use some brushing up ) could I use a Taylor polynomial instead and achieve the same result - what I mean is I have no idea how to solve for the parameters using matrices.

theNerd

30-03-2012, 12:12

Um... what kind of drive configuration are you using? If you are using swerve, I could probably help.

We are using a tank drive.

Ether

30-03-2012, 19:24

I have no idea how to solve for the parameters using matrices.

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);

Ether

30-03-2012, 21:01

what if the current heading angle was a slope of infinity?

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

Whippet

30-03-2012, 21:09

Why not use tank control mode? That way, you can drive curves, but it takes a little more practice to use. It's just a matter of willingness to learn, but will save a lot of time programming. So it's kind of a double-edged sword.

Ether

30-03-2012, 21:45

MichaelBick

30-03-2012, 22:06

Why not use tank control mode? That way, you can drive curves, but it takes a little more practice to use. It's just a matter of willingness to learn, but tank mode can do curves, and will save a lot of time programming. So it's kind of a double-edged sword.

I agree. One of the reasons that a lowered center drive(I'm assuming not 4 wheel or treads, as I would not try these maneuvers with those drivetrains) is so competitive, is because it needs very little programming. Sure, some teams have more complex code(I'm thinking 254 with their physics engine for turn radius) that is really nice, it is not worth the effort for the negligible gain, unless you have extra time(like maybe getting a camera working and shooting accurately).

theNerd

31-03-2012, 00:13

Lets not discuss why we would or would not do this but instead, focus on how. Thanks!

Please re-read my original message

theNerd

31-03-2012, 00:17

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);

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!

Ether

31-03-2012, 00:56

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

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.

theNerd

31-03-2012, 12:29

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?

Ether

31-03-2012, 12:49

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)

dcherba

31-03-2012, 20:37

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/Quaternions_and_spatial_rotation
has a good primer on the subject. This is commonly used for computer graphics and simulations.

Have fun

Ether

31-03-2012, 22:09

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.

theNerd

01-04-2012, 14:11

[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.

MichaelBick

01-04-2012, 18:22

Tom Line

01-04-2012, 19:12

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.

Ether

02-04-2012, 00:59

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.

Andrew Lawrence

02-04-2012, 01:06

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.

dbeckwith

02-04-2012, 09:19

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.

Ether

02-04-2012, 09:30

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 (http://www.chiefdelphi.com/media/papers/download/2896); or you can use the gyro input to the mecanum drive provided in WPILib

theNerd

02-04-2012, 14:40

theNerd

02-04-2012, 14:48

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.

I have checked their code and the sections that I have seen are not well documented and I am confused on the logic behind their code.

Ether

03-04-2012, 00:04

How would I go about working with bezier curves as I have never worked with them in my life :) .

First play around with these Java applets to get a feel for what a Bezier curve is:

http://www.math.ucla.edu/~baker/java/hoefer/Bezier.htm

http://www.math.ubc.ca/~cass/gfx/bezier.html

... then I will try to answer your questions

dbeckwith

03-04-2012, 07:24

How would I go about working with bezier curves as I have never worked with them in my life :) .

Bezier curves are defined by a formula that can be generalized for any degree, but for the most common, degree 3, the equation for any point along the curve is: B(t) = (1 - t)^3 * P0 + 3 * (1 - t)^2 * t * P1 + 3 * (1 - t) * t^2 * P2 + t^3 * P3 for any t between 0 and 1, inclusive. So you have four control points in a degree 3 curve which you can use to define the shape of the curve, its endpoints, and its tangents at the endpoints (the applet Ether linked to are great for seeing how these affect the curve). I'm not a math genius like Ether so I'm not really sure exactly how to implement them into your question, but in my experience with computer graphics I've found that Bezier splines are easier to control in a way that makes sense than cubic splines.

The Wikipedia article on Bezier curves has a lot of cool information on them: http://en.wikipedia.org/wiki/Bezier_curves

Also if you needed to get the slope of a Bezier curve at some point for your pathing algorithm, I found this: http://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/Bezier/bezier-der.html

theNerd

03-04-2012, 14:08

... then I will try to answer your questions
]

Ok, I am ready. I have quite a good feel for Bezier curves now....which is great timing since my Calc class just hit parametric equations.

PTC_Chris

13-09-2012, 12:46

I have been conducting Mathcad webinars for FIRST teams. A search turned up this conversation and I have implemented the Matrix Solution as a Mathcad worksheet. The attached .zip folder contains both a Mathcad Prime 2.0 and a .pdf version of the worksheet.

PTC is once again offering Mathcad Prime for free to participants in FIRST Robotics programs. Contact FIRSTsupport@ptc.com if you are interested in obtaining a copy of Mathcad Prime.

You can learn how to create Mathcad worksheets like this on our Wednesday Night Robotalk Webinars (7 pm EST). This Wednesday night (Sept 19 2012) we will focus on the tools used in the attached worksheet. To register for the webinar click here (http://www.ptc.com/appserver/wcms/forms/index.jsp?&im_dbkey=148025&im_language=en).

Chris

Ether

13-09-2012, 13:16

I have implemented the Matrix Solution as a Mathcad worksheet.

Thanks for posting this Chris. Unfortunately, the example you picked is a case where a cubic polynomial is surely not what you'd want. A Bezier curve would give a much more desirable curve for the robot to follow. Does Mathcad support Bezier curves?

Attached is a Bezier curve for the example you gave.

Also, there's an Excel spreadsheet here for those who don't have Maxima* installed:

http://www.chiefdelphi.com/media/papers/2713

* Maxima is a free open-source Computer Algebra System, available for download at the link below. Highly recommended:

http://maxima.sourceforge.net/

PTC_Chris

14-09-2012, 08:39

@Ether: The example in the .pdf was more deliberate than unfortunate. I chose a tangent line slope that would exagerate the effect and help high school students to make connections to their work in schools. (I am an educator, not an engineer.)

I included the .pdf more to show what a Mathcad worksheet looks like. I think it is more transparent than either Excel or Maxima because Mathcad uses standard math notation. The .pdf was not meant to be the primary contribution.

The Mathcad worksheet is a computational document, so it is possible to change the parameters. The worksheet is therefore a general method for calculating a cubic polynomial given the endpoints and tangent slopes. As you observed in your post, there are some examples where the solution is efficient, others where it is not. With the Mathcad worksheet, it is possible to quickly derive and plot a path and determine whether or not it is a solution worth implementing.

It is certainly possible to build a Mathcad worksheet to generate the Bezier Curve. I will add this to my "Want to Do" list and post it to the Thread when I cross it off. Thanks for the suggestion and the Excel worksheet. I'll probably convert your Excel spreadsheet to Mathcad.