WCD vs. Swerve

[JamesTerm]

Originally Posted by Ether

That is called the Forward Kinematic Problem, and for swerve it has no kinematic solution for arbitrarily chosen values of the wheel speeds and angles. See the discussion starting at the bottom of Page7 of this paper.

Thanks for this link as well... I am a bit surprised that there are no solution cases. In my mind the robot is going to do something for those cases, and I'd look at this from a physics standpoint... by applying a vectored force to each point and let the opposing forces cancel each other out. Perhaps this is over-simplified, but perhaps it is worth testing a solution like this against the problem presented here, and see how equal they become.

[Ether]

Originally Posted by JamesTerm

Thanks for this link as well... I am a bit surprised that there are no solution cases. In my mind the robot is going to do something for those cases

I did not say there were no solutions or that the robot would not do something. I said there were no kinematic solutions. Take a closer look at the link I posted previously; the meaning of this is explained there.

, and I'd look at this from a physics standpoint...

Kinematics is physics :-)

by applying a vectored force to each point

This is dynamics (also physics). For arbitrarily chosen values for each of the four wheel speeds and steering angles, a dynamic analysis of this sort could be done. Write a paper when you're done.

[JamesTerm]

Originally Posted by Ether

I did not say there were no solutions or that the robot would not do something. I said there were no kinematic solutions. Take a closer look at the link I posted previously; the meaning of this is explained there.

Kinematic until just now was Greek to me... thanks for pointing that out... I come from a kinectic and dynamic study of problems in my work on the game. I don't really understand these abstract terms to their fullest yet, but I do know the details of what I know just not really the scope and label of it.

I find it interesting how dropping an adjective from a sentence can really change the meaning.

FWIW I am not good at writing papers, but I can submit some code example when I get to that point.

[JamesTerm]

Originally Posted by Gdeaver

1640 has done 4 wheel independent drive, independent steering the last two years.

I have a question for you... (or anyone else that can answer)... Does your robot work in a way where you can submit desired angles to each wheel independently? (I presume that is what the *independent* means in your statement).

If so, how do you sense when the rotation has reached its angle? potentiometer? IIRC Bomb Squad uses windows motors on each wheel to control the swerve. From what I have heard, other teams use a rod to swerve both front wheels the same amount, and can then have a manual control setup doing it this way.

[Ether]

Originally Posted by JamesTerm

The other half will be the reverse of that where given the wheel angles and speeds, what is the current x, y and rotational (i.e. angular) velocities.

Why do you want to do this?

[JamesTerm]

Originally Posted by Ether

Why do you want to do this?

"Originally Posted by JamesTerm
The other half will be the reverse of that where given the wheel angles and speeds, what is the current x, y and rotational (i.e. angular) velocities.
"

For 2 reasons... one is to render the images position on our simulation. I use text graphics on OSG (open scene graph). The other reason is for autonomous where if I can interpolate the position on a 2D field I can calculate the desired velocity and orientation to hit a target.

I have achieved these tasks with WCD, but I have not yet confirmed angular motion (logomotion did not require that). I am still researching this, but I believe I can have accurate angular turns if I have reliable encoder readings of distance. Anyhow... none of this matters if we switch to swerve.

[Ether]

Under what circumstances would you ever be commanding wheel speeds and steering angles that are not the result of an inverse kinematic calculation? If you are using kinematically correct speeds and steering angles for all four wheels, you should be getting the vehicle translational and rotational velocities which were used to compute those speeds and steering angles¹. Use those vehicle translational and rotational velocities to render the image's position in your simulation.

If your concern is about rapidly changing operator commands and the dynamic response of your steering and/or drive motors, and you are envisioning measuring actual wheel speeds and steering angles and using them to figure out where the vehicle is over time, then yes, you would need a way to compute what the vehicle is doing at each moment in time based on the measured wheel speeds and angles. How do you propose to do this computation?


¹ unless your wheels are slipping/sliding (due to an obstruction perhaps). In that case predicting vehicle motion accurately without additional sensors seems problematic.

Originally Posted by JamesTerm

"Originally Posted by JamesTerm
The other half will be the reverse of that where given the wheel angles and speeds, what is the current x, y and rotational (i.e. angular) velocities.
"

For 2 reasons... one is to render the images position on our simulation. I use text graphics on OSG (open scene graph). The other reason is for autonomous where if I can interpolate the position on a 2D field I can calculate the desired velocity and orientation to hit a target.

I have achieved these tasks with WCD, but I have not yet confirmed angular motion (logomotion did not require that). I am still researching this, but I believe I can have accurate angular turns if I have reliable encoder readings of distance. Anyhow... none of this matters if we switch to swerve.

[Ether]

Originally Posted by Ether

How do you propose to do this computation?

If the wheel speeds and steering angles are not too far from an inverse kinematic solution, the following computation may be adequate to estimate vehicle behavior:

FWD = (sFR*cos(aFR)+sFL*cos(aFL)+sRL*cos(aRL)+sRR*cos(aRR))/4;

STR = (sFR*sin(aFR)+sFL*sin(aFL)+sRL*sin(aRL)+sRR*sin(aRR))/4;

omega = ((sFR*cos(atan2(W,L)+pi/2-aFR)+sFL*cos(atan2(-W,L)+pi/2-aFL)
+sRL*cos(atan2(-W,-L)+pi/2-aRL)+sRR*cos(atan2(W,-L)+pi/2-aRR))/4)/
(sqrt(L^2+W^2)/2);

... where:

L and W are wheelbase and trackwidth in inches

sFR, sFL, sRL, and sRR are wheel tangential speeds in feet/second

aFR, aFL, aRL, and aRR are wheel angles in radians clockwise from straight ahead

FWD and STR are vehicle velocity in feet/sec in the forward and strafe_right directions

and omega is the vehicle clockwise rotational velocity in radians/sec

[JamesTerm]

Originally Posted by Ether

If the wheel speeds and steering angles are not too far from an inverse kinematic solution, the following computation may be adequate to estimate vehicle behavior:

Thanks so much for these equations! I am so happy that there is someone here who understands what I'm trying to do.

Now that you have submitted these... I gotta try them out and let you know how they behave... I hope to have something written in the next few days.

Thanks again!

P.S. I do as you say take actual measurments and work with these over a slice of time... the slice of time is measured in the main loop and submitted as a delta double seconds parameter through the entire cycle. (I do not use any other threads... i.e. my own PID).

[JamesCH95]

Originally Posted by JamesTerm

That all depends on how you look at it... It is possible to swap out a button control on functionality that uses an analog intensity parameter. The idea is that the button starts digital, but then transforms to analog given the amount of time it is held down. I hope that makes sense.... it is like taking the joystick and moving it from zero to full intensity in a given amount of time.

The most important point of this is that if designed right it is easy to swap buttons with axis controls with minimal code change... or overhead. This is why I love c++.

If you do not want to use a joystick, I would suggest using the analog triggers on a game pad. This would allow the driver to have two joysticks and two analog triggers at their disposal.

[JamesTerm]

Originally Posted by Ether

If the wheel speeds and steering angles are not too far from an inverse kinematic solution, the following computation may be adequate to estimate vehicle behavior:

I have been working with both equations and have had success with my first iteration (from velocity to the vars and back to velocity)... It appears the omega equation is almost twice as much as what it originally was. I've tweaked with the equation and found substituting the pi/2 with dimensions.lengh()/2... it falls short but is much closer.

dimensions.length is sqrt(l*l + w*w)

[JamesTerm]

Originally Posted by JamesCH95

If you do not want to use a joystick, I would suggest using the analog triggers on a game pad. This would allow the driver to have two joysticks and two analog triggers at their disposal.

Ah yeah that is cool as well... or better yet have the driver use the kinnect and lean to the left or right.

[JamesTerm]

Originally Posted by Ether

omega = ((sFR*cos(atan2(W,L)+pi/2-aFR)+sFL*cos(atan2(-W,L)+pi/2-aFL)
+sRL*cos(atan2(-W,-L)+pi/2-aRL)+sRR*cos(atan2(W,-L)+pi/2-aRR))/4)/
(sqrt(L^2+W^2)/2);

This is what I have confirmed the omega equation to be:

const double omega = (((_.sFR*cos(atan2(W,L)+(HP-_.aFR))/4)+
(_.sFL*cos(atan2(-W,L)+(HP-_.aFL))/4)+
(_.sRL*cos(atan2(-W,-L)+(HP-_.aRL))/4)+
(_.sRR*cos(atan2(W,-L)+(HP-_.aRR))/4)));

Since it is just about Christmas I'll show you a similar equation I wrote (that helped me figure this one out)... This is what I use when appling a vectored force to a point within a rigid body.

void PhysicsEntity_2D::ApplyFractionalForce( const osg::Vec2d &force, const osg::Vec2d &point,double FrameDuration )
{
	//Use this as a "get by" if the code doesn't work properly
#if 0
	ApplyFractionalForce(force,FrameDuration);
	return;
#endif
	//Here is a rough draft to solve in 2 dimensions
	//A=atan2(py,px)   point
	//M=pi/2 - A
	//L=atan2(fy,fx)  force
	//N=L+M
	//Y=sin(N)*f.length = contribution for force
	//X=cos(N)*f.length = contribution for torque

	double TorqueToApply;
	osg::Vec2d ForceToApply;
	double RadialArmDistance;

	{
		double A=atan2(point[1],point[0]);
		double M=(M_PI/2) - A;
		double L=atan2(-force[1],-force[0]);
		double N=L+M;

		double ForceLength= sqrt((force[1]*force[1])+(force[0]*force[0]));
		RadialArmDistance= sqrt((point[1]*point[1])+(point[0]*point[0]));
		//I've reserved a special case for ships which haven't specified  their radius size, in which case we simply factor out the radial arm too
		if ((m_RadiusOfConcentratedMass==1.0)&&(RadialArmDistance>1.0)) RadialArmDistance=1.0;

		//Fr = t   ... We should multiply force by the radial arm distance to get the torque
		//but instead,  we pass it off to physics where the multiply gets applied directly against the Radius of Concentrated Mass
		//We could multiply here but doing it the other way keeps the torque value low, which also makes it easier to debug
		TorqueToApply=(cos(N)*ForceLength); 
	}

	osg::Vec2d vecToCenter = -point;
	//Note we should be able to support a point set at 0,0,0 in which case we use the force itself as the direction... otherwise a zero'd point
	//results in a zero'd vector which would omit applying the force
	if (vecToCenter.length2()==0.0)
		vecToCenter=-force;

	vecToCenter.normalize();

	ForceToApply = vecToCenter * (force * vecToCenter);

	ApplyFractionalForce(ForceToApply,FrameDuration);
	ApplyFractionalTorque(TorqueToApply,FrameDuration,RadialArmDistance);
}

[Ether]

Originally Posted by Ether

omega = ((sFR*cos(atan2(W,L)+pi/2-aFR)+sFL*cos(atan2(-W,L)+pi/2-aFL)
+sRL*cos(atan2(-W,-L)+pi/2-aRL)+sRR*cos(atan2(W,-L)+pi/2-aRR))/4)/
(sqrt(L^2+W^2)/2);

Originally Posted by JamesTerm

This is what I have confirmed the omega equation to be:

const double omega = (((_.sFR*cos(atan2(W,L)+(HP-_.aFR))/4)+
(_.sFL*cos(atan2(-W,L)+(HP-_.aFL))/4)+
(_.sRL*cos(atan2(-W,-L)+(HP-_.aRL))/4)+
(_.sRR*cos(atan2(W,-L)+(HP-_.aRR))/4)));

Assuming "HP" means "pi/2", then the equation you posted is arithmetically identical to the one I gave you, with the exception that you removed the final "/(sqrt(L^2+W^2)/2)"... which is required if you want omega to be in units of angular velocity (rad/sec).

There was a typo in my original post. The notes at the bottom said "L and W are wheelbase and trackwidth in inches". That should have said feet.

[JamesTerm]

Originally Posted by Ether

Assuming "HP" means "pi/2", then the equation you posted is arithmetically identical to the one I gave you, with the exception that you removed the final "/(sqrt(L^2+W^2)/2)"... which is required if you want omega to be in units of angular velocity (rad/sec).

HP means pi/2... I work in meters, so the other part wasn't an issue for me. Oh and each vector is divided by 4. I have a working demo here:

http://www.termstech.com/files/SwerveDriveDemo.wmv

The demo adds a layer of swivel management where it is paced to 18 radians per second on angular acceleration and only 270 degrees of freedom.

Thanks again for these equations... now I just need a swerve drive robot to put this software on.