calculating position using follower wheels

Ether · October 4, 2013, 5:29pm

*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 · October 4, 2013, 6:07pm

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 · October 4, 2013, 6:10pm

Nice work. That was quick.

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

flameout · October 4, 2013, 6:14pm

solution
[spoiler]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[/spoiler]

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 · October 4, 2013, 6:41pm

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 · October 4, 2013, 9:33pm

Solution
[spoiler]
(1.3836,-0.3707); 15 degrees clockwise
[/spoiler]

Can this be done without integrals?

EricH · October 4, 2013, 9:41pm

Not answering the math questions… just this one.

A spoiler tag looks like spoiler=yourspoilernamehere] what the spoiler is[/spoiler] (without the space in the first bracket) and ends up looking like:

This is a spoiler

I warned you this was a spoiler.

And now back to the math…

RyanCahoon · October 4, 2013, 10:14pm

Question 3

[spoiler]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(2Pi/3)t + Pi/12] + 4Cos(2*Pi/3)*t + Pi/12], t, 0, 3]
= -1.3836

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

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

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 · October 4, 2013, 10:17pm

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 · October 4, 2013, 10:31pm

I did have it rotate counter-clockwise (oops). Corrected answer is the same, with both coordinates their opposites.

Ether · October 5, 2013, 9:13am

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

*

3 omni followers C code.txt (974 Bytes)

3 omni followers C code.txt (974 Bytes)

maths222 · October 5, 2013, 10:01am

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 · October 5, 2013, 11:08am

*Question 4:

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

*
*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:

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

maths222 · October 5, 2013, 12:06pm

Justification
[spoiler]
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.
[/spoiler]

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

Ether · October 5, 2013, 12:23pm

Excellent. A very intuitive explanation.

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

*

arc & chord.pdf (15.8 KB)

arc & chord.pdf (15.8 KB)

RyanCahoon · October 6, 2013, 2:20am

It’s Mathematica’s syntax, which is also accepted by Wolfram Alpha, making it useful for quick one-line integral problems.

Ether · October 6, 2013, 9:37pm

*Just for fun.

*

Ether · October 7, 2013, 6:32pm

**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 · October 7, 2013, 7:01pm

solution
[spoiler]X position: -5.9443 ft
Y position: -9.1103 ft
Distance: 129.5972 ft[/spoiler]

MATLAB:

code
[spoiler]

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)

[/spoiler]

Ether · October 7, 2013, 7:22pm

flameout:

solution

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

MATLAB:

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.