robots.poses package

robots.poses.position module

class robots.poses.position.FrameProvider

Bases: object

get_transform(frame)

Returns the transformation between this frame and the map.

If the frame is unknown, raises UnknownFrameError.

exception robots.poses.position.InvalidFrameError

Bases: exceptions.RuntimeError

class robots.poses.position.PoseManager(robot)

Bases: object

A pose is for us a dict {'x':x, 'y':y, 'z':z, 'qx':qx, 'qy':qy, 'qz':qz, 'qw':qw, 'frame':frame}, ie a (x, y, z) cartesian pose in meter interpreted in a specific reference frame, and a quaternion describing the orientation of the object in radians.

This class helps with:

• converting from other convention to our convention,
• converting back to other conventions.

add_frame_provider(provider)

angular_distance(angle1, angle2)

Returns the (minimal, oriented) angular distance between two angles after normalization on the unit circle.

Angles are assumed to be radians.

The result is oriented (from angle1 to angle2) and guaranteed to be in range ]-pi, pi].

distance(pose1, pose2='base_link')

Returns the euclidian distance between two pyRobots poses.

If the second pose is omitted, “base_link” is assumed (ie, distance between a pose and the robot itself).

euler(pose)

get(raw)

takes a loosly defined ‘pose’ as input and returns a properly formatted and normalized pose.

Input may be:

• a frame
• an incomplete pose dictionary
• a list or tuple (x,y,z), (x,y,z,frame) or (z,y,z,rx,ry,rz) or (x,y,z,qx,qy,qz,qw)

inframe(pose, frame)

Transform a pose from one frame to another one.

Uses transformation matrices. Could be refactored to use directly quaternions.

static isin(point, polygon)

Determines if a 2D point is inside a given 2D polygon or not.

Parameters:

• point – a (x,y) pair
• polygon – a list of (x,y) pairs.

Copied from: http://www.ariel.com.au/a/python-point-int-poly.html

myself()

Returns the current robot’s pose, ie the pose of the ROS TF ‘base_link’ frame.

normalize(pose)

static normalize_angle(angle)

Returns equivalent angle such as -pi < angle <= pi

static normalizedict(pose)

normalizelist(pose)

pantilt(pose, ref='/base_link')

Convert a xyz target to pan and tilt angles from a given viewpoint.

Parameters:

• pose – the target pose
• ref – the reference frame (default to base_link)

Returns:

(pan, tilt) in radians, in ]-pi, pi]

static quaternion_from_euler(rx, ry, rz)

test_angular_distance()

Small regression test for the computation of angular distances

exception robots.poses.position.UnknownFrameError

Bases: exceptions.RuntimeError

robots.poses.ros_positions module

class robots.poses.ros_positions.ROSFrames

Bases: robots.poses.position.FrameProvider

asROSpose(pose)

Returns a ROS PoseStamped from a pyRobots pose.

Parameters:pose – a standard pyRobots pose (SPARK id, TF frame, [x,y,z], [x,y,z,rx,ry,rz], [x,y,z,qx,qy,qw,qz], {‘x’:..., ‘y’:...,...}) Returns:the corresponding ROS PoseStamped

get_transform(frame)

inframe(pose, frame)

Transforms a given pose in the given frame.

publish_transform(name, pose)

Publishes a new TF frame called ‘name’ based on the pyRobots transform ‘pose’.

Note that this function does not normalize the input pose, which must already be a dictionary with the keys [x,y,z,qx,qy,qz,qw,frame].