com.kuka.nav
Class Pose

java.lang.Object
  com.kuka.nav.XYTheta
      com.kuka.nav.TupleSE2
          com.kuka.nav.Pose

All Implemented Interfaces:

com.kuka.parameters.IParameter, Serializable

public class Pose
extends TupleSE2

The class holding a robot pose.

See Also:

Serialized Form

Field Summary

Fields inherited from class com.kuka.nav.XYTheta

_theta, _x, _y

Constructor Summary

Pose()
Creates a pose at (0,0,0).

Pose(double x, double y, double theta)
Creates a pose at the specified coordinates.

Pose(Pose pose)
Copy constructor.

Pose(TupleSE2 t)
Creates a pose from a tuple t (consisting of a twodimensional coordinate and an angle).

Method Summary

Pose	add(TupleSE2 other)
boolean	approxEqualsCart(TupleSE2 pose) Indicates whether the cartesian distance of the (x,y)-values and the distance of the angles of this pose and the given one are approximately equal with a tolerance of 1e-11.
boolean	approxEqualsCart(TupleSE2 pose, double toleranceLinear, double toleranceAngular) Indicates whether the cartesian distance of the (x,y)-values and the distance of the angles of this pose and the given one are approximately equal with the given translational and rotational tolerance.
boolean	approxEqualsMax(TupleSE2 pose) Indicates whether some other NavigationPose is "equal to" this one with a tolerance linear and angular tolerance of 1E-11 (with respect to the maximum norm).
boolean	approxEqualsMax(TupleSE2 pose, double toleranceLinear, double toleranceAngular) Indicates whether some other NavigationPose is "equal to" this one with respect to the given tolerance (with respect to the maximum norm).
Pose	closestPose(Pose[] poses) Picks the closest pose from an array of poses, using the 2d distance.
Pose	closestPose(Pose[] poses, Pose velocity) Picks the closest pose from an array of poses, using the a distance weighted by the maximum velocities in x- y- and theta direction.
Pose	copy()
double	distanceRot(TupleSE2 pose) Calculates the angular difference between this pose and the given one.
double	distanceTrans(TupleSE2 pose) Calculates the euclidean square-norm of the robots pose to a given pose in the 2-dimensional phase space (x,y).
Pose	invert() Interprets this TupleSE2 as a coordinate transformation, inverts it and returns the result.
Pose	multiply(TupleSE2 other) 'Multiplies' this tuple with the passed one, i.e. calculates the transformation of this TupleSE2 by applying the passed TupleSE2 to it.
static Pose	parseNavigationPose(String poseString) Parses a string and constructs a NavigationPose.
Pose	subtract(TupleSE2 other)
Position	toPosition() Returns the position of this pose.
String	toString()
Pose	withTheta(double newTheta) Returns a new TupleSE2 with a changed theta-coordinate.
Pose	withX(double newX) Returns a new TupleSE2 with a changed x-coordinate.
Pose	withY(double newY) Returns a new TupleSE2 with a changed y-coordinate.

Methods inherited from class com.kuka.nav.TupleSE2

min, norm, substract

Methods inherited from class com.kuka.nav.XYTheta

equals, getTheta, getX, getY, hashCode

Methods inherited from class java.lang.Object

clone, finalize, getClass, notify, notifyAll, wait, wait, wait

Constructor Detail

Pose

public Pose(double x,
            double y,
            double theta)

Creates a pose at the specified coordinates.

Parameters:

x - the x value

y - the z value

theta - the theta value in radians

Pose

public Pose(Pose pose)

Copy constructor. Creates a pose at the NavigationPose 'pose'.

Parameters:

pose - The pose from which the data is copied.

Pose

public Pose(TupleSE2 t)

Creates a pose from a tuple t (consisting of a twodimensional coordinate and an angle).

Parameters:

t - The tuple from which the pose is to be created.

Pose

public Pose()

Creates a pose at (0,0,0).

Method Detail

withX

public Pose withX(double newX)

Description copied from class: XYTheta

Returns a new TupleSE2 with a changed x-coordinate.

Overrides:

withX in class TupleSE2

Parameters:

newX - The new x coordinate.

Returns:

A new object with changed values.

withY

public Pose withY(double newY)

Description copied from class: XYTheta

Returns a new TupleSE2 with a changed y-coordinate.

Overrides:

withY in class TupleSE2

Parameters:

newY - The new y coordinate.

Returns:

A new object with changed values.

withTheta

public Pose withTheta(double newTheta)

Description copied from class: XYTheta

Returns a new TupleSE2 with a changed theta-coordinate.

Overrides:

withTheta in class TupleSE2

Parameters:

newTheta - The new theta coordinate.

Returns:

A new object with changed values.

subtract

public Pose subtract(TupleSE2 other)

Parameters:

other - Another tuple to substract from this tuple.

Returns:

A new tuple resulting from this minus other.

add

public Pose add(TupleSE2 other)

Parameters:

other - Another tuple to add to this tuple.

Returns:

A new tuple resulting from this plus other.

invert

public Pose invert()

Description copied from class: TupleSE2

Interprets this TupleSE2 as a coordinate transformation, inverts it and returns the result.

Overrides:

invert in class TupleSE2

Returns:

the inverse of this TupleSE2

multiply

public Pose multiply(TupleSE2 other)

Description copied from class: TupleSE2

'Multiplies' this tuple with the passed one, i.e. calculates the transformation of this TupleSE2 by applying the passed TupleSE2 to it. This can be interpreted as a coordinate transformation, first performing a rotation, then a translation.

Overrides:

multiply in class TupleSE2

Parameters:

other - the tuple to 'multiply' with

Returns:

the resulting transformation

approxEqualsMax

public boolean approxEqualsMax(TupleSE2 pose)

Indicates whether some other NavigationPose is "equal to" this one with a tolerance linear and angular tolerance of 1E-11 (with respect to the maximum norm).

Parameters:

pose - the reference object with which to compare.

Returns:

true if this pose is the same as the argument; false otherwise.

approxEqualsMax

public boolean approxEqualsMax(TupleSE2 pose,
                               double toleranceLinear,
                               double toleranceAngular)

Indicates whether some other NavigationPose is "equal to" this one with respect to the given tolerance (with respect to the maximum norm).

Parameters:

pose - the reference object with which to compare.

toleranceLinear - the tolerance allowed for the poses to be "equal in x and y coordinates"

toleranceAngular - the tolerance allowed for the poses to be "equal in the orientation"

Returns:

true if this pose is the same as the argument; false otherwise.

approxEqualsCart

public boolean approxEqualsCart(TupleSE2 pose)

Indicates whether the cartesian distance of the (x,y)-values and the distance of the angles of this pose and the given one are approximately equal with a tolerance of 1e-11.

Parameters:

pose - the other pose

Returns:

true, if the poses are approximately equal false, otherwise

approxEqualsCart

public boolean approxEqualsCart(TupleSE2 pose,
                                double toleranceLinear,
                                double toleranceAngular)

Indicates whether the cartesian distance of the (x,y)-values and the distance of the angles of this pose and the given one are approximately equal with the given translational and rotational tolerance.

Parameters:

pose - the other pose

toleranceLinear - the tolerance allowed for the distance of the positions

toleranceAngular - the tolerance allowed for the angles

Returns:

true, if the poses are approximately equal false, otherwise

distanceTrans

public double distanceTrans(TupleSE2 pose)

Calculates the euclidean square-norm of the robots pose to a given pose in the 2-dimensional phase space (x,y).

Parameters:

pose - the reference object with which to calculate the distance.

Returns:

Double distance to pose.

distanceRot

public double distanceRot(TupleSE2 pose)

Calculates the angular difference between this pose and the given one. The result is normalized to (0; π].

Parameters:

pose - the other pose

Returns:

the angular difference

toPosition

public Position toPosition()

Returns the position of this pose.

Returns:

the position

copy

public Pose copy()

Specified by:

copy in interface com.kuka.parameters.IParameter

Overrides:

copy in class XYTheta

toString

public String toString()

Overrides:

toString in class TupleSE2

parseNavigationPose

public static Pose parseNavigationPose(String poseString)

Parses a string and constructs a NavigationPose. The string can be of the form 'x,y,theta' or '', where x, y, and theta are floating-point numbers.

Parameters:

poseString - The string to be parsed

Returns:

The pose generated out of the string.

closestPose

public Pose closestPose(Pose[] poses)

Picks the closest pose from an array of poses, using the 2d distance. If there are several poses, that are equally close (considering the (x,y)-position, then the pose is picked where the orientation matches best.

Parameters:

poses - An array of poses to check against

Returns:

The closest pose out of the given poses. Returns null if poses was empty.

closestPose

public Pose closestPose(Pose[] poses,
                        Pose velocity)

Picks the closest pose from an array of poses, using the a distance weighted by the maximum velocities in x- y- and theta direction. This yields an estimate of the time that is necessary to reach a pose. The pose that can be reached in the shortest time is chosen.

Parameters:

poses - An array of poses to check against

velocity - An array of poses to check against

Returns:

The closest pose out of the given poses. Returns null if poses was empty.