Determining Which Way To Turn Revisted
Author
Hi All,
I'm going to have to explain the problem I'm having through example. Think that will be easiest. Up to not I have an A* alg with determines the path correctly. Then some vector post processing with works out the angles needed to turn and which direction to follow said path.
My problem is working out which direction to turn on the first angle (The actual angle is alway correct). In theory, using the X product works fine, however I'm using these commands to control a real robot, so it may under turn or over turn a bit.
The things I know are: 1) The path I need to follow. 2) The robots current XY position and angle.
Assume these are the map cells it needs to follow:
(0,0) Starting at 0 Degrees
(1,1) Turn Right 45 degrees
(2,2) Forward
(3,3) Forward
(4,4) Finish.
If at (1,1) the robot has only turned 39 degrees I'm correctly working out that on the next command it will need to turn 6 degrees, similarly if its turned 51 degrees etc.
Like I say it's finding the which way it's supposed to turn is my problem. X Prod doesn't work Z component doesn't always work until the robot is quite a way off course.
Any one have any suggestions. This is driving me nuts
Many Thanks
Mark
I admit I don't quite understand the problem as described, but if the problem is simply how to determine which direction to turn given a target position (in 2-d), the solution is straightforward:
vector2 f = robot.current_forward_direction;vector2 t = target_position - robot.current_position;float angle = atan2(perp_dot(f,t), dot(f,t));
Author
Sorry yes, I've been finding it hard to explain.
I've tried that. Unfortunately still same problem. I still get no difference to allow me to decide on which direction to take. Here are two examples I've just run:
1)
Starting Pos (0,0)
Turn 45 degrees.
New Pos (2,2) Angle 37 degrees
Next Command
Turn 8 Degrees
New Pos (2,2) Angle 45 degrees
OK
2)
Starting Pos (0,0)
Turn 45 degrees.
New Pos (2,2) Angle 51 degrees
Turn 6 Degrees
New Pos (2,2) Angle 57 degrees
ERROR
This is the code I'm using:
Not sure if that makes things any clearer? I thought it should work with just the x prod, doesn't :(
Thanks
Mark
[Edited by - markgame66 on March 16, 2008 5:07:16 PM]
I've tried that. Unfortunately still same problem. I still get no difference to allow me to decide on which direction to take. Here are two examples I've just run:
1)
Starting Pos (0,0)
Turn 45 degrees.
New Pos (2,2) Angle 37 degrees
Next Command
Turn 8 Degrees
New Pos (2,2) Angle 45 degrees
OK
2)
Starting Pos (0,0)
Turn 45 degrees.
New Pos (2,2) Angle 51 degrees
Turn 6 Degrees
New Pos (2,2) Angle 57 degrees
ERROR
This is the code I'm using:
RobotVector.X = lRobotsCell->X; RobotVector.Y = lRobotsCell->Y; // Work out the target vector - lPathCell is the next cell calculated by the A* alg TargetVector.X = (lPathCell->X - lRobotsCell->X); TargetVector.Y = (lPathCell->Y - lRobotsCell->Y); // Find the length of the vector Length = SquareRoot(x*x + y*y) lTargetVectorMagnatude= sqrt(static_cast<double>((TargetVector.X * TargetVector.X) + (TargetVector.Y * TargetVector.Y))); // This is calculating the correction angle. The position of the robot is a unit vector, // i.e. where the length is 1 (We don't have another point from which to determine the // robots vector. // Need to be measured anti-clockwise double lRobotOrientationRads = Utilities::Deg2Rad(lRobotsCell->Orientation); lPathCell->pParentCell->NavAngle = (TargetVector.X*cos(lRobotOrientationRads)) + (TargetVector.Y*sin(lRobotOrientationRads)); lPathCell->pParentCell->NavAngle = lPathCell->pParentCell->NavAngle/lTargetVectorMagnatude; lPathCell->pParentCell->NavAngle = Utilities::Rad2Deg(acos(lPathCell->pParentCell->NavAngle)); lPathCell->Orientation = lRobotsCell->Orientation - static_cast<int>(lPathCell->pParentCell->NavAngle); if ( lPathCell->Orientation >= 360 ) { lPathCell->Orientation = lPathCell->Orientation - 360; } else if ( lPathCell->Orientation <= -360 ) { lPathCell->Orientation = lPathCell->Orientation + 360; } // Depending on the sign of the determine which way to turn // This is the problem bit... CrossProductVector.Z = (RobotVector.X * TargetVector.Y ) - (RobotVector.Y * TargetVector.X); lDotProduct = ((RobotVector.X * TargetVector.X) + (RobotVector.Y * TargetVector.Y)); double latanResult = atan2(CrossProductVector.Z, lDotProduct); latanResult = Utilities::Rad2Deg(latanResult); if (latanResult < 0) { lPathCell->pParentCell->NavAngle= -lPathCell->pParentCell->NavAngle; }Not sure if that makes things any clearer? I thought it should work with just the x prod, doesn't :(
Thanks
Mark
[Edited by - markgame66 on March 16, 2008 5:07:16 PM]
I'd be happy to look at your code, but could you edit your post and place the code in [source] tags? That'll make it easier to read.
Author
Think this looks ok ;)
RobotVector.X = lRobotsCell->X; RobotVector.Y = lRobotsCell->Y; // Work out the target vector - lPathCell is the next cell calculated by the A* alg TargetVector.X = (lPathCell->X - lRobotsCell->X); TargetVector.Y = (lPathCell->Y - lRobotsCell->Y); // Find the length of the vector Length = SquareRoot(x*x + y*y) lTargetVectorMagnatude= sqrt(static_cast<double>((TargetVector.X * TargetVector.X) + (TargetVector.Y * TargetVector.Y))); // This is calculating the correction angle. The position of the robot is a unit vector, // i.e. where the length is 1 (We don't have another point from which to determine the // robots vector. // Need to be measured anti-clockwise double lRobotOrientationRads = Utilities::Deg2Rad(lRobotsCell->Orientation); lPathCell->pParentCell->NavAngle = (TargetVector.X*cos(lRobotOrientationRads)) + (TargetVector.Y*sin(lRobotOrientationRads)); lPathCell->pParentCell->NavAngle = lPathCell->pParentCell->NavAngle/lTargetVectorMagnatude; lPathCell->pParentCell->NavAngle = Utilities::Rad2Deg(acos(lPathCell->pParentCell->NavAngle)); lPathCell->Orientation = lRobotsCell->Orientation - static_cast<int>(lPathCell->pParentCell->NavAngle); if ( lPathCell->Orientation >= 360 ) { lPathCell->Orientation = lPathCell->Orientation - 360; } else if ( lPathCell->Orientation <= -360 ) { lPathCell->Orientation = lPathCell->Orientation + 360; } // Depending on the sign of the determine which way to turn // This is the problem bit... CrossProductVector.Z = (RobotVector.X * TargetVector.Y ) - (RobotVector.Y * TargetVector.X); lDotProduct = ((RobotVector.X * TargetVector.X) + (RobotVector.Y * TargetVector.Y)); double latanResult = atan2(CrossProductVector.Z, lDotProduct); latanResult = Utilities::Rad2Deg(latanResult); if (latanResult < 0) { lPathCell->pParentCell->NavAngle= -lPathCell->pParentCell->NavAngle; }
Author
Should anyone stumble across this - I've figured out the solution.
I realiesed as I know the size of the bot, the direction and an initial point I can work out another point along the direction of travel. Once I have that I can get a vector for the bot. Then, as usual, use the Z component of the X prod to determine direction. YAY it works!
Mark
I realiesed as I know the size of the bot, the direction and an initial point I can work out another point along the direction of travel. Once I have that I can get a vector for the bot. Then, as usual, use the Z component of the X prod to determine direction. YAY it works!
Mark
This topic is closed to new replies.
Advertisement
Popular Topics
Advertisement
