klampt.model.coordinates module

A module to help manage coordinate frames and objects attached to them. Similar to the tf module in ROS.

You may attach points / vectors to frames and determine relative or world coordinates in a straightforward, object-oriented way.

The coordinates module is set up with a default coordinate manager so that if you call coordinates.[X], where [X] is a method of klampt.model.coordinates.Manager, such as setWorldModel(), addPoint(), addFrame(), etc., then the default Manager instance gets called.

Advanced users might create their own Manager, or swap top-level managers in/out using setManager().

class klampt.model.coordinates.Frame(name, worldCoordinates=([1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0]), parent=None, relativeCoordinates=None)[source]

Bases: object

Represents some coordinate frame in space.

name()[source]

Returns the name of this frame

Return type:: str

data()[source]: If any data is attached to this frame, returns it

worldOrigin()[source]

Returns an element of R^3 denoting the translation of the origin of this frame in world coordinates

Return type:: Sequence[float]

relativeOrigin()[source]

Returns an element of R^3 denoting the translation of the origin of this frame relative to its parent

Return type:: Sequence[float]

worldRotation()[source]

Returns an element of SO(3) denoting the rotation from this frame to world coordinates

Return type:: Sequence[float]

relativeRotation()[source]

Returns an element of SO(3) denoting the rotation from this frame to its parent

Return type:: Sequence[float]

worldCoordinates()[source]

Returns an element of SE(3) denoting the transform from this frame to world coordinates

Return type:: Tuple[Sequence[float], Sequence[float]]

relativeCoordinates()[source]

Returns an element of SE(3) denoting the transform from this frame to its parent

Return type:: Tuple[Sequence[float], Sequence[float]]

parent()[source]

Returns the parent of the frame, or None if it’s given in the world frame.

Return type:: Frame

class klampt.model.coordinates.Transform(source, destination=None)[source]

Bases: object

A transform from one Frame (source) to another (destination). The destination may be None, in which case the transform is the world transform of the source.

The difference between a Transform and a relative Frame (i.e., one with a parent) is that a Transform is a sort of “read-only” structure whose coordinates change as the frames’ coordinates change.

source()[source]

Returns the source Frame

Return type:: Frame

destination()[source]

Returns the source Frame

Return type:: Frame

coordinates()[source]

Returns the SE(3) coordinates that transform elements from the source to the destination Frame.

Return type:: Tuple[Sequence[float], Sequence[float]]

translationCoordinates()[source]

Returns the coordinates of the origin of this frame in R^3, relative to its destination

Return type:: Sequence[float]

rotationCoordinates()[source]

Returns the SO(3) coordinates that rotate elements from the source to the destination Frame

Return type:: Sequence[float]

toWorld()[source]

Returns a Transform designating the transformation from the source frame to the world frame.

Return type:: Transform

to(frame)[source]

Returns a Transform designating the transformation from the source frame to the given frame.

Return type:: Transform

class klampt.model.coordinates.Point(localCoordinates=[0, 0, 0], frame=None, name=None)[source]

Bases: object

Represents a point in 3D space. It is attached to a frame, so if the frame is changed then its world coordinates will also change.

localCoordinates()[source]

Returns the coordinates of this point in its parent Frame

Return type:: Sequence[float]

worldCoordinates()[source]

Returns the coordinates of this point in the world Frame

Return type:: Sequence[float]

frame()[source]

Returns the frame to which this Point is attached

Return type:: Frame

toWorld()[source]

Returns a Point representing the same point in space, but in the world reference frame

Return type:: Point

to(newframe)[source]

Returns a Point representing the same point in space, but in a different reference frame

Return type:: Point

localOffset(dir)[source]

Offsets this point by a vector in local coordinates

Return type:: None

worldOffset(dir)[source]

Offsets this point by a vector in world coordinates

Return type:: None

class klampt.model.coordinates.Direction(localCoordinates=[0, 0, 0], frame=None, name=None)[source]

Bases: object

Represents a directional quantity in 3D space. It is attached to a frame, so if the frame is rotated then its world coordinates will also change.

localCoordinates()[source]

Return type:: Sequence[float]

worldCoordinates()[source]

Return type:: Sequence[float]

frame()[source]

Return type:: Frame

toWorld()[source]

Returns a Direction representing the same direction in space, but in the world reference frame

Return type:: Direction

to(newframe)[source]

Returns a Direction representing the same direction in space, but in a different reference frame

Return type:: Direction

scale(amount)[source]

Scales this direction by a scalar amount

Return type:: None

localOffset(dir)[source]

Offsets this direction by a vector in local coordinates

Return type:: None

worldOffset(dir)[source]

Offsets this direction by a vector in world coordinates

Return type:: None

class klampt.model.coordinates.Group[source]

Bases: object

A collection of Frames, Points, Directions, and sub-Groups. All groups have a privileged frame called ‘root’. The default manager is a Group with a privileged frame called ‘world’ which is just an alias for ‘root’.

Subgroup items can be accessed using the syntax [group]:[itemname]. Subgroups can also be nested.

frames

a map from frame names to Frame objects

Type:: dict

childLists

a map from frame names to lists of children

Type:: dict

points

a map from point names to Point objects

Type:: dict

directions

a map from direction names to Direction objects

Type:: dict

subgroups

a map from subgroup names to Group objects

Type:: dict

rootFrame()[source]

Return type:: Optional[Frame]

destroy()[source]

Call this to destroy a group cleanly

Return type:: None

setWorldModel(worldModel)[source]

Sets this group to contain all entities of a world model

Return type:: None

setRobotModel(robotModel)[source]

Sets this group to contain all links of a robot model

Return type:: None

setController(controller)[source]

Given a SimRobotController, sets this group to contain all sensed and commanded frames.

Return type:: None

setSimBody(name, simBody)[source]

Sets this group to be attached to a simBody

Return type:: None

updateFromWorld()[source]

For any frames with associated world elements, updates the transforms from the world elements.

Return type:: None

updateToWorld()[source]

For any frames with associated world elements, updates the transforms of the world elements. Note: this does NOT perform inverse kinematics!

Return type:: None

addFrame(name, worldCoordinates=None, parent=None, relativeCoordinates=None)[source]

Adds a new named Frame, possibly with a parent. ‘parent’ may either be a string identifying another named Frame in this Group, or it can be a Frame object. (Warning: unknown behavior may result from specifying a Frame not in this Group).

Either worldCoordinates or relativeCoordinates must be given. If worldCoordinates is given, then the frame’s initial relative transform is determined by the current coordinates of the parent. If all parameters are left as default, the frame is placed directly at the origin of the parent

Return type:: Frame

addPoint(name, coordinates=[0, 0, 0], frame='root')[source]

Return type:: Point

addDirection(name, coordinates=[0, 0, 0], frame='root')[source]

Return type:: Direction

addGroup(name, group=None, parentFrame='root')[source]

Adds a subgroup to this group. If parentFrame is given, then the group is attached relative to the given frame. Otherwise, it is assumed attached to the root frame.

Return type:: Group

deleteFrame(name)[source]

Deletes the named frame. All items that refer to this frame will be automatically converted to be relative to the root coordinate system

Return type:: None

deletePoint(name)[source]

Return type:: None

deleteDirection(name)[source]

Return type:: None

deleteGroup(name)[source]

Return type:: None

setFrameCoordinates(name, coordinates, parent='relative')[source]

Sets the coordinates of the frame, given as an se3 element. The coordinates can be given either in ‘relative’ mode, where the coordinates are the natural coordinates of the frame relative to its parent, or in ‘world’ mode, where the coordinates are the global world coordinates, or they can be given relative to any other frame in this coordinate Group. If None, this defaults to the root frame of this Group.

Return type:: None

updateDependentFrames(frame)[source]

Whenever Frame’s world coordinates are updated, call this to update the downstream frames. This will be called automatically via setFrameCoordinates but not if you change a Frame’s coordinates manually.

Return type:: None

frame(name)[source]

Retrieves a named Frame.

Return type:: Frame

getPoint(name)[source]

Retrieves a named Point.

Return type:: Point

getDirection(name)[source]

Retrieves a named Direction.

Return type:: Direction

toWorld(object)[source]: Converts a Transform, Point, or Direction to have coordinates relative to the world frame.

to(object, frame)[source]: Converts a Transform, Point, or Direction to have coordinates relative to the given frame ‘frame’.

transform(sourceFrame, destFrame='root')[source]: Makes a Transform object from the source frame to the destination frame.

point(coordinates=[0, 0, 0], frame='root')[source]: Makes a Point object with the given local coordinates in the given frame. Does not add it to the list of managed points.

direction(coordinates=[0, 0, 0], frame='root')[source]: Makes a Direction object with the given local coordinates in the given frame. Does not add it to the list of managed points.

pointFromWorld(worldCoordinates=[0, 0, 0], frame='root')[source]: Alias for to(point(worldCoordinates,’root’),frame)

directionFromWorld(worldCoordinates=[0, 0, 0], frame='world')[source]: Alias for to(direction(worldCoordinates,’root’),frame)

listFrames(indent=0)[source]: Prints all the frames in this group and subgroups

listItems(indent=0)[source]: Prints all the items in this group

class klampt.model.coordinates.Manager[source]

Bases: Group

A manager of coordinate frames.

worldFrame()[source]

Return type:: Frame

destroy()[source]: Call this to destroy a group cleanly

deleteFrame(name)[source]: Deletes the named frame. All items that refer to this frame will be automatically converted to be relative to the root coordinate system

setFrameCoordinates(name, coordinates, parent='relative')[source]: Sets the coordinates of the frame, given as an se3 element. The coordinates can be given either in ‘relative’ mode, where the coordinates are the natural coordinates of the frame relative to its parent, or in ‘world’ mode, where the coordinates are the global world coordinates, or they can be given relative to any other frame in this coordinate Group. If None, this defaults to the root frame of this Group.

klampt.model.coordinates.manager()[source]

Retrieves the default top-level manager

Return type:: Manager

klampt.model.coordinates.setManager(manager)[source]

Sets the new top-level manager to a new Manager instance, and returns the old top-level manager.

Return type:: None

klampt.model.coordinates.destroy(*args, **kwargs)

klampt.model.coordinates.setWorldModel(*args, **kwargs)

klampt.model.coordinates.setRobotModel(*args, **kwargs)

klampt.model.coordinates.setController(*args, **kwargs)

klampt.model.coordinates.setSimBody(*args, **kwargs)

klampt.model.coordinates.updateFromWorld(*args, **kwargs)

klampt.model.coordinates.updateToWorld(*args, **kwargs)

klampt.model.coordinates.addFrame(*args, **kwargs)

klampt.model.coordinates.addPoint(*args, **kwargs)

klampt.model.coordinates.addDirection(*args, **kwargs)

klampt.model.coordinates.addGroup(*args, **kwargs)

klampt.model.coordinates.deleteFrame(*args, **kwargs)

klampt.model.coordinates.deletePoint(*args, **kwargs)

klampt.model.coordinates.deleteDirection(*args, **kwargs)

klampt.model.coordinates.deleteGroup(*args, **kwargs)

klampt.model.coordinates.setFrameCoordinates(*args, **kwargs)

klampt.model.coordinates.frame(*args, **kwargs)

klampt.model.coordinates.getPoint(*args, **kwargs)

klampt.model.coordinates.getDirection(*args, **kwargs)

klampt.model.coordinates.toWorld(*args, **kwargs)

klampt.model.coordinates.to(*args, **kwargs)

klampt.model.coordinates.transform(*args, **kwargs)

klampt.model.coordinates.point(*args, **kwargs)

klampt.model.coordinates.direction(*args, **kwargs)

klampt.model.coordinates.pointFromWorld(*args, **kwargs)

klampt.model.coordinates.directionFromWorld(*args, **kwargs)

klampt.model.coordinates.listFrames(*args, **kwargs)

klampt.model.coordinates.listItems(*args, **kwargs)

klampt.model.coordinates.ik_objective(obj, target)[source]

Returns an IK objective that attempts to fix the given klampt.coordinates object ‘obj’ at given target object ‘target’.

Return type:

IKObjective

Parameters:

obj – An instance of one of the {Point,Direction,Transform,Frame} classes.
target – If ‘obj’ is a Point, Direction, or Frame objects, this must be an object of the same type of ‘obj’ denoting the target to which ‘obj’ should be fixed. In other words, the local coordinates of ‘obj’ relative to ‘target’s parent frame will be equal to ‘target’s local coordinates. If obj is a Transform object, this element is an se3 object.

Returns:

An IK objective to be used with the klampt.ik module.

Since the klampt.ik module is not aware about custom frames, an ancestor of the object must be attached to a RobotModelLink or a RigidObjectModel, or else None will be returned. The same goes for target, if provided.

TODO: support lists of objects to fix.

TODO: support Direction constraints.

klampt.model.coordinates.ik_fixed_objective(obj, ref=None)[source]

Returns an IK objective that attempts to fix the given klampt.coordinates object at its current pose. If ref=None, its pose is fixed in world coordinates. Otherwise, its pose is fixed relative to the reference frame ref.

Parameters:

obj – An instance of one of the {Point,Direction,Transform,Frame} classes.
ref (optional) – either None, or a Frame object denoting the reference frame to which the object should be fixed. (If obj is a Transform object, its destination frame is used as the reference frame, and this argument is ignored.)

Returns:

An IK objective to be used with the klampt.ik module. For Point, Direction, and Frame objects this objective fixes the object coordinates relative to the ref frame, or the world if None frame is provided. For Transform objects the source frame is fixed relative to the destination frame.

Return type:

IKObjective

Since the klampt.ik module is not aware about custom frames, an ancestor of the object must be attached to a RobotModelLink or a RigidObjectModel, or else None will be returned. The same goes for ref, if provided.

TODO: support lists of objects to fix.

TODO: support Direction constraints.