calculating position using follower wheels

[Ether]

Just for fun.

[RyanCahoon]

Originally Posted by Ether

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]

Originally Posted by Ether

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]

Spoiler for solution:

X position: -5.9443 ft
Y position: -9.1103 ft
Distance: 129.5972 ft

MATLAB:

Spoiler for 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]

Originally Posted by flameout

Spoiler for solution:

X position: -5.9443 ft
Y position: -9.1103 ft
Distance: 129.5972 ft

MATLAB:

Spoiler for 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]

Originally Posted by Ether

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

Originally Posted by Ether

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]

Originally Posted by RyanCahoon

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]

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

[flameout]

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

Spoiler for 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]

Originally Posted by flameout

I've refactored my code to be much faster...

How slow was it before? Maxima solves Question 5 in less than half a second:

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]

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

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]

Originally Posted by Ether

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.

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.

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

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]

Originally Posted by flameout

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.

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]

Originally Posted by Ether

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

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:

% 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]

Question 6:

Same as Question 5, except:

15 seconds instead of 30

and

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

[flameout]

Question 6 solution:

Spoiler for solution:

Position: ( -3.7271, -4.0749)
Distance: 60.8613

Code (slightly modified from my last script):

Spoiler for 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.