#include <upose2pose.h>

Inheritance diagram for UPose2pose:

Public Member Functions
bool	get22arcLeft (double r1, double h1, double h2, FILE logFp=NULL)

bool	get22arcRight (double r1, double h1, double h2, FILE logFp=NULL)

bool	get2arcQ4 (double r1, double h1, double h2, FILE logFp=NULL)

bool	get2hereALA (int manType, double initVel, double maxAcc, double maxTurnAcc, double radius1, double arc1, double dist, double radius2, double arc2, FILE *logFp)

bool	get2hereLALA (int manType, double initVel, double maxAcc, double maxTurnAcc, double minTurnRad, double breakDist, double breakToVel, double radius1, double arc1, double dist, double radius2, double arc2, double *finalBreak)

bool	get2LeftLineLeft (double initVel, double maxAcc, double maxTurnAcc, double radius1, double arc1, double dist, double radius2, double arc2, FILE logFp)

bool	get2LeftLineRight (double initVel, double maxAcc, double maxTurnAcc, double radius1, double arc1, double dist, double radius2, double arc2, FILE logFp)

UPose	get2line (double turnRad, double turn1, double direct, double *turn2)

UPose	get2lineStartLeft (double turnRad, double turn1, double direct, double *turn2)

bool	get2RightLineLeft (double initVel, double maxAcc, double maxTurnAcc, double radius1, double arc1, double dist, double radius2, double arc2, FILE logFp)

bool	get2RightLineRight (double initVel, double maxAcc, double maxTurnAcc, double radius1, double arc1, double dist, double radius2, double arc2, FILE logFp)

bool	get2ViaBreakLeftLineLeft (double initVel, double maxAcc, double maxTurnAcc, double minTurnRad, double breakDist, double toVel, double radius1, double arc1, double dist, double radius2, double arc2, double finalBreak)

bool	get2ViaBreakLeftLineRight (double initVel, double maxAcc, double maxTurnAcc, double minTurnRad, double breakDist, double toVel, double radius1, double arc1, double dist, double radius2, double arc2, double finalBreak)

bool	get2ViaBreakRightLineLeft (double initVel, double maxAcc, double maxTurnAcc, double minTurnRad, double initialBreakDist, double toVel, double radius1, double arc1, double dist, double radius2, double arc2, double finalBreak)

bool	get2ViaBreakRightLineRight (double initVel, double maxAcc, double maxTurnAcc, double minTurnRad, double initialBreakDist, double toVel, double radius1, double arc1, double dist, double radius2, double arc2, double finalBreak)

UPose2pose	operator= (UPose source)

UPose2pose	operator= (UPoseV source)

	UPose2pose ()

	UPose2pose (double ix, double iy, double ih, double iv)

	~UPose2pose ()

Public Member Functions inherited from UPoseV
virtual void	fprint (FILE fd, const char prestring)

virtual const char *	getDataType ()

UPose	getPose ()

double	getVel ()

UPoseV	operator+ (UPoseV pDelta)

UPoseV	operator+ (UPose pDelta)

UPoseV	operator- (UPoseV pRef)

UPoseV	operator= (UPose source)

UPoseV	operator= (UPoseV source)

void	print (const char prestring, char buff, const int buffCnt)

void	print (const char *prestring)

void	set (double ix, double iy, double ih, double iv)

void	set (UPose pose, double iv)

void	setVel (double velocity)

virtual void	snprint (const char preString, char buff, const int buffCnt)

	UPoseV ()

	UPoseV (double x, double y, double h, double v)

virtual	~UPoseV ()

Public Member Functions inherited from UPose
void	add (double dist, double headingChange)

void	add (double ix, double iy, double ih)

UMatrix4	asCol3 ()

UMatrix4	asCol4 ()

UMatrix4	asMatrix2x2MtoP ()

UMatrix4	asMatrix2x2PtoM ()

UMatrix4	asMatrix3x3MtoP ()

UMatrix4	asMatrix3x3PtoM ()

UMatrix4	asMatrix4x4MtoP ()

UMatrix4	asMatrix4x4PtoM ()

UMatrix4	asMatrix4x4PtoMPos ()

UMatrix4	asRow3 ()

UMatrix4	asRow4 ()

virtual void	clear ()

virtual const char *	codeXml (char buf, const int bufCnt, const char extraAttr)

virtual const char *	codeXml (const char name, char buf, const int bufCnt, const char *extraAttr)

void	fprint (FILE fd, const char prestring)

char *	getAsSml (const char name, char buff, int buffCnt)

double	getDistance (UPose other)

double	getDistance (UPose *other)

double	getDistance (UPosition other)

double	getDistance (UPosition *other)

double	getDistToPoseLine (const double Px, const double Py)

double	getDistToPoseLineSigned (const double Px, const double Py)

double	getHeadingDeg ()

double	getHeadingDiff (UPose other)

double	getHeadingRad ()

UPosition	getMapToPose (UPosition mapPos)

U2Dpos	getMapToPose (U2Dpos mapPos)

UPosition	getMapToPose (UPose *mapPos)

UPosition	getMapToPose (UPose mapPos)

CvPoint	getMapToPose (CvPoint mapPos)

UPose	getMapToPosePose (UPose *mapPose)

UPose	getMapToPosePose (UPose mapPose)

UPosition	getPos (double z=0.0)

UPosition	getPoseToMap (UPosition posePos)

U2Dpos	getPoseToMap (U2Dpos posePos)

UPosition	getPoseToMap (double localX, double localY)

UPosition	getPoseToMap (UPose posePos)

CvPoint	getPoseToMap (CvPoint mapPos)

UPose	getPoseToMapPose (UPose poseLocal)

UPose	getPoseToMapPose (double x, double y, double h)

UPose	neg ()

UPose	operator+ (UPose pDelta)

UPose	operator+ (UPoseV pDelta)

UPose	operator+ (UPoseTime pDelta)

UPose	operator+ (UPoseTVQ pDelta)

UPose	operator- (UPose pRef)

UPose	operator- (UPoseV pRef)

UPose	operator- (UPoseTime pRef)

UPose	operator- (UPoseTVQ pRef)

UPose	operator= (UPoseTime source)

UPose	operator= (UPoseV source)

UPose	operator= (UPoseTVQ source)

UPose	operator= (UMatrix4 mat)

void	print (char buf, const char prestring, int bufLng)

void	print (const char *str)

void	set (UPosition pos, URotation rot)

void	set (double ix, double iy, double ih)

UPose	set (UMatrix *mat)

	UPose ()

	UPose (double x, double y, double h)

	UPose (UPose *source)

	UPose (UPosition pos, URotation rot)

virtual	~UPose ()

Public Member Functions inherited from UDataBase
bool	isA (const char *typeString)

virtual bool	isAlsoA (const char *typeString)

	UDataBase ()

virtual	~UDataBase ()

Additional Inherited Members
Public Attributes inherited from UPoseV
double	vel

Public Attributes inherited from UPose
double	h

double	x

double	y

Protected Member Functions inherited from UPose
UPose	addCed (UPose D)

UPose	addCed (UPose *D)

void	addDeltaPose (UPose D)

void	asAddC (UPose Vbase, UPose D)

void	asNegC (UPose D)

void	asSubC (UPose Va, UPose Vbase)

void	subC (UPose *Vbase)

UPose	subCed (UPose Vbase)

UPose	subCed (UPose *Vbase)

Detailed Description

Functions to get from one pose to another

Author: Christian Andersen

Constructor & Destructor Documentation

UPose2pose::UPose2pose ( )

Constructor

UPose2pose::UPose2pose	(	double	ix,
		double	iy,
		double	ih,
		double	iv
	)

Constructor with initial value

UPose2pose::~UPose2pose ( )

Destructor

Member Function Documentation

bool UPose2pose::get22arcLeft	(	double *	r1,
		double *	h1,
		double *	h2,
		FILE *	logFp = `NULL`
	)

Get to this position from pose (0,0,0) in just 2 arcs (left, then right) with the same turn radius. Returns true if valid result (otherwise most likely impossible). Returns result radius in 'r1', first arc in 'h1' (radians) and second arc in 'h2' (radians).

References get2arcQ4(), UPose::h, UPoseV::vel, UPose::x, and UPose::y.

bool UPose2pose::get22arcRight	(	double *	r1,
		double *	h1,
		double *	h2,
		FILE *	logFp = `NULL`
	)

Get to this position from pose (0,0,0) in just 2 arcs (right, then left) with the same turn radius. Returns true if valid result (otherwise most likely impossible). Returns result radius in 'r1', first arc in 'h1' (radians) and second arc in 'h2' (radians).

References get2arcQ4().

bool UPose2pose::get2arcQ4	(	double *	r1,
		double *	h1,
		double *	h2,
		FILE *	logFp = `NULL`
	)

Get a navigation solution with 2 arcs, where first arc is to the right (and the second to the left). The two arcs has the same radius, and is 'h1' to the right and 'h2' to the left after the right turn. Returns true if solution has found a solution in less than 10 iterations. Get to this position from pose (0,0,0) in just 2 arcs (right, then left) with same turn radius. Returns true if valid result (otherwise most likely impossible). Returns result radius in 'r1', arc right in 'h1' (radians) and arc left in 'h2' (radians).

References absd(), UMatrix::get(), UPose::h, UMatrix4::inversed(), limitToPi(), UMatrix::setRC(), UMatrix::setRow(), sqr(), UPose::x, and UPose::y.

Referenced by get22arcLeft(), and get22arcRight().

bool UPose2pose::get2hereALA	(	int *	manType,
		double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		FILE *	logFp
	)

Get the best solution to destination pose. Uses at maximum two arcs and one line inbetween. Starts at 0,0 with heading 0 and speed 'initVel', and gets to this.x .y .h, aiming for the end velocity this.vel. If not possible, then ends in a smaller turnradius then needed for a this.vel speed. If 'logFp' != NULL, then some calculations may be written in logfile. Returns true if a solution is found. Fails if initial velocity is too high.

References get2LeftLineLeft(), get2LeftLineRight(), get2RightLineLeft(), and get2RightLineRight().

Referenced by UManSeq::expandAddManALA(), operator=(), and testTurn2().

bool UPose2pose::get2hereLALA	(	int *	manType,
		double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		double *	breakDist,
		double *	breakToVel,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		double *	finalBreak
	)

Get the best solution to destination pose. Uses at maximum one initial break or accelerate line, two arcs and one line inbetween. Starts at 0,0 with heading 0 and speed 'initVel', and gets to this.x .y .h, aiming for the end velocity this.vel. If not possible, then ends in a smaller turnradius then needed for a this.vel speed. If 'logFp' != NULL, then some calculations may be written in logfile. Returns true if a solution is found. Fails if initial velocity is too high.

References get2ViaBreakLeftLineLeft(), get2ViaBreakLeftLineRight(), get2ViaBreakRightLineLeft(), and get2ViaBreakRightLineRight().

Referenced by UManSeq::expandAddManLALA(), and operator=().

bool UPose2pose::get2LeftLineLeft	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		FILE *	logFp
	)

Same as get2RightLineRight(...), just with mirrored values

References get2RightLineRight(), UPose::h, UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereALA(), and operator=().

bool UPose2pose::get2LeftLineRight	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		FILE *	logFp
	)

Same as get2RightLineLeft(...), just with mirrored values

References get2RightLineLeft(), UPose::h, UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereALA(), and operator=().

UPose UPose2pose::get2line	(	double	turnRad,
		double *	turn1,
		double *	direct,
		double *	turn2
	)

Get a solution to get to the line defined by this pose, estimates drive behavoiur of the 'drive' and 'driveon' smrcl commands. The solution is intended for obstacle collision detect, and implemented as one 'driveon' command. The difference to real drive is untested, and will probably works best for an ackermann steered robot, that sets a turning limit not much narrower than the calculated turn radius. Turn radiuswill be calculated as 'pi/2 * gA / gD' (for gA=2 and gD=0.75 turn radius is about 4m)

Parameters

turnRad	used turn radius
turn1	is the initial turn - positive is to the left
direct	is the (potential) straight part
turn2	is the end turn - positive is left

Returns: the pose (on the line), where the manoeuvre is (assumed to be) finished.

References U2Dlined::distanceSigned(), get2lineStartLeft(), U2Dseg::getPositionOnLine(), UPose::h, U2Dseg::setFromPose(), U2Dpos::x, UPose::x, U2Dpos::y, and UPose::y.

Referenced by UManSeq::expandManDriveon(), operator=(), and testDriveonEst().

UPose UPose2pose::get2lineStartLeft	(	double	turnRad,
		double *	turn1,
		double *	direct,
		double *	turn2
	)

Get a solution to get to the line defined by this pose, estimates driveon behavoiur This function covers situations that will start with a left turn.

Parameters

turnRad	is the assumed turn radius
turn1	is the initial turn - positive is to the left
direct	is the (potential) straight part
turn2	is the end turn - positive is left

Returns: the pose (on the line), where the manoeuvre is (assumed to be) finished.

Todo:: error if h < ~ -M_PI/2

References U2Dlined::distanceSigned(), U2Dseg::getPositionOnLine(), UPose::h, limitTo2Pi(), U2Dseg::setFromPose(), U2Dpos::x, UPose::x, U2Dpos::y, and UPose::y.

Referenced by get2line(), and operator=().

bool UPose2pose::get2RightLineLeft	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		FILE *	logFp
	)

Calculate a pose-to-pose manoeuvre starting in a right turn, then a straight line and then a left turn. If not possible, or not possible with current speed and acc limits, then return false. The end velocity is in the end poseV, the initial velocity is in 'initVel'. on exit, the 5 result values are set (if not NULL or a solution is not found). Returns true if successful.

References a, UPose::h, sqr(), UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereALA(), get2LeftLineRight(), and operator=().

bool UPose2pose::get2RightLineRight	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		FILE *	logFp
	)

Calculate a pose-to-pose manoeuvre starting in a right turn, then a straight line and then a right turn. If not possible, or not possible with current speed and acc limits, then return false. The desired end velocity is in the end poseV, the initial velocity is in 'initVel'. on exit, the 5 result values are set (if not NULL or a solution is not found). Returns true if successful.

References a, UPose::h, limitToPi(), sqr(), UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereALA(), get2LeftLineLeft(), and operator=().

bool UPose2pose::get2ViaBreakLeftLineLeft	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		double *	breakDist,
		double *	toVel,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		double *	finalBreak
	)

Same as get2ViaBreakRightLineRight(...), just with mirrored values

References get2ViaBreakRightLineRight(), UPose::h, UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereLALA(), operator=(), and testPose2Pose().

bool UPose2pose::get2ViaBreakLeftLineRight	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		double *	breakDist,
		double *	toVel,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		double *	finalBreak
	)

References get2ViaBreakRightLineLeft(), UPose::h, UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereLALA(), operator=(), and testPose2Pose().

bool UPose2pose::get2ViaBreakRightLineLeft	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		double *	initialBreakDist,
		double *	toVel,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		double *	finalBreak
	)

Get the needed manoeuvre to get to this position, given this initial velocity. The solution consists of 2 arcs and a straight line in-between. There may be an initial straight line to break to an allowed speed for the turn.

References a, UPose::h, limitTo2Pi(), maxd(), sqr(), UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereLALA(), get2ViaBreakLeftLineRight(), operator=(), and testPose2Pose().

bool UPose2pose::get2ViaBreakRightLineRight	(	double	initVel,
		double	maxAcc,
		double	maxTurnAcc,
		double	minTurnRad,
		double *	initialBreakDist,
		double *	toVel,
		double *	radius1,
		double *	arc1,
		double *	dist,
		double *	radius2,
		double *	arc2,
		double *	finalBreak
	)

Calculate a pose-to-pose manoeuvre starting in a right turn, then a straight line and then a right turn. An initial break or acceleration for distance 'initialBreakDist', may be needed (to turn velocity 'toVel'. A final break may be needed too for 'finalBreak' distance. The end velocity may be lower than specified! (but not faster) If not possible, or not possible with current speed and acc limits, then return false. The desired end velocity is in the end poseV, the initial velocity is in 'initVel'. on exit, the 8 result values are set (if not NULL or no solution found). Returns true if successful.

References a, UPose::h, limitTo2Pi(), limitToPi(), maxd(), mind(), sqr(), UPoseV::vel, UPose::x, and UPose::y.

Referenced by get2hereLALA(), get2ViaBreakLeftLineLeft(), operator=(), and testPose2Pose().

UPose2pose UPose2pose::operator= ( UPose source )

inline

Try to find a manoeuver to this pose Assign from base pose

References UPose::h, UPose::x, and UPose::y.

UPose2pose UPose2pose::operator= ( UPoseV source )

inline

Assign from base pose

References dist, get2hereALA(), get2hereLALA(), get2LeftLineLeft(), get2LeftLineRight(), get2line(), get2lineStartLeft(), get2RightLineLeft(), get2RightLineRight(), get2ViaBreakLeftLineLeft(), get2ViaBreakLeftLineRight(), get2ViaBreakRightLineLeft(), get2ViaBreakRightLineRight(), UPose::h, UPose::x, and UPose::y.

The documentation for this class was generated from the following files:

Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation