Setting Servo Goals to Point at an Object Position

[Duckocide]

Hi

I'm getting lost in math and need some advice. I have a built a tank with a compound body (collection of boxes). Attached (via a RevoluteJoint in Servo mode) is the turret (Using Vector3.Up as the axis of freedom). Attached to the turret is a barrel. Once again using a RevoluteJoint (Vector3.Right as the axis of freedom).

I'm trying to work out the "Goal" settings for the servo's. That is, I have another entity somewhere in world space and I want to set the RevoluteJoint Servo Goal's so that the turret (rotation, relative to tank, around Vector3.Up) and barrel (Rotation, relative to turret, around Vector3.Right) yaw and pitch respectively to point towards the object.

I've ended up with some messy radian angle calculations using Math.Atan2((double)(ty - sy), (double)(tx - sx)) - Where t? is target and s? is origin - that try to take in to account the various orientations. But I'm guessing there is a better way than this using Quaternions and the ToolKit.

Can someone help me? I'm seeing Toolbox.GetQuaternionBetweenNormalizedVectors() is a potential way forward, but need some advice/code pointers (i.e. If I get a quaternion for the rotation from the turret to the object, how to I turn that in to a "Goal" setting. Ditto for the barrel).

[Norbo]

If there was an AngularMotor between the tank body and the turret barrel, a quaternion-based approach (or just a 3DOF approach in general) would end up being pretty natural. However, given that there are only two degrees of freedom available to control (turret yaw and pitch), the simplest approach probably won't end up using full rotations just because they introduce a third degree of freedom which would then need to be controlled.

Here's a fairly direct approach:

For yaw:
yawGoal = Math.Atan2(dot(fromTurretToTarget, measurementAxisY), dot(fromTurretToTarget, measurementAxisX)
where:
fromTurretToTarget = targetPosition - turretPosition
measurementAxisX = tankBodyToTurretBodyRevoluteJoint.Motor.Basis.XAxis
measurementAxisY = tankBodyToTurretBodyRevoluteJoint.Motor.Basis.YAxis

Similarly, for pitch:
pitchGoal = Math.Atan2(dot(fromTurretToTarget, measurementAxisY), dot(fromTurretToTarget, measurementAxisX)
where:
fromTurretToTarget = targetPosition - turretPosition
measurementAxisX = turretBodyToTurretBarrelRevoluteJoint.Motor.Basis.XAxis
measurementAxisY = turretBodyToTurretBarrelRevoluteJoint.Motor.Basis.YAxis

This approach projects the target direction onto a 'measurement plane' defined by the constraint's own basis. The constraint actually uses that basis in the same way to compute the current angle and interpret the goal angle during normal operation. It converts the 3d problem into a simple 2d trig problem.

Beware of singularities; if fromTurretToTarget is perpendicular to the measurement plane, then both Y and X parameters will be zero and Atan2 will fail because there is no unique solution.

[Duckocide]

Wonderful... Really appreciate the advice / steer!

However, is there a typo with "measurementAxisY = tankBodyToTurretBodyRevoluteJoint.Motor.Basis.YAxis"?

(YAxis isn't a property on the Basis object - I'm using v1.2.0 binaries)

[Norbo]

However, is there a typo with "measurementAxisY = tankBodyToTurretBodyRevoluteJoint.Motor.Basis.YAxis"?

Oops, yes. I forgot that I had eliminated the explicit Y axis; the primary and X axes uniquely specify the Y axis via cross product.

So, it should look like:

Code: Select all

measurementAxisX = tankBodyToTurretBodyRevoluteJoint.Motor.Basis.XAxis
measurementAxisY = Vector3.Cross(tankBodyToTurretBodyRevoluteJoint.Motor.Basis.PrimaryAxis, measurementAxisX)

[Duckocide]

Well. Something isn't quite right. The tankToTurretRevoluteJoint just spins round and round to varying speeds depending upon target position. It is definitely in Servo mode so guessing there is something wrong with the math? Any thoughts. Guessing it is something to do with the Basis and XAxis changing each frame?

My code:

Code: Select all

            
Vector3 fromTurretToTarget = Target - BEPUEntity.Position;
Vector3 measurementAxisX = TurretPivot.Motor.Basis.XAxis;
Vector3 measurementAxisY = Vector3.Cross(TurretPivot.Motor.Basis.PrimaryAxis, measurementAxisX);
TurretPivot.Motor.Settings.Servo.Goal = (float)Math.Atan2((double)Vector3.Dot(fromTurretToTarget, measurementAxisY), (double)Vector3.Dot(fromTurretToTarget, measurementAxisX));

Where BEPUEntity is the physics object for entity B on tankToTurretRevoluteJount (The turret) and TurretPivot is tankToTurretRevoluteJoint.

(I'll put gimble lock checks in once working )

[Norbo]

In general, the Basis of a constraint is attached to ConnectionA. So, if the constraint was created with with tankTurret as ConnectionA (passed in as the first parameter of the constructor), the basis will change with as the tank turret rotates. To make things more intuitive, make the tankBody the first parameter.

[Norbo]

Just read your post over again, I missed some things which make my above explanation confusing:

Where BEPUEntity is the physics object for entity B on tankToTurretRevoluteJount (The turret) and TurretPivot is tankToTurretRevoluteJoint.

If you've already got the tankToTurretRevoluteJoint equivalent created as:
ConnectionA: tankBody
ConnectionB: turretBody

and the turretBodyToTurretBarrelRevoluteJoint equivalent as:
ConnectionA: turretBody
ConnectionB: turretBarrel

then the 'goal constantly changes as the turret spins' issue should already be avoided. If that's the setup and wonky stuff is still happening, I'd recommend trying to implement it in the TankDemo reference implementation. If it still happens, I could take a direct look at the whole setup.

[Duckocide]

OK. I'll need to take a look later when I get back from work. I'll post up the bepu set up as well. RevoluteJoints are set up as follows:

TurretPivot...
ConnectionA: tank (collision body)
ConnectionB: turret (sphere)

BarrelPivot...
ConnectionA: turret (sphere)
ConnectionB: barrel (sphere - not representative, merely object for world transform)

[Duckocide]

Norbo! You da man

Ignore that last post of mine. I had re-arranged the order of the entities on the RevoluteJoint - Probably when I was hacking around with my own attempts. With the tank body first, it is stable.

Thanks for the advice. Looking good. Certainly for the Turret!

Update: Yes. The Barrel is working fine as well.

Really helpful this post, thanks Norbo!!!