calculating position using follower wheels

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

Students, Mentors, engineers, and professors welcome.

[flameout]

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]

Originally Posted by flameout

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]

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]

Originally Posted by flameout

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]

Spoiler for Solution:

(1.3836,-0.3707); 15 degrees clockwise

Can this be done without integrals?

[EricH]

Not answering the math questions... just this one.

Originally Posted by flameout

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]

Originally Posted by Ether

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

Originally Posted by maths222

...

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]

Originally Posted by RyanCahoon

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]

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

[Ether]

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]

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]

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]

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]

Originally Posted by maths222

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.