Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Math and Science (http://www.chiefdelphi.com/forums/forumdisplay.php?f=70)
-   -   calculating position using follower wheels (http://www.chiefdelphi.com/forums/showthread.php?t=120022)

Ether 04-10-2013 17:29
calculating position using follower wheels
 

A certain robot with a 3 degree-of-freedom drivetrain (FWD, STR, RCW) is on a flat level floor. At time T=0, its center of geometry (CoG) is located at the origin of an XY coordinate system fixed with respect to the floor; it is facing 15 degrees clockwise from the +Y axis; and it has the the following robot-centric constant motions:
forward = 5 ft/s
strafe_right = 4 ft/sec
rotate_CW = 120 degrees/sec
Question 1: What are the coordinates 3 seconds later, and what direction is the robot facing?

Students, Mentors, engineers, and professors welcome.

flameout 04-10-2013 18:07
Re: calculating position using follower wheels
 
It is at the origin, facing 15 degrees clockwise from the +Y axis.

EDIT: 120 deg/sec * 3 sec = 360 degrees -- it just goes in a circle.

Ether 04-10-2013 18:10
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1294610)
It is at the origin, facing 15 degrees clockwise from the +Y axis.

EDIT: 120 deg/sec * 3 sec = 360 degrees -- it just goes in a circle.
Nice work. That was quick.

Question 2: What's the radius of the circle?

flameout 04-10-2013 18:14
Re: calculating position using follower wheels
 
Spoiler for solution:
Speed = sqrt(4^2+5^2), which approximately equals 6.4 ft/s

6.4 ft/s * 3 sec = 19.2 feet (circumference of the circle).

Thus the radius is 19.2 feet / (2*pi), or approximately 3.06 feet


EDIT: Perfectly matched text color w/ background color.
EDIT2: Oops, it's 5 ft/s forward and 4 ft/s strafing, not 4 ft/s and 3 ft/s
EDIT3: Changed colored text to a spoiler -- thank you EricH

Ether 04-10-2013 18:41
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1294612)
Text made the same color as the background so as not to give it away for others -- highlight to read.
Good job. I see this is too easy.

This one is quite a bit more difficult:

Question 3: Exactly the same as Question 1, except the FWD speed is a function of time, as follows: FWD = 5.0 + 1.0*T. In other words, FWD starts with the value 5.0 at T=0, and increases smoothly and linearly at a rate of 1 ft/sec/sec. The STR and RCW remain constant at 4 ft/sec and 120 deg/sec respectively.

maths222 04-10-2013 21:33
Re: calculating position using follower wheels
 
Spoiler for Solution:

(1.3836,-0.3707); 15 degrees clockwise

Can this be done without integrals?

EricH 04-10-2013 21:41
Re: calculating position using follower wheels
 
Not answering the math questions... just this one.
Quote:
Originally Posted by flameout (Post 1294612)
Text made the same color as the background so as not to give it away for others -- highlight to read. (Is there a way to spoiler tag text here?)
A spoiler tag looks like [ spoiler=yourspoilernamehere] what the spoiler is[/spoiler] (without the space in the first bracket) and ends up looking like:

Spoiler for This is a spoiler:
I warned you this was a spoiler.


And now back to the math...

RyanCahoon 04-10-2013 22:14
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1294614)
Question 3:
Spoiler for Question 3:
T=0: (0,0) pi/12 (from +Y axis)

FWD = (5.0 + 1.0*T) ft/s
STR = 4 ft/sec
RCW = 120 degrees/sec

T=3:
x = Integrate[(5 + t)*Sin[(2*Pi/3)*t + Pi/12] + 4*Cos[(2*Pi/3)*t + Pi/12], t, 0, 3]
= -1.3836

y = Integrate[(5 + t)*Cos[(2*Pi/3)*t + Pi/12] - 4*Sin[(2*Pi/3)*t + Pi/12], t, 0, 3]
= 0.3707

angle = (2*Pi/3)*3 + Pi/12 = Pi/12 = 15 degrees


Quote:
Originally Posted by maths222 (Post 1294619)
...
maths222, what coordinate orientation did you use? If X x Y is out of the page (like axes are usually drawn), I think you got your angle sign wrong (the strafe movement cancels out. rotation is clockwise, so the robot's forward movement is toward +X for most of the first half of the movement, and -X for the second half. since the robot moves faster during the second half, a negative result makes sense)

flameout 04-10-2013 22:17
Re: calculating position using follower wheels
 
Quote:
Originally Posted by RyanCahoon (Post 1294637)
maths222, what coordinate orientation did you use? If X x Y is out of the page (like axes are usually drawn), I think you got your angle sign wrong (the strafe movement cancels out. rotation is clockwise, so the robot's forward movement is toward +X for most of the first half of the movement, and -X for the second half. since the robot moves faster during the second half, a negative result makes sense)
I think he's just reporting the angular orientation of the robot. Since the angular rate does not depend on the location (in the XY plane), it is still facing 15 degrees clockwise from the Y axis.

maths222 04-10-2013 22:31
Re: calculating position using follower wheels
 
I did have it rotate counter-clockwise (oops). Corrected answer is the same, with both coordinates their opposites.

Ether 05-10-2013 09:13
Re: calculating position using follower wheels
 
2 Attachment(s)

Great work, Jacob and Ryan. Reps to you both.

Ryan: what CAS did you use for that? The syntax you used was rejected by Maxima, Octave, and SciLab. (I got it to work in Maxima by changing the syntax a bit)

Jacob: You got the right answer, but didn't show your work. How did you solve it?


This has a potential practical application for FRC. If you put 3 omni follower wheels -- in the the correct configuration -- on a robot, you can get FWD, STR, and RCW information from them. Then you can use something like the attached C code to get the position and orientation of the robot.

maths222 05-10-2013 10:01
Re: calculating position using follower wheels
 
I used Microsoft mathematics, which only allows radians in calculus functions:
X:
integral((5+1t)cos((90-15)(pi/180)-120pi/180t)+3sin((90-15)(pi/180)-120pi/180t), t, 0, 3)
Y:
integral((5+1t)sin((90-15)(pi/180)-120pi/180t)+3cos((90-15)(pi/180)-120pi/180t), t, 0, 3)

Ether 05-10-2013 11:08
Re: calculating position using follower wheels
 

Question 4:

In the code block highlighted in blue at the bottom of post 11 the following code appears for calculating position and heading:

Code:

Q+=dR/2.0;
X+=dF*sin(Q)+dS*cos(Q);
Y+=dF*cos(Q)-dS*sin(Q);
Q+=dR/2.0;
Give the mathematical justification for updating the heading "Q" twice in half-steps, instead of doing it like this:

Code:

Q+=dR;
X+=dF*sin(Q)+dS*cos(Q);
Y+=dF*cos(Q)-dS*sin(Q);

maths222 05-10-2013 12:06
Re: calculating position using follower wheels
 
Spoiler for Justification:

For a given time interval, the robot turns from Q_i to Q_f. The average velocity over that time period is not in the direction of Q_i or Q_f, but somewhere in between; within the sample rate of the idler wheels, the most accurate angle is halfway in between. Therefore, half of dQ is added before, then the sin and cos of Q are used to calculate the new X and Y positions, and then the other half is added.


It is not the best proof, but it explains the general reasoning.

Ether 05-10-2013 12:23
Re: calculating position using follower wheels
 
1 Attachment(s)
Quote:
Originally Posted by maths222 (Post 1294714)
Spoiler for Justification:

For a given time interval, the robot turns from Q_i to Q_f. The average velocity over that time period is not in the direction of Q_i or Q_f, but somewhere in between; within the sample rate of the idler wheels, the most accurate angle is halfway in between. Therefore, half of dQ is added before, then the sin and cos of Q are used to calculate the new X and Y positions, and then the other half is added.


It is not the best proof, but it explains the general reasoning.
Excellent. A very intuitive explanation.

See attachment for additional explanation using geometry and a bit of calculus.

RyanCahoon 06-10-2013 02:20
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1294695)
Ryan: what CAS did you use for that? The syntax you used was rejected by Maxima, Octave, and SciLab.
It's Mathematica's syntax, which is also accepted by Wolfram Alpha, making it useful for quick one-line integral problems.

Ether 06-10-2013 21:37
Re: calculating position using follower wheels
 
5 Attachment(s)

Just for fun.

Ether 07-10-2013 18:32
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1294603)

A certain robot with a 3 degree-of-freedom drivetrain (FWD, STR, RCW) is on a flat level floor. At time T=0, its center of geometry (CoG) is located at the origin of an XY coordinate system fixed with respect to the floor; it is facing 15 degrees clockwise from the +Y axis; and it has the the following robot-centric constant motions:
forward = 5 ft/s
strafe_right = 4 ft/sec
rotate_CW = 120 degrees/sec
Question 1: What are the coordinates 3 seconds later, and what direction is the robot facing?
Question 5: Same as Question 1 (above), except:
forward = 5*sin(t/2) ft/s
strafe_right = 4*sin(t/2.2) ft/sec
rotate_CW = 1.5*sin(t/2.5) radians/sec
5a) What are the XY coordinates 30 seconds later?

5b) What is the path length traveled during that 30 seconds?

flameout 07-10-2013 19:01
Re: calculating position using follower wheels
 
Spoiler for solution:
X position: -5.9443 ft
Y position: -9.1103 ft
Distance: 129.5972 ft


MATLAB:
Spoiler for code:
Code:
rotate_CW = @(t) 1.5*sin(t/2.5);
angle = @(t) 5*pi/12 - integral(rotate_CW, 0, t, 'ArrayValued', true);
forward = @(t) 5*sin(t/2);
strafe_right = @(t) 4*sin(t/2.2);
xvel = @(t) cos(angle(t)) * forward(t) + cos(angle(t) - pi/2) * strafe_right(t);
yvel = @(t) sin(angle(t)) * forward(t) + sin(angle(t) - pi/2) * strafe_right(t);
xpos = @(t) integral(xvel, 0, t, 'ArrayValued', true);
ypos = @(t) integral(yvel, 0, t, 'ArrayValued', true);
spd  = @(t) sqrt(forward(t)^2 + strafe_right(t)^2);
distance = @(t) integral(spd, 0, t, 'ArrayValued', true);

x_position = xpos(30)
y_position = ypos(30)
distance = distance(30)

Ether 07-10-2013 19:22
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1295134)
Spoiler for solution:
X position: -5.9443 ft
Y position: -9.1103 ft
Distance: 129.5972 ft


MATLAB:
Spoiler for code:
Code:
rotate_CW = @(t) 1.5*sin(t/2.5);
angle = @(t) 5*pi/12 - integral(rotate_CW, 0, t, 'ArrayValued', true);
forward = @(t) 5*sin(t/2);
strafe_right = @(t) 4*sin(t/2.2);
xvel = @(t) cos(angle(t)) * forward(t) + cos(angle(t) - pi/2) * strafe_right(t);
yvel = @(t) sin(angle(t)) * forward(t) + sin(angle(t) - pi/2) * strafe_right(t);
xpos = @(t) integral(xvel, 0, t, 'ArrayValued', true);
ypos = @(t) integral(yvel, 0, t, 'ArrayValued', true);
spd  = @(t) sqrt(forward(t)^2 + strafe_right(t)^2);
distance = @(t) integral(spd, 0, t, 'ArrayValued', true);

x_position = xpos(30)
y_position = ypos(30)
distance = distance(30)
Impressive. Reps to you.

RyanCahoon 07-10-2013 19:36
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1295122)
5a) What are the XY coordinates 30 seconds later?
Spoiler for 5a:
T=0: (0,0) pi/12 (from +Y axis)

F(t) = 5*sin(t/2)
S(t) = 4*sin(t/2.2)
dQ = 1.5*sin(t/2.5)

Q(t) = (Integrate[1.5*Sin[t/2.5], t, 0, t]) + Pi/12 = 4.0118-3.75*Cos[0.4*t]

T=30:
dX(t) = F(t)*Sin[Q(t)]+S(t)*Cos[Q(t)]
X = Integrate[dX(t), t, 0, 30]
= Integrate[5*Sin[t/2]*Sin[4.0118-3.75*Cos[0.4*t]] + 4*Sin[t/2.2]*Cos[4.0118-3.75*Cos[0.4*t]], t, 0, 30]
= -5.94429

dY(t) = F(t)*Cos[Q(t)]-S(t)*Sin[Q(t)]
y = Integrate[dY(t), t, 0, 30]
= Integrate[5*Sin[t/2]*Cos[4.0118-3.75*Cos[0.4*t]] - 4*Sin[t/2.2]*Sin[4.0118-3.75*Cos[0.4*t]], t, 0, 30]
= -9.11027


Quote:
Originally Posted by Ether (Post 1295122)
5b) What is the path length traveled during that 30 seconds?
Spoiler for 5b:
T=0: (0,0) pi/12 (from +Y axis)

FWD = 5*sin(t/2) ft/s
STR = 4*sin(t/2.2) ft/sec
RCW = 1.5*sin(t/2.5) radians/sec

T=30:
distance = Integrate[Sqrt[(dX(t))^2+(dY(t))^2], t, 0, 30]
= Integrate[Sqrt[(5*Sin[t/2]*Sin[4.0118-3.75*Cos[0.4*t]] + 4*Sin[t/2.2]*Cos[4.0118-3.75*Cos[0.4*t]])^2+(5*Sin[t/2]*Cos[4.0118-3.75*Cos[0.4*t]] - 4*Sin[t/2.2]*Sin[4.0118-3.75*Cos[0.4*t]])^2], t, 0, 30]
= 129.597

Ether 07-10-2013 19:48
Re: calculating position using follower wheels
 
Quote:
Originally Posted by RyanCahoon (Post 1295146)
Spoiler for 5a:
T=0: (0,0) pi/12 (from +Y axis)

F(t) = 5*sin(t/2)
S(t) = 4*sin(t/2.2)
dQ = 1.5*sin(t/2.5)

Q(t) = (Integrate[1.5*Sin[t/2.5], t, 0, t]) + Pi/12 = 4.0118-3.75*Cos[0.4*t]

T=30:
dX(t) = F(t)*Sin[Q(t)]+S(t)*Cos[Q(t)]
X = Integrate[dX(t), t, 0, 30]
= Integrate[5*Sin[t/2]*Sin[4.0118-3.75*Cos[0.4*t]] + 4*Sin[t/2.2]*Cos[4.0118-3.75*Cos[0.4*t]], t, 0, 30]
= -5.94429

dY(t) = F(t)*Cos[Q(t)]-S(t)*Sin[Q(t)]
y = Integrate[dY(t), t, 0, 30]
= Integrate[5*Sin[t/2]*Cos[4.0118-3.75*Cos[0.4*t]] - 4*Sin[t/2.2]*Sin[4.0118-3.75*Cos[0.4*t]], t, 0, 30]
= -9.11027




Spoiler for 5b:
T=0: (0,0) pi/12 (from +Y axis)

FWD = 5*sin(t/2) ft/s
STR = 4*sin(t/2.2) ft/sec
RCW = 1.5*sin(t/2.5) radians/sec

T=30:
distance = Integrate[Sqrt[(dX(t))^2+(dY(t))^2], t, 0, 30]
= Integrate[Sqrt[(5*Sin[t/2]*Sin[4.0118-3.75*Cos[0.4*t]] + 4*Sin[t/2.2]*Cos[4.0118-3.75*Cos[0.4*t]])^2+(5*Sin[t/2]*Cos[4.0118-3.75*Cos[0.4*t]] - 4*Sin[t/2.2]*Sin[4.0118-3.75*Cos[0.4*t]])^2], t, 0, 30]
= 129.597
Nice work too. You guys rock.

Ether 07-10-2013 19:51
Re: calculating position using follower wheels
 

In one hour from the time of this post, I will post Question 6.

flameout 07-10-2013 19:55
Re: calculating position using follower wheels
 
1 Attachment(s)
I've refactored my code to be much faster (using basic manual Euler integration instead of MATLAB's integral() function) and plotted the robot's position for problem 5. The plot is attached.

Here is the code. I would have done this in C++, but MATLAB lets me plot much more easily :rolleyes:
Spoiler for code:
Code:
% Set up various useful functions that don't require integration to calculate
rotate_CW = @(t) 1.5*sin(t/2.5);
forward = @(t) 5*sin(t/2);
strafe_right = @(t) 4*sin(t/2.2);
xvel = @(t,angle) cos(angle) * forward(t) + cos(angle - pi/2) * strafe_right(t);
yvel = @(t,angle) sin(angle) * forward(t) + sin(angle - pi/2) * strafe_right(t);
spd  = @(t) sqrt(forward(t)^2 + strafe_right(t)^2);

% Set parameters and initialize variables (preallocating variables for performance)
duration = 30;
steps    = 30000; %dt = 1 millisecond
dt      = duration / steps;
angle    = zeros(1,steps+1); % Off by one -- t = 0 is also in each vector
xpos    = zeros(1,steps+1);
ypos    = zeros(1,steps+1);
distance = zeros(1,steps+1);

% Initialize angle to 15 degrees positive off the Y axis
angle(1) = 5*pi/12;

% Do the integration. Using Euler integration
for iter = 1:steps
        time            = duration * iter / steps;
        xpos(iter+1)    = xpos(iter) + dt * xvel(time, angle(iter));
        ypos(iter+1)    = ypos(iter) + dt * yvel(time, angle(iter));
        distance(iter+1) = distance(iter) + dt * spd(time);

        % No intuitive tricks with angle here -- just do stupid simple Euler integration.
        angle(iter+1)    = angle(iter) - dt * rotate_CW(time);
end

Ether 07-10-2013 20:26
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1295150)
I've refactored my code to be much faster...
How slow was it before? Maxima solves Question 5 in less than half a second:

Code:
Maxima 5.27.0 http://maxima.sourceforge.net
using Lisp GNU Common Lisp (GCL) GCL 2.6.8

assume(t>0)$

Qo: 15/180*%pi$

dFdT: 5.0*sin(t/2.0)$
dSdT: 4.0*sin(t/2.2)$
dQdT: 1.5*sin(t/2.5)$

Q: Qo + integrate(dQdT,t,0,t)$

dXdT: dFdT*sin(Q)+dSdT*cos(Q)$
dYdT: dFdT*cos(Q)-dSdT*sin(Q)$
dLdT: sqrt(dXdT^2+dYdT^2)$

quad_qags(dXdT, t, 0, 30);
      [- 5.94428316349145, 5.1523954241625701E-10, 315, 0]

quad_qags(dYdT, t, 0, 30);
      [- 9.110270199929753, 3.4870273187953572E-10, 315, 0]

quad_qags(dLdT, t, 0, 30);
      [129.5971872631463, 1.1496649242892635E-6, 441, 0]
Quote:
...using basic manual Euler integration
Use trapezoidal instead. It's very simple to do and much more accurate, so you can use fewer steps and speed it up even more.

Also see Post 13 for a simple change to improve accuracy so you can use fewer steps and run even faster. Post 15 explains why.


Quote:
I would have done this in C++, but MATLAB lets me plot much more easily
Get yourself a copy of gnuplot. It's free and script-driven, so you can write a script to edit/compile/run/and plot your C++ program.

flameout 07-10-2013 20:39
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1295157)
How slow was it before? Maxima solves Question 5 in less than half a second:
It took one or two seconds (MATLAB isn't terribly fast). It was entirely numerical, performing a numerical integration (for angle) inside another numerical integration (to get position).

Where it was painfully slow was plotting the position trajectory, where it needed to run the numerical integration inside a numerical integration over and over (plenty of redundant calculations). To get a high-quality plot would have taken over an hour of execution time.

Quote:
Originally Posted by Ether
Use trapezoidal instead. It's very simple to do and much more accurate, so you can use fewer steps and speed it up even more.
If I really cared about accuracy and speed, I would be using Simpson's Rule integration (perhaps using trapezoidal first to get an error estimate, for adaptive integration). Also, I'd analytically solve for the integrals that are analytically solvable (such as the angle).

Another good technique would be to use an ODE solver rather than pure integration -- if I were to do this, I'd probably use MATLAB's built in 4th/5th order adaptive Runge-Kutta solver (ode45). This would probably be plenty fast as well.

Quote:
Also see Post 13 for a simple change to improve accuracy so you can use fewer steps and run even faster. Post 15 explains why.
I saw that, and considered doing that. I don't have a good reason for choosing what I did, other than that it "works" and is easy to explain (as a left-hand Riemann sum).

Quote:
Get yourself a copy of gnuplot. It's free and script-driven, so you can write a script to edit/compile/run/and plot your C++ program.
I really need to learn that sometime...

Ether 07-10-2013 20:55
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1295160)
If I really cared about accuracy and speed, I would be using Simpson's Rule integration...
It's a cost/benefit thing. The cost of converting Euler to trapezoidal for this problem is very small. Going from trap to Simpson's would require more rework, make the code less readable, and probably even slow it down for equivalent accuracy.

Quote:
Another good technique would be to use an ODE solver rather than pure integration -- if I were to do this, I'd probably use MATLAB's built in 4th/5th order adaptive Runge-Kutta solver (ode45). This would probably be plenty fast as well.
It would be interesting to do a comparison. My guess is that for this problem, trapezoidal in compiled C would be just as fast for equivalent accuracy as RK in MatLab.

flameout 07-10-2013 21:07
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1295170)
It's a cost/benefit thing. The cost of converting Euler to trapezoidal for this problem is very small. Going from trap to Simpson's would require more rework, make the code less readable, and probably even slow it down for equivalent accuracy.
True (except for the last part -- I don't think Simpson's integration would slow it down for equivalent accuracy, but I haven't tried it, so I don't know).

Quote:
It would be interesting to do a comparison. My guess is that for this problem, trapezoidal in compiled C would be just as fast for equivalent accuracy as RK in MatLab
Having used MATLAB a bit, I think compiled C++ w/ trapezoidal integration would be way faster for the same accuracy than MATLAB code using an RK-based solver.

Here's code that solves it using one of MATLAB's adaptive ODE solvers (4/5th-order Runge-Kutta (I don't recall which formulation it is)):
Spoiler for more code:
Code:
% Set up various useful functions that don't require integration to calculate
angle = @(t) 5*pi/12 - 3.75 + 3.75 * cos(t/2.5); % Obtained analytically (by hand)
forward = @(t) 5*sin(t/2);
strafe_right = @(t) 4*sin(t/2.2);
xvel = @(t) cos(angle(t)) * forward(t) + cos(angle(t) - pi/2) * strafe_right(t);
yvel = @(t) sin(angle(t)) * forward(t) + sin(angle(t) - pi/2) * strafe_right(t);
spd  = @(t) sqrt(forward(t)^2 + strafe_right(t)^2);

% Set up for the ODE solution
tspan = [0 30];
x0    = [0; 0; 0]; % xpos, ypos, distance

% The ODE function
dynamics = @(t,x) [xvel(t); yvel(t); spd(t)];

% Call ode45
diffsoln = ode45(dynamics, tspan, x0);

% Function for obtaining the state at a given time (for plotting)
statefun = @(t) deval(diffsoln, t);

% Time values for plot (using a lot of points so it looks good).
times = linspace(tspan(1), tspan(2), 10000);

% X and Y positions
xpos = [1 0 0] * statefun(times);
ypos = [0 1 0] * statefun(times);

% Do the plotting
plot(xpos, ypos, 'LineWidth', 5)

Ether 07-10-2013 21:13
Re: calculating position using follower wheels
 

Question 6:

Same as Question 5, except:

15 seconds instead of 30

and

rotate_CW = (e^T-1)^(1/17) radians/sec

flameout 07-10-2013 21:22
Re: calculating position using follower wheels
 
Question 6 solution:
Spoiler for solution:
Position: ( -3.7271, -4.0749)
Distance: 60.8613


Code (slightly modified from my last script):
Spoiler for code:
Code:
% Set up various useful functions that don't require integration to calculate
rotate_CW = @(t) (exp(t) - 1)^(1/17);
forward = @(t) 5*sin(t/2);
strafe_right = @(t) 4*sin(t/2.2);
xvel = @(t,angle) cos(angle) * forward(t) + cos(angle - pi/2) * strafe_right(t);
yvel = @(t,angle) sin(angle) * forward(t) + sin(angle - pi/2) * strafe_right(t);
spd  = @(t) sqrt(forward(t)^2 + strafe_right(t)^2);

% Set up for the ODE solution
tspan = [0 15];
x0    = [0; 0; 0; 5*pi/12]; % xpos, ypos, distance, angle

% The ODE function
dynamics = @(t,x) [xvel(t, x(4)); yvel(t, x(4)); spd(t); -rotate_CW(t)];

% Call ode45
diffsoln = ode45(dynamics, tspan, x0);

% Function for obtaining the state at a given time (for plotting)
statefun = @(t) deval(diffsoln, t);

% Time values for plot (using a lot of points so it looks good).
times = linspace(tspan(1), tspan(2), 10000);

% X and Y positions
xpos = [1 0 0 0] * statefun(times);
ypos = [0 1 0 0] * statefun(times);

% Do the plotting
plot(xpos, ypos, 'LineWidth', 5)

% Ending positions and distance
end_x    = xpos(end)
end_y    = ypos(end)
distance = [0 0 1 0] * statefun(tspan(2))


Perhaps I should stop and give someone else a chance to answer first -- this is way too easy when all I need to do is change a script I already wrote.

Ether 07-10-2013 22:12
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1295177)
this is way too easy when all I need to do is change a script I already wrote.
Yeah; I had already prepared Question 6 back when everyone was using a closed-form analytical solution for Q(t). You beat me to the punch by setting up your numerical integration script.


Question 6 solution:
Spoiler for solution:
Position: ( -3.7271, -4.0749)
Distance: 60.8613
I think you are showing more decimal places than are warranted for the accuracy of your solution method. I'm differing from you in the third decimal place for X and Y. Compiled C on 8-year-old 32-bit Windows XP machine takes roughly 40ms to compute.

Can someone else weigh in with their numbers for this?

flameout 07-10-2013 22:28
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1295185)
Yeah; I had already prepared Question 6 back when everyone was using a closed-form analytical solution for Q(t). You beat me to the punch by setting up your numerical integration script.
Makes sense.

Quote:
Originally Posted by Ether
Question 6 solution:
Spoiler for solution:
Position: ( -3.7271, -4.0749)
Distance: 60.8613
I think you are showing more decimal places than are warranted for the accuracy of your solution method.
I never bothered to do any accuracy analysis, so that's probably the case.

After lowering the tolerances and calculating bounds for the error (total error should be less than 1e-7), I got the following revised figures:
Spoiler for solution:
Position: (-3.7350, -4.0685)
Distance: 60.8668

New parameters and error analysis:
Relative and absolute tolerances set to 10^-12 (previously 10^-6): The error at each step should not exceed max(10^-12, |x * 10^-12|), where x is the current value of the ODE solution
Number of ODE solver steps: 1022
Maximum element of the state: 60.8668
Upper bound for error: max(10^-12, |60.8668 * 10^-12|) * 1022 = 6.2267e-08

Ether 07-10-2013 23:10
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1295191)
After lowering the tolerances and calculating bounds for the error (total error should be less than 1e-7), I got the following revised figures...
Spoiler for solution:
Position: (-3.7350, -4.0685)
Distance: 60.8668
That matches my results out to the number of decimal places shown, except I'm getting a 6 instead of a 5 in the fourth decimal place of the Y value.

flameout 08-10-2013 00:10
Re: calculating position using follower wheels
 
Quote:
Originally Posted by Ether (Post 1295199)
That matches my results out to the number of decimal places shown, except I'm getting a 6 instead of a 5 in the fourth decimal place of the Y value
I re-ran my script once for each of MATLAB's ODE solvers, adjusting the tolerances as necessary to get a reasonable solve time.

Every solver agreed through the fifth decimal place (for the Y value). The stiff solvers needed significantly larger error tolerances and took more steps; their predicted error was much higher.

The nonstiff solvers all agreed through the seventh decimal place.

The lowest predicted error was obtained through the ode113 solver, using a relative tolerance of 100 * epsilon and an absolute tolerance of 10^-12. According to my error estimate, all of the following digits are correct:

Spoiler for solution:
Position: (-3.73497544, -4.06852591)
Distance: 60.86682701


It was interesting going through all the solvers -- many of the stiff solvers had error estimates in excess of 10^-4. I guess this shows that the choice of solver really can have an effect on the error, and not just the solution time.

Ether 08-10-2013 00:50
Re: calculating position using follower wheels
 
Quote:
Originally Posted by flameout (Post 1295205)
Every solver agreed through the fifth decimal place (for the Y value).
The rotate_CW function has some nasty behavior near zero. So I tried replacing the numerical integration of that function in the region T<0.01 with an analytic solution of a 3rd order Taylor expansion. I'm now matching your results with a time step of 1 microsecond.

Bleary-eyed, time to call it quits for the night.

Ether 01-11-2013 17:53
Re: calculating position using follower wheels
 
1 Attachment(s)

The large black rectangle in the attached sketch represents a top view of a robot.

The forward direction of the robot is upwards in the sketch.

The 4 red arrows represent 4 unpowered instrumented (with encoders) omni follower wheels. For each of the wheels, the arrow points in the + direction for that wheel.


Question7

Find the formula for the FWD (forward), STR (strafe right), and RCW (rotate clockwise) robot motions in terms of the X1, Y1, X2, and Y2 wheel speeds.

RyanCahoon 01-11-2013 18:39
Re: calculating position using follower wheels
 
Spoiler for Question 7:
FWD=(Y1+Y2)/2
STR=(X1+X2)/2
RCW=(X1-X2)/L=(Y2-Y1)/W

Ether 01-11-2013 19:51
Re: calculating position using follower wheels
 
Quote:
Originally Posted by RyanCahoon (Post 1299724)
Spoiler for Question 7:
FWD=(Y1+Y2)/2
STR=(X1+X2)/2
RCW=(X1-X2)/L=(Y2-Y1)/W
Looks good to me. Anybody disagree or have questions?


All times are GMT -5. The time now is 07:44.

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