# klampt.robotsim (core classes) module¶

The robotsim module contains all of the core classes and functions from the C++ API. These are imported into the main klampt namespace.

Note: The C++ API is converted from SWIG, so the documentation may be a little rough. The first lines of the documentation for overloaded SWIG functions may describe the signature for each function overload. For example, klampt.WorldModel.add() contains the listing:

add (name,robot): RobotModel

Parameters: * name (str) –
* robot (RobotModel, optional) –
* obj (RigidObjectModel, optional) –
* terrain (TerrainModel, optional) –


The colon followed by a type descriptor, : Type, gives the type of the return value. This means that if the second argument is a RobotModel, the first overload is matched, and the return value is a klampt.RobotModel.

## Modeling robots and worlds¶

Imported into the main klampt package.

 WorldModel(*args) The main world class, containing robots, rigid objects, and static environment geometry. A model of a dynamic and kinematic robot. A reference to a link of a RobotModel. RobotModelDriver() A reference to a driver of a RobotModel. A rigid movable object. Static environment geometry. Stores mass information for a rigid body or robot link. Stores contact parameters for an entity. setRandomSeed(seed) Sets the random seed used by the configuration sampler. destroy() destroys internal data structures

## Modeling geometries¶

Imported into the main klampt package.

 Geometry3D(*args) The three-D geometry container used throughout Klampt. Appearance(*args) Geometry appearance information. A geometric primitive. A 3D indexed triangle mesh class. A 3D point cloud class. An axis-aligned volumetric grid, typically a signed distance transform with > 0 indicating outside and < 0 indicating inside. Stores a set of points to be set into a ConvexHull type. Configures the _ext distance queries of Geometry3D. The result from a “fancy” distance query of Geometry3D. The result from a contact query of Geometry3D.

## Inverse kinematics¶

Imported into the main klampt package.

 IKObjective(*args) A class defining an inverse kinematic target. IKSolver(*args) An inverse kinematics solver based on the Newton-Raphson technique. An inverse kinematics target for matching points between two robots and/or objects. An inverse kinematics solver between multiple robots and/or objects. SampleTransform(*args) Returns a transformation (R,t) from link relative to link2, sampled at random from the space of transforms that satisfies the objective obj.

## Simulation¶

Imported into the main klampt package.

 Simulator(model) A dynamics simulator for a WorldModel. A reference to a rigid body inside a Simulator (either a RigidObjectModel, TerrainModel, or a link of a RobotModel). An interface to ODE’s hinge and slider joints. A controller for a simulated robot. SimRobotSensor(*args) A sensor on a simulated robot.

## Equilibrium testing¶

 comEquilibrium(*args) Tests whether the given COM com is stable for the given contacts and the given external force fext. comEquilibrium2D(*args) Tests whether the given COM com is stable for the given contacts and the given external force fext. equilibriumTorques(*args) Solves for the torques / forces that keep the robot balanced against gravity. forceClosure(*args) Returns true if the list of contact points has force closure. forceClosure2D(*args) Returns true if the list of 2D contact points has force closure. Globally sets the number of edges used in the friction cone approximation. supportPolygon(*args) Calculates the support polygon for a given set of contacts and a downward external force (0,0,-g). supportPolygon2D(*args) Calculates the support polygon (interval) for a given set of contacts and a downward external force (0,-g).

## Input/Output¶

Imported into the klampt.io package

 SubscribeToStream(*args) Subscribes a Geometry3D to a stream. DetachFromStream(protocol, name) Unsubscribes from a stream previously subscribed to via SubscribeToStream() ProcessStreams(*args) Does some processing on stream subscriptions. WaitForStream(protocol, name, timeout) Waits up to timeout seconds for an update on the given stream. Exports the WorldModel to a JSON string ready for use in Three.js. Exports the WorldModel to a JSON string ready for use in Three.js.

## Visualization¶

For use in GLWidgetPlugin.

 ObjectPoser(object) param object RobotPoser(robot) param robot

## Module contents¶

Classes:

 AABBPoser() Appearance(*args) Geometry appearance information. BoxPoser() Stores contact parameters for an entity. The result from a contact query of Geometry3D. Stores a set of points to be set into a ConvexHull type. The result from a “fancy” distance query of Geometry3D. Configures the _ext distance queries of Geometry3D. An inverse kinematics target for matching points between two robots and/or objects. An inverse kinematics solver between multiple robots and/or objects. A geometric primitive. Geometry3D(*args) The three-D geometry container used throughout Klampt. IKObjective(*args) A class defining an inverse kinematic target. IKSolver(*args) An inverse kinematics solver based on the Newton-Raphson technique. Stores mass information for a rigid body or robot link. ObjectPoser(object) param object A 3D point cloud class. PointPoser() A rigid movable object. A model of a dynamic and kinematic robot. RobotModelDriver() A reference to a driver of a RobotModel. A reference to a link of a RobotModel. RobotPoser(robot) param robot A reference to a rigid body inside a Simulator (either a RigidObjectModel, TerrainModel, or a link of a RobotModel). An interface to ODE’s hinge and slider joints. A controller for a simulated robot. SimRobotSensor(*args) A sensor on a simulated robot. Simulator(model) A dynamics simulator for a WorldModel. SpherePoser() SwigPyIterator(*args, **kwargs) Static environment geometry. TransformPoser() A 3D indexed triangle mesh class. Viewport() An axis-aligned volumetric grid, typically a signed distance transform with > 0 indicating outside and < 0 indicating inside. Widget() WidgetSet() WorldModel(*args) The main world class, containing robots, rigid objects, and static environment geometry. stringMap(*args)

Functions:

 AABBPoser_swigregister Appearance_swigregister BoxPoser_swigregister ContactParameters_swigregister ContactQueryResult_swigregister ConvexHull_swigregister DetachFromStream(protocol, name) Unsubscribes from a stream previously subscribed to via SubscribeToStream() DistanceQueryResult_swigregister DistanceQuerySettings_swigregister GeneralizedIKObjective_swigregister GeneralizedIKSolver_swigregister GeometricPrimitive_swigregister Geometry3D_swigregister IKObjective_swigregister IKSolver_swigregister Mass_swigregister ObjectPoser_swigregister PointCloud_swigregister PointPoser_swigregister ProcessStreams(*args) Does some processing on stream subscriptions. RigidObjectModel_swigregister RobotModelDriver_swigregister RobotModelLink_swigregister RobotModel_swigregister RobotPoser_swigregister SampleTransform(*args) Returns a transformation (R,t) from link relative to link2, sampled at random from the space of transforms that satisfies the objective obj. SimBody_swigregister SimJoint_swigregister SimRobotController_swigregister SimRobotSensor_swigregister Simulator_swigregister SpherePoser_swigregister SubscribeToStream(*args) Subscribes a Geometry3D to a stream. SwigPyIterator_swigregister TerrainModel_swigregister ThreeJSGetScene(arg1) Exports the WorldModel to a JSON string ready for use in Three.js. ThreeJSGetTransforms(arg1) Exports the WorldModel to a JSON string ready for use in Three.js. TransformPoser_swigregister TriangleMesh_swigregister Viewport_swigregister VolumeGrid_swigregister WaitForStream(protocol, name, timeout) Waits up to timeout seconds for an update on the given stream. WidgetSet_swigregister Widget_swigregister WorldModel_swigregister comEquilibrium(*args) Tests whether the given COM com is stable for the given contacts and the given external force fext. comEquilibrium2D(*args) Tests whether the given COM com is stable for the given contacts and the given external force fext. destroy() destroys internal data structures doubleArray_frompointer doubleArray_swigregister doubleMatrix_swigregister doubleVector_swigregister equilibriumTorques(*args) Solves for the torques / forces that keep the robot balanced against gravity. floatArray_frompointer floatArray_swigregister floatVector_swigregister forceClosure(*args) Returns true if the list of contact points has force closure. forceClosure2D(*args) Returns true if the list of 2D contact points has force closure. intArray_frompointer intArray_swigregister intVector_swigregister setFrictionConeApproximationEdges(numEdges) Globally sets the number of edges used in the friction cone approximation. setRandomSeed(seed) Sets the random seed used by the configuration sampler. stringMap_swigregister stringVector_swigregister supportPolygon(*args) Calculates the support polygon for a given set of contacts and a downward external force (0,0,-g). supportPolygon2D(*args) Calculates the support polygon (interval) for a given set of contacts and a downward external force (0,-g).
class klampt.Appearance(*args)[source]

Bases: object

Geometry appearance information. Supports vertex/edge/face rendering, per-vertex color, and basic color texture mapping. Uses OpenGL display lists, so repeated calls are fast.

For more complex appearances, you will need to call your own OpenGL calls.

Appearances can be either references to appearances of objects in the world, or they can be standalone.

Performance note: Avoid rebuilding buffers (e.g., via refresh() as much as possible.

C++ includes: appearance.h

__init__ (): Appearance

__init__ (app): Appearance

Parameters

app (Appearance, optional) –

Attributes:

 ALL EDGES EMISSIVE FACES SPECULAR VERTICES appearancePtr Appearance_appearancePtr_get(Appearance self) -> void * id Appearance_id_get(Appearance self) -> int world Appearance_world_get(Appearance self) -> int

Methods:

 Creates a standalone appearance from this appearance. drawGL(*args) Draws the given geometry with this appearance. drawWorldGL(geom) Draws the given geometry with this appearance. Frees the data associated with this appearance, if standalone. getColor(*args) Gets color of the object or a feature. getDraw(*args) Returns whether this object or feature is visible. getElementColor(feature, element) Gets the per-element color for the given feature. Retrieves the specular highlight shininess. Returns true if this is a standalone appearance. refresh([deep]) call this to rebuild internal buffers, e.g., when the OpenGL context changes. set(arg2) Copies the appearance of the argument into this appearance. setColor(*args) Sets color of the object or a feature. setColors(feature, colors[, alpha]) Sets per-element color for elements of the given feature type. setCreaseAngle(creaseAngleRads) For meshes, sets the crease angle. setDraw(*args) Turns on/off visibility of the object or a feature. setElementColor(feature, element, r, g, b[, a]) Sets the per-element color for the given feature. setPointSize(size) For point clouds, sets the point size. setShininess(shininess[, strength]) Sets the specular highlight shininess and strength. setSilhouette(radius[, r, g, b, a]) For meshes sets a silhouette radius and color. Sets per-vertex texture coordinates. setTexture1D(w, format, bytes) Sets a 1D texture of the given width. setTexture2D(w, h, format, bytes[, topdown]) Sets a 2D texture of the given width/height.
ALL = 0
EDGES = 2
EMISSIVE = 4
FACES = 3
SPECULAR = 5
VERTICES = 1
property appearancePtr

Appearance_appearancePtr_get(Appearance self) -> void *

clone()[source]

Creates a standalone appearance from this appearance.

Returns

Return type

Appearance

drawGL(*args)[source]

Draws the given geometry with this appearance. NOTE: for best performance, an appearance should only be drawn with a single geometry. Otherwise, the OpenGL display lists will be completely recreated.

drawGL ()

drawGL (geom)

Parameters

geom (Geometry3D, optional) –

Note that the geometry’s current transform is NOT respected, and this only draws the geometry in its local transform.

drawWorldGL(geom)[source]

Draws the given geometry with this appearance. NOTE: for best performance, an appearance should only be drawn with a single geometry. Otherwise, the OpenGL display lists will be completely recreated.

Parameters

geom (Geometry3D) –

Differs from drawGL in that the geometry’s current transform is applied before drawing.

free()[source]

Frees the data associated with this appearance, if standalone.

getColor(*args)[source]

Gets color of the object or a feature.

getColor ()

getColor (feature)

Parameters

feature (int, optional) –

If 0 arguments are given, retrieves the main object color.

If 1 arguments are given, returns the color of the given feature. feature. feature can be ALL, VERTICES, EDGES, FACES, EMISSIVE, or SPECULAR.

getDraw(*args)[source]

Returns whether this object or feature is visible.

getDraw (): bool

getDraw (feature): bool

Parameters

feature (int, optional) –

Returns

Return type

(bool)

If no arguments are given, returns whether the object is visible.

If one int argument is given, returns whether the given feature is visible. feature can be ALL, VERTICES, EDGES, or FACES.

getElementColor(feature, element)[source]

Gets the per-element color for the given feature.

Parameters
• feature (int) –

• element (int) –

getShininess()[source]

Retrieves the specular highlight shininess.

Returns

Return type

float

property id

Appearance_id_get(Appearance self) -> int

isStandalone()[source]

Returns true if this is a standalone appearance.

Returns

Return type

bool

refresh(deep=True)[source]

call this to rebuild internal buffers, e.g., when the OpenGL context changes. If deep=True, the entire data structure will be revised. Use this for streaming data, for example.

refresh (deep=True)

refresh ()

Parameters

deep (bool, optional) – default value True

set(arg2)[source]

Copies the appearance of the argument into this appearance.

Parameters

arg2 (Appearance) –

setColor(*args)[source]

Sets color of the object or a feature.

setColor (r,g,b,a=1)

setColor (r,g,b)

setColor (feature,r,g,b,a)

Parameters
• r (float) –

• g (float) –

• b (float) –

• a (float, optional) – default value 1

• feature (int, optional) –

If 3 or 4 arguments are given, changes the object color.

If 5 arguments are given, changes the color of the given feature. feature can be ALL, VERTICES, EDGES, FACES, EMISSIVE, or SPECULAR.

setColors(feature, colors, alpha=False)[source]

Sets per-element color for elements of the given feature type.

setColors (feature,colors,alpha=False)

setColors (feature,colors)

Parameters
• feature (int) –

• colors (list of floats) –

• alpha (bool, optional) – default value False

If alpha=True, colors are assumed to be 4*N rgba values, where N is the number of features of that type.

Otherwise they are assumed to be 3*N rgb values. Only supports feature=VERTICES and feature=FACES

setCreaseAngle(creaseAngleRads)[source]

For meshes, sets the crease angle. Set to 0 to disable smoothing.

Parameters

setDraw(*args)[source]

Turns on/off visibility of the object or a feature.

setDraw (draw)

setDraw (feature,draw)

Parameters
• draw (bool) –

• feature (int, optional) –

If one argument is given, turns the object visibility on or off

If two arguments are given, turns the feature (first int argument) visibility on or off. feature can be ALL, VERTICES, EDGES, or FACES.

setElementColor(feature, element, r, g, b, a=1)[source]

Sets the per-element color for the given feature.

setElementColor (feature,element,r,g,b,a=1)

setElementColor (feature,element,r,g,b)

Parameters
• feature (int) –

• element (int) –

• r (float) –

• g (float) –

• b (float) –

• a (float, optional) – default value 1

setPointSize(size)[source]

For point clouds, sets the point size.

Parameters

size (float) –

setShininess(shininess, strength=- 1)[source]

Sets the specular highlight shininess and strength. To turn off, use setShininess(0). The specular strength can be set via the second argument. setShininess(20,0.1). Note that this changes the specular color.

setShininess (shininess,strength=-1)

setShininess (shininess)

Parameters
• shininess (float) –

• strength (float, optional) – default value -1

setSilhouette(radius, r=0, g=0, b=0, a=1)[source]

For meshes sets a silhouette radius and color. Set the radius to 0 to disable silhouette drawing.

Parameters

• r (float, optional) – default value 0

• g (float, optional) – default value 0

• b (float, optional) – default value 0

• a (float, optional) – default value 1

setTexcoords(uvs)[source]

Sets per-vertex texture coordinates.

Parameters

uvs (list of floats) –

If the texture is 1D, uvs is an array of length n containing 1D texture coordinates.

If the texture is 2D, uvs is an array of length 2n containing U-V coordinates u1, v1, u2, v2, …, un, vn.

You may also set uvs to be empty, which turns off texture mapping altogether.

setTexture1D(w, format, bytes)[source]

Sets a 1D texture of the given width. Valid format strings are.

Parameters
• w (int) –

• format (str) –

• char (std::vector< unsigned) –

• bytes (std::allocator) –

• “”: turn off texture mapping

• rgb8: unsigned byte RGB colors with red in the 1st byte, green in the 2nd, blue in the 3rd

• bgr8: unsigned byte RGB colors with blue in the 1st byte, green in the 2nd, green in the 3rd

• rgba8: unsigned byte RGBA colors with red in the 1st byte and alpha in the 4th

• bgra8: unsigned byte RGBA colors with blue in the 1st byte and alpha in the 4th

• l8: unsigned byte grayscale colors

setTexture2D(w, h, format, bytes, topdown=True)[source]

Sets a 2D texture of the given width/height. See setTexture1D() for valid format strings.

setTexture2D (w,h,format,char,bytes,topdown=True)

setTexture2D (w,h,format,char,bytes)

Parameters
• w (int) –

• h (int) –

• format (str) –

• char (std::vector< unsigned) –

• bytes (std::allocator) –

• topdown (bool, optional) – default value True

bytes is is given in order left to right, top to bottom if topdown==True. Otherwise, it is given in order left to right, bottom to top.

property world

Appearance_world_get(Appearance self) -> int

class klampt.ContactParameters[source]

Bases: object

Stores contact parameters for an entity. Currently only used for simulation, but could be used for contact mechanics in the future.

kFriction

The coefficient of (Coulomb) friction, in range [0,inf).

Type

float

kRestitution

The coefficient of restitution, in range [0,1].

Type

float

kStiffness

The stiffness of the material, in range (0,inf) (default inf, perfectly rigid).

Type

float

kDamping

The damping of the material, in range (0,inf) (default inf, perfectly rigid).

Type

float

C++ includes: robotmodel.h

Attributes:

 kDamping ContactParameters_kDamping_get(ContactParameters self) -> double kFriction ContactParameters_kFriction_get(ContactParameters self) -> double kRestitution ContactParameters_kRestitution_get(ContactParameters self) -> double kStiffness ContactParameters_kStiffness_get(ContactParameters self) -> double
property kDamping

ContactParameters_kDamping_get(ContactParameters self) -> double

property kFriction

ContactParameters_kFriction_get(ContactParameters self) -> double

property kRestitution

ContactParameters_kRestitution_get(ContactParameters self) -> double

property kStiffness

ContactParameters_kStiffness_get(ContactParameters self) -> double

class klampt.ContactQueryResult[source]

Bases: object

The result from a contact query of Geometry3D. The number of contacts n is variable.

depths

penetration depths for each contact point. The depth is measured with respect to the padded geometry, and must be nonnegative. A value of 0 indicates that depth cannot be determined accurately.

Type

list of n floats

points1, points2

contact points on self vs other, The top level list has n entries, and each entry is a 3-list expressed in world coordinates. If an object is padded, these points are on the surface of the padded geometry.

Type

list of n lists of floats

normals

the outward-facing contact normal from this to other at each contact point, given in world coordinates. Each entry is a 3-list, and can be a unit vector, or [0,0,0] if the the normal cannot be computed properly.

Type

list of n lists of floats

elems1, elems2

for compound objects, these are the element indices corresponding to each contact.

Type

list of n ints

C++ includes: geometry.h

Attributes:

 depths ContactQueryResult_depths_get(ContactQueryResult self) -> doubleVector elems1 ContactQueryResult_elems1_get(ContactQueryResult self) -> intVector elems2 ContactQueryResult_elems2_get(ContactQueryResult self) -> intVector normals ContactQueryResult_normals_get(ContactQueryResult self) -> doubleMatrix points1 ContactQueryResult_points1_get(ContactQueryResult self) -> doubleMatrix points2 ContactQueryResult_points2_get(ContactQueryResult self) -> doubleMatrix
property depths

ContactQueryResult_depths_get(ContactQueryResult self) -> doubleVector

property elems1

ContactQueryResult_elems1_get(ContactQueryResult self) -> intVector

property elems2

ContactQueryResult_elems2_get(ContactQueryResult self) -> intVector

property normals

ContactQueryResult_normals_get(ContactQueryResult self) -> doubleMatrix

property points1

ContactQueryResult_points1_get(ContactQueryResult self) -> doubleMatrix

property points2

ContactQueryResult_points2_get(ContactQueryResult self) -> doubleMatrix

class klampt.ConvexHull[source]

Bases: object

Stores a set of points to be set into a ConvexHull type. Note: These may not actually be the vertices of the convex hull; the actual convex hull may be computed internally for some datatypes.

points

a list of points, given as a flattened coordinate list [x1,y1,z1,x2,y2,…]

Type

SWIG vector of floats

C++ includes: geometry.h

Methods:

 Adds a point. getPoint(index) Retrieves a point. Returns the # of points. transform(R, t) Transforms all the vertices by the rigid transform v=R*v+t. Translates all the vertices by v=v+t.

Attributes:

 points ConvexHull_points_get(ConvexHull self) -> doubleVector
addPoint(pt)[source]

Parameters

pt (list of 3 floats) –

getPoint(index)[source]

Retrieves a point.

Parameters

index (int) –

numPoints()[source]

Returns the # of points.

Returns

Return type

int

property points

ConvexHull_points_get(ConvexHull self) -> doubleVector

transform(R, t)[source]

Transforms all the vertices by the rigid transform v=R*v+t.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

translate(t)[source]

Translates all the vertices by v=v+t.

Parameters

t (list of 3 floats) –

class klampt.DistanceQueryResult[source]

Bases: object

The result from a “fancy” distance query of Geometry3D.

d

The calculated distance, with negative values indicating penetration. Can also be upperBound if the branch was hit.

Type

float

hasClosestPoints

If true, the closest point information is given in cp0 and cp1, and elem1 and elem2

Type

bool

hasGradients

Type

bool

cp1, cp2

closest points on self vs other, both given in world coordinates

Type

list of 3 floats, optional

grad1, grad2

the gradients of the objects’ signed distance fields at the closest points. Given in world coordinates.

I.e., to move object1 to touch object2, move it in direction grad1 by distance -d. Note that grad2 is always -grad1.

Type

list of 3 floats, optional

elems1, elems2

for compound objects, these are the element indices corresponding to the closest points.

Type

int

C++ includes: geometry.h

Attributes:

 cp1 DistanceQueryResult_cp1_get(DistanceQueryResult self) -> doubleVector cp2 DistanceQueryResult_cp2_get(DistanceQueryResult self) -> doubleVector d DistanceQueryResult_d_get(DistanceQueryResult self) -> double elem1 DistanceQueryResult_elem1_get(DistanceQueryResult self) -> int elem2 DistanceQueryResult_elem2_get(DistanceQueryResult self) -> int grad1 DistanceQueryResult_grad1_get(DistanceQueryResult self) -> doubleVector grad2 DistanceQueryResult_grad2_get(DistanceQueryResult self) -> doubleVector hasClosestPoints DistanceQueryResult_hasClosestPoints_get(DistanceQueryResult self) -> bool hasGradients DistanceQueryResult_hasGradients_get(DistanceQueryResult self) -> bool
property cp1

DistanceQueryResult_cp1_get(DistanceQueryResult self) -> doubleVector

property cp2

DistanceQueryResult_cp2_get(DistanceQueryResult self) -> doubleVector

property d

DistanceQueryResult_d_get(DistanceQueryResult self) -> double

property elem1

DistanceQueryResult_elem1_get(DistanceQueryResult self) -> int

property elem2

DistanceQueryResult_elem2_get(DistanceQueryResult self) -> int

property grad1

property grad2

property hasClosestPoints

DistanceQueryResult_hasClosestPoints_get(DistanceQueryResult self) -> bool

property hasGradients

class klampt.DistanceQuerySettings[source]

Bases: object

Configures the _ext distance queries of Geometry3D.

The calculated result satisfies $$Dcalc \leq D(1+relErr) + absErr$$ unless $$D \geq upperBound$$, in which case Dcalc=upperBound may be returned.

relErr

Allows a relative error in the reported distance to speed up computation. Default 0.

Type

float, optional

absErr

Allows an absolute error in the reported distance to speed up computation. Default 0.

Type

float, optional

upperBound

The calculation may branch if D exceeds this bound.

Type

float, optional

C++ includes: geometry.h

Attributes:

 absErr DistanceQuerySettings_absErr_get(DistanceQuerySettings self) -> double relErr DistanceQuerySettings_relErr_get(DistanceQuerySettings self) -> double upperBound DistanceQuerySettings_upperBound_get(DistanceQuerySettings self) -> double
property absErr

DistanceQuerySettings_absErr_get(DistanceQuerySettings self) -> double

property relErr

DistanceQuerySettings_relErr_get(DistanceQuerySettings self) -> double

property upperBound

DistanceQuerySettings_upperBound_get(DistanceQuerySettings self) -> double

class klampt.GeneralizedIKObjective(*args)[source]

Bases: object

An inverse kinematics target for matching points between two robots and/or objects.

The objects are chosen upon construction, so the following are valid:

• GeneralizedIKObjective(a) is an objective for object a to be constrained relative to the environment.

• GeneralizedIKObjective(a,b) is an objective for object a to be constrained relative to b. Here a and b can be links on any robot or rigid objects.

Once constructed, call setPoint, setPoints, or setTransform to specify the nature of the constraint.

C++ includes: robotik.h

__init__ (obj): GeneralizedIKObjective

__init__ (link): GeneralizedIKObjective

__init__ (link,link2): GeneralizedIKObjective

__init__ (link,obj2): GeneralizedIKObjective

__init__ (obj,link2): GeneralizedIKObjective

__init__ (obj,obj2): GeneralizedIKObjective

Parameters

Attributes:

 goal GeneralizedIKObjective_goal_get(GeneralizedIKObjective self) -> IKGoal isObj1 GeneralizedIKObjective_isObj1_get(GeneralizedIKObjective self) -> bool isObj2 GeneralizedIKObjective_isObj2_get(GeneralizedIKObjective self) -> bool link1 GeneralizedIKObjective_link1_get(GeneralizedIKObjective self) -> RobotModelLink link2 GeneralizedIKObjective_link2_get(GeneralizedIKObjective self) -> RobotModelLink obj1 GeneralizedIKObjective_obj1_get(GeneralizedIKObjective self) -> RigidObjectModel obj2 GeneralizedIKObjective_obj2_get(GeneralizedIKObjective self) -> RigidObjectModel

Methods:

 setPoint(p1, p2) param p1 setPoints(p1s, p2s) param p1s setTransform(R, t) param R
property goal

GeneralizedIKObjective_goal_get(GeneralizedIKObjective self) -> IKGoal

property isObj1

GeneralizedIKObjective_isObj1_get(GeneralizedIKObjective self) -> bool

property isObj2

GeneralizedIKObjective_isObj2_get(GeneralizedIKObjective self) -> bool

property link1

property link2

property obj1

GeneralizedIKObjective_obj1_get(GeneralizedIKObjective self) -> RigidObjectModel

property obj2

GeneralizedIKObjective_obj2_get(GeneralizedIKObjective self) -> RigidObjectModel

setPoint(p1, p2)[source]
Parameters
• p1 (list of 3 floats) –

• p2 (list of 3 floats) –

setPoints(p1s, p2s)[source]
Parameters
• p1s (object) –

• p2s (object) –

setTransform(R, t)[source]
Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

class klampt.GeneralizedIKSolver(world)[source]

Bases: object

An inverse kinematics solver between multiple robots and/or objects. NOT IMPLEMENTED YET.

C++ includes: robotik.h

Parameters

world (WorldModel) –

Methods:

 add(objective) Adds a new simultaneous objective. Returns a matrix describing the instantaneous derivative of the objective with respect to the active parameters. Returns a vector describing the error of the objective. Samples an initial random configuration. setMaxIters(iters) Sets the max # of iterations (default 100) Sets the constraint solve tolerance (default 1e-3) Tries to find a configuration that satifies all simultaneous objectives up to the desired tolerance.

Attributes:

 maxIters GeneralizedIKSolver_maxIters_get(GeneralizedIKSolver self) -> int objectives GeneralizedIKSolver_objectives_get(GeneralizedIKSolver self) -> std::vector< GeneralizedIKObjective,std::allocator< GeneralizedIKObjective > > * tol GeneralizedIKSolver_tol_get(GeneralizedIKSolver self) -> double useJointLimits GeneralizedIKSolver_useJointLimits_get(GeneralizedIKSolver self) -> bool world GeneralizedIKSolver_world_get(GeneralizedIKSolver self) -> WorldModel
add(objective)[source]

Parameters

objective (GeneralizedIKObjective) –

getJacobian()[source]

Returns a matrix describing the instantaneous derivative of the objective with respect to the active parameters.

getResidual()[source]

Returns a vector describing the error of the objective.

property maxIters

GeneralizedIKSolver_maxIters_get(GeneralizedIKSolver self) -> int

property objectives

GeneralizedIKSolver_objectives_get(GeneralizedIKSolver self) -> std::vector< GeneralizedIKObjective,std::allocator< GeneralizedIKObjective > > *

sampleInitial()[source]

Samples an initial random configuration.

setMaxIters(iters)[source]

Sets the max # of iterations (default 100)

Parameters

iters (int) –

setTolerance(res)[source]

Sets the constraint solve tolerance (default 1e-3)

Parameters

res (float) –

solve()[source]

Tries to find a configuration that satifies all simultaneous objectives up to the desired tolerance.

Returns: res,iters (pair of bool, int): res indicates whether x converged, and iters is the number of iterations used.

property tol

GeneralizedIKSolver_tol_get(GeneralizedIKSolver self) -> double

property useJointLimits

GeneralizedIKSolver_useJointLimits_get(GeneralizedIKSolver self) -> bool

property world

GeneralizedIKSolver_world_get(GeneralizedIKSolver self) -> WorldModel

class klampt.GeometricPrimitive[source]

Bases: object

A geometric primitive. So far only points, spheres, segments, and AABBs can be constructed manually in the Python API.

type

Can be “Point”, “Sphere”, “Segment”, “Triangle”, “Polygon”, “AABB”, and “Box”. Semi-supported types include “Ellipsoid”, and “Cylinder”.

Type

str

properties

a list of parameters defining the primitive. The interpretation is type-specific.

Type

SWIG vector

C++ includes: geometry.h

Methods:

 loadString(str) param str returns setAABB(bmin, bmax) param bmin setBox(ori, R, dims) param ori param pt setPolygon(verts) param verts setSegment(a, b) param a setSphere(c, r) param c setTriangle(a, b, c) param a

Attributes:

 properties GeometricPrimitive_properties_get(GeometricPrimitive self) -> doubleVector type GeometricPrimitive_type_get(GeometricPrimitive self) -> std::string const &
loadString(str)[source]
Parameters

str (str) –

Returns

Return type

bool

property properties

GeometricPrimitive_properties_get(GeometricPrimitive self) -> doubleVector

saveString()[source]
Returns

Return type

str

setAABB(bmin, bmax)[source]
Parameters
• bmin (list of 3 floats) –

• bmax (list of 3 floats) –

setBox(ori, R, dims)[source]
Parameters
• ori (list of 3 floats) –

• R (list of 9 floats (so3 element)) –

• dims (list of 3 floats) –

setPoint(pt)[source]
Parameters

pt (list of 3 floats) –

setPolygon(verts)[source]
Parameters

verts (list of floats) –

setSegment(a, b)[source]
Parameters
• a (list of 3 floats) –

• b (list of 3 floats) –

setSphere(c, r)[source]
Parameters
• c (list of 3 floats) –

• r (float) –

setTriangle(a, b, c)[source]
Parameters
• a (list of 3 floats) –

• b (list of 3 floats) –

• c (list of 3 floats) –

property type

GeometricPrimitive_type_get(GeometricPrimitive self) -> std::string const &

class klampt.Geometry3D(*args)[source]

Bases: object

The three-D geometry container used throughout Klampt.

There are five currently supported types of geometry:

This class acts as a uniform container of all of these types.

There are two modes in which a Geometry3D can be used. It can be a standalone geometry, which means it is a container of geometry data, or it can be a reference to a world item’s geometry. For references, modifiers change the world item’s geometry.

Current transform

Each geometry stores a “current” transform, which is automatically updated for world items’ geometries. Proximity queries are then performed with respect to the transformed geometries. Crucially, the underlying geometry is not changed, because that could be computationally expensive.

Creating / modifying the geometry

Use the constructor, the set(), or the set[TYPE]() methods to completely change the geometry’s data.

Note: if you want to set a world item’s geometry to be equal to a standalone geometry, use the set(rhs) function rather than the assignment (=) operator.

Modifiers include:

Note

Avoid the use of translate, rotate, and transform to represent object movement. Use setCurrentTransform instead.

Proximity queries

For most geometry types (TriangleMesh, PointCloud, ConvexHull), the first time you perform a query, some collision detection data structures will be initialized. This preprocessing step can take some time for complex geometries.

Collision margins

Each object also has a “collision margin” which may virtually fatten the object, as far as proximity queries are concerned. This is useful for setting collision avoidance margins in motion planning. Use the setCollisionMargin() and getCollisionMargin() methods to access the margin. By default the margin is zero.

Note

The geometry margin is NOT the same thing as simulation body collision padding! All proximity queries are affected by the collision padding, inside or outside of simulation.

Conversions

Many geometry types can be converted to and from one another using the convert() method. This can also be used to remesh TriangleMesh objects and PointCloud objects.

C++ includes: geometry.h

__init__ (): Geometry3D

__init__ (arg2): Geometry3D

Parameters

Methods:

 Creates a standalone geometry from this geometry. collides(other) Returns true if this geometry collides with the other. contacts(other, padding1, padding2[, …]) Returns the set of contact points between this and other. convert(type[, param]) Converts a geometry to another type, if a conversion is available. distance(other) Returns the the distance and closest points between the given geometries. distance_ext(other, settings) A customizable version of Geometry3D.distance(). Returns the the distance and closest point to the input point, given in world coordinates. distance_point_ext(pt, settings) A customizable version of Geometry3D.distance_point(). distance_simple(other[, relErr, absErr]) Version 0.8: this is the same as the old distance() function. Returns true if this has no contents (not the same as numElements()==0) Frees the data associated with this geometry, if standalone. Returns the axis-aligned bounding box of the object as a tuple (bmin,bmax). Returns a tighter axis-aligned bounding box of the object than Geometry3D.getBB(). Returns the padding around the base geometry. Returns a ConvexHull if this geometry is of type ConvexHull. Gets the current transformation. getElement(element) Returns an element of the Geometry3D if it is a Group, TriangleMesh, or PointCloud. Returns a GeometricPrimitive if this geometry is of type GeometricPrimitive. Returns a PointCloud if this geometry is of type PointCloud. Returns a TriangleMesh if this geometry is of type TriangleMesh. Returns a VolumeGrid if this geometry is of type VolumeGrid. Returns true if this is a standalone geometry. Loads from file. Returns the number of sub-elements in this geometry. rayCast(s, d) Returns (hit,pt) where hit is true if the ray starting at s and pointing in direction d hits the geometry (given in world coordinates); pt is the hit point, in world coordinates. rayCast_ext(s, d) Returns (hit_element,pt) where hit_element is >= 0 if ray starting at s and pointing in direction d hits the geometry (given in world coordinates). Rotates the geometry data. Saves to file. scale(*args) Scales the geometry data with different factors on each axis. set(arg2) Copies the geometry of the argument into this geometry. setCollisionMargin(margin) Sets a padding around the base geometry which affects the results of proximity queries. setConvexHull(arg2) Sets this Geometry3D to a ConvexHull. setConvexHullGroup(g1, g2) Sets this Geometry3D to be a convex hull of two geometries. Sets the current transformation (not modifying the underlying data) setElement(element, data) Sets an element of the Geometry3D if it is a Group, TriangleMesh, or PointCloud. Sets this Geometry3D to a GeometricPrimitive. Sets this Geometry3D to a group geometry. setPointCloud(arg2) Sets this Geometry3D to a PointCloud. Sets this Geometry3D to a TriangleMesh. setVolumeGrid(arg2) Sets this Geometry3D to a volumeGrid. support(dir) Calculates the furthest point on this geometry in the direction dir. transform(R, t) Translates/rotates/scales the geometry data. Translates the geometry data. Returns the type of geometry: TriangleMesh, PointCloud, VolumeGrid, GeometricPrimitive, or Group. withinDistance(other, tol) Returns true if this geometry is within distance tol to other.

Attributes:

 geomPtr Geometry3D_geomPtr_get(Geometry3D self) -> void * id Geometry3D_id_get(Geometry3D self) -> int world Geometry3D_world_get(Geometry3D self) -> int
clone()[source]

Creates a standalone geometry from this geometry.

Returns

Return type

Geometry3D

collides(other)[source]

Returns true if this geometry collides with the other.

Parameters

other (Geometry3D) –

Returns

Return type

bool

Unsupported types:

• VolumeGrid - GeometricPrimitive [aabb, box, triangle, polygon]

• VolumeGrid - TriangleMesh

• VolumeGrid - VolumeGrid

• ConvexHull - anything else besides ConvexHull

contacts(other, padding1, padding2, maxContacts=0)[source]

Returns the set of contact points between this and other. This set is a discrete representation of the region of surface overlap, which is defined as all pairs of points within distance self.collisionMargin + other.collisionMargin + padding1 + padding2.

contacts (other,padding1,padding2,maxContacts=0): ContactQueryResult

contacts (other,padding1,padding2): ContactQueryResult

Parameters
• other (Geometry3D) –

• maxContacts (int, optional) – default value 0

Returns

Return type

For some geometry types (TriangleMesh-TriangleMesh, TriangleMesh-PointCloud, PointCloud-PointCloud) padding must be positive to get meaningful contact poitns and normals.

If maxContacts != 0 a clustering postprocessing step is performed.

Unsupported types:

• GeometricPrimitive-GeometricPrimitive subtypes segment vs aabb

• VolumeGrid-GeometricPrimitive any subtypes except point and sphere. also, the results are potentially inaccurate for non-convex VolumeGrids.

• VolumeGrid-TriangleMesh

• VolumeGrid-VolumeGrid

• ConvexHull - anything

convert(type, param=0)[source]

Converts a geometry to another type, if a conversion is available. The interpretation of param depends on the type of conversion, with 0 being a reasonable default.

convert (type,param=0): Geometry3D

convert (type): Geometry3D

Parameters
• type (str) –

• param (float, optional) – default value 0

Returns

Return type

Available conversions are:

• TriangleMesh -> PointCloud. param is the desired dispersion of the points, by default set to the average triangle diameter. At least all of the mesh’s vertices will be returned.

• TriangleMesh -> VolumeGrid. Converted using the fast marching method with good results only if the mesh is watertight. param is the grid resolution, by default set to the average triangle diameter.

• TriangleMesh -> ConvexHull. If param==0, just calculates a convex hull. Otherwise, uses convex decomposition with the HACD library.

• PointCloud -> TriangleMesh. Available if the point cloud is structured. param is the threshold for splitting triangles by depth discontinuity. param is by default infinity.

• PointCloud -> ConvexHull. Converted using SOLID / Qhull.

• GeometricPrimitive -> anything. param determines the desired resolution.

• VolumeGrid -> TriangleMesh. param determines the level set for the marching cubes algorithm.

• VolumeGrid -> PointCloud. param determines the level set.

• ConvexHull -> TriangleMesh.

• ConvexHull -> PointCloud. param is the desired dispersion of the points. Equivalent to ConvexHull -> TriangleMesh -> PointCloud

distance(other)[source]

Returns the the distance and closest points between the given geometries. This may be either the normal distance or the signed distance, depending on the geometry type.

Parameters

other (Geometry3D) –

Returns

Return type

DistanceQueryResult

The normal distance returns 0 if the two objects are touching (this.collides(other)=True).

The signed distance returns the negative penetration depth if the objects are touching. Only the following combinations of geometry types return signed distances:

• GeometricPrimitive-GeometricPrimitive (Python-supported sub-types only)

• GeometricPrimitive-TriangleMesh (surface only)

• GeometricPrimitive-PointCloud

• GeometricPrimitive-VolumeGrid

• TriangleMesh (surface only)-GeometricPrimitive

• PointCloud-VolumeGrid

• ConvexHull - ConvexHull

If penetration is supported, a negative distance is returned and cp1,cp2 are the deepest penetrating points.

Unsupported types:

• GeometricPrimitive-GeometricPrimitive subtypes segment vs aabb

• PointCloud-PointCloud

• VolumeGrid-TriangleMesh

• VolumeGrid-VolumeGrid

• ConvexHull - anything else besides ConvexHull

See the comments of the distance_point function

distance_ext(other, settings)[source]

A customizable version of Geometry3D.distance(). The settings for the calculation can be customized with relErr, absErr, and upperBound, e.g., to break if the closest points are at least upperBound distance from one another.

Parameters
Returns

Return type

DistanceQueryResult

distance_point(pt)[source]

Returns the the distance and closest point to the input point, given in world coordinates. An exception is raised if this operation is not supported with the given geometry type.

Parameters

pt (list of 3 floats) –

Returns

Return type

DistanceQueryResult

The return value contains the distance, closest points, and gradients if available.

For some geometry types, the signed distance is returned. The signed distance returns the negative penetration depth if pt is within this. The following geometry types return signed distances:

• GeometricPrimitive

• PointCloud (approximate, if the cloud is a set of balls with the radius property)

• VolumeGrid

• ConvexHull

For other types, a signed distance will be returned if the geometry has a positive collision margin, and the point penetrates less than this margin.

distance_point_ext(pt, settings)[source]

A customizable version of Geometry3D.distance_point(). The settings for the calculation can be customized with relErr, absErr, and upperBound, e.g., to break if the closest points are at least upperBound distance from one another.

Parameters
Returns

Return type

DistanceQueryResult

distance_simple(other, relErr=0, absErr=0)[source]

Version 0.8: this is the same as the old distance() function.

distance_simple (other,relErr=0,absErr=0): float

distance_simple (other,relErr=0): float

distance_simple (other): float

Parameters
• other (Geometry3D) –

• relErr (float, optional) – default value 0

• absErr (float, optional) – default value 0

Returns

Return type

(float)

Returns the distance from this geometry to the other. If either geometry contains volume information, this value may be negative to indicate penetration. See Geometry3D.distance() for more information.

empty()[source]

Returns true if this has no contents (not the same as numElements()==0)

Returns

Return type

bool

free()[source]

Frees the data associated with this geometry, if standalone.

property geomPtr

Geometry3D_geomPtr_get(Geometry3D self) -> void *

getBB()[source]

Returns the axis-aligned bounding box of the object as a tuple (bmin,bmax). Note: O(1) time, but may not be tight.

getBBTight()[source]

Returns a tighter axis-aligned bounding box of the object than Geometry3D.getBB(). Worst case O(n) time.

getCollisionMargin()[source]

Returns the padding around the base geometry. Default 0.

Returns

Return type

float

getConvexHull()[source]

Returns a ConvexHull if this geometry is of type ConvexHull.

Returns

Return type

ConvexHull

getCurrentTransform()[source]

Gets the current transformation.

getElement(element)[source]

Returns an element of the Geometry3D if it is a Group, TriangleMesh, or PointCloud. The element will be in local coordinates. Raises an error if this is of any other type.

Parameters

element (int) –

Returns

Return type

Geometry3D

getGeometricPrimitive()[source]

Returns a GeometricPrimitive if this geometry is of type GeometricPrimitive.

Returns

Return type

GeometricPrimitive

getPointCloud()[source]

Returns a PointCloud if this geometry is of type PointCloud.

Returns

Return type

PointCloud

getTriangleMesh()[source]

Returns a TriangleMesh if this geometry is of type TriangleMesh.

Returns

Return type

TriangleMesh

getVolumeGrid()[source]

Returns a VolumeGrid if this geometry is of type VolumeGrid.

Returns

Return type

VolumeGrid

property id

Geometry3D_id_get(Geometry3D self) -> int

isStandalone()[source]

Returns true if this is a standalone geometry.

Returns

Return type

bool

loadFile(fn)[source]

Loads from file. Standard mesh types, PCD files, and .geom files are supported.

Parameters

fn (str) –

Returns

Return type

bool

numElements()[source]

Returns the number of sub-elements in this geometry.

Returns

Return type

int

rayCast(s, d)[source]

Returns (hit,pt) where hit is true if the ray starting at s and pointing in direction d hits the geometry (given in world coordinates); pt is the hit point, in world coordinates.

Parameters
• s (list of 3 floats) –

• d (list of 3 floats) –

Returns

Return type

bool

Supported types:

• GeometricPrimitive

• TriangleMesh

• PointCloud (need a positive collision margin, or points need to have a ‘radius’ property assigned)

• VolumeGrid

• Group (groups of the aforementioned types)

rayCast_ext(s, d)[source]

Returns (hit_element,pt) where hit_element is >= 0 if ray starting at s and pointing in direction d hits the geometry (given in world coordinates).

Parameters
• s (list of 3 floats) –

• d (list of 3 floats) –

Returns

Return type

int

• hit_element is -1 if the object is not hit, otherwise it gives the index of the element (triangle, point, sub-object) that was hit. For geometric primitives, this will be 0.

• pt is the hit point, in world coordinates.

Supported types:

• GeometricPrimitive

• TriangleMesh

• PointCloud (need a positive collision margin, or points need to have a ‘radius’ property assigned)

• VolumeGrid

• Group (groups of the aforementioned types)

rotate(R)[source]

Rotates the geometry data. Permanently modifies the data and resets any collision data structures.

Parameters

R (list of 9 floats (so3 element)) –

saveFile(fn)[source]

Saves to file. Standard mesh types, PCD files, and .geom files are supported.

Parameters

fn (str) –

Returns

Return type

bool

scale(*args)[source]

Scales the geometry data with different factors on each axis. Permanently modifies the data and resets any collision data structures.

scale (s)

scale (sx,sy,sz)

Parameters
• s (float, optional) –

• sx (float, optional) –

• sy (float, optional) –

• sz (float, optional) –

set(arg2)[source]

Copies the geometry of the argument into this geometry.

Parameters

arg2 (Geometry3D) –

setCollisionMargin(margin)[source]

Sets a padding around the base geometry which affects the results of proximity queries.

Parameters

margin (float) –

setConvexHull(arg2)[source]

Sets this Geometry3D to a ConvexHull.

Parameters

arg2 (ConvexHull) –

setConvexHullGroup(g1, g2)[source]

Sets this Geometry3D to be a convex hull of two geometries. Note: the relative transform of these two objects is frozen in place; i.e., setting the current transform of g2 doesn’t do anything to this object.

Parameters
setCurrentTransform(R, t)[source]

Sets the current transformation (not modifying the underlying data)

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

setElement(element, data)[source]

Sets an element of the Geometry3D if it is a Group, TriangleMesh, or PointCloud. The element will be in local coordinates. Raises an error if this is of any other type.

Parameters
setGeometricPrimitive(arg2)[source]

Sets this Geometry3D to a GeometricPrimitive.

Parameters

arg2 (GeometricPrimitive) –

setGroup()[source]

Sets this Geometry3D to a group geometry. To add sub-geometries, repeatedly call setElement() with increasing indices.

setPointCloud(arg2)[source]

Sets this Geometry3D to a PointCloud.

Parameters

arg2 (PointCloud) –

setTriangleMesh(arg2)[source]

Sets this Geometry3D to a TriangleMesh.

Parameters

arg2 (TriangleMesh) –

setVolumeGrid(arg2)[source]

Sets this Geometry3D to a volumeGrid.

Parameters

arg2 (VolumeGrid) –

support(dir)[source]

Calculates the furthest point on this geometry in the direction dir.

Parameters

dir (list of 3 floats) –

Supported types:

• ConvexHull

transform(R, t)[source]

Translates/rotates/scales the geometry data. Permanently modifies the data and resets any collision data structures.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

translate(t)[source]

Translates the geometry data. Permanently modifies the data and resets any collision data structures.

Parameters

t (list of 3 floats) –

type()[source]

Returns the type of geometry: TriangleMesh, PointCloud, VolumeGrid, GeometricPrimitive, or Group.

Returns

Return type

str

withinDistance(other, tol)[source]

Returns true if this geometry is within distance tol to other.

Parameters
Returns

Return type

bool

property world

Geometry3D_world_get(Geometry3D self) -> int

class klampt.IKObjective(*args)[source]

Bases: object

A class defining an inverse kinematic target. Either a link on a robot can take on a fixed position/orientation in the world frame, or a relative position/orientation to another frame.

The positionScale and orientationScale attributes scale the solver’s residual vector. This affects whether the convergence tolerance is met, and also controls the emphasis on each objective / component when the objective cannot be reached. By default these are both 1.

C++ includes: robotik.h

With no arguments, constructs a blank IKObjective. Given an IKObjective, acts as a copy constructor.

__init__ (): IKObjective

__init__ (arg2): IKObjective

Parameters

arg2 (IKObjective, optional) –

Methods:

 closestMatch(R, t) Gets the transform T that’s closest to the transform (R,t) and that satisfies the IK goal’s constraints. Copy constructor. The index of the destination link, or -1 if fixed to the world. Returns the local and global position of the position constraint. For linear and planar constraints, returns the direction. For fixed rotation constraints, returns the orientation. For axis rotation constraints, returns the local and global axes. For fixed-transform constraints, returns the transform (R,t) The index of the robot link that is constrained. loadString(str) Loads the objective from a Klamp’t-native formatted string. Sets the destination coordinates of this constraint to fit the given target transform. Returns the number of position dimensions constrained (0-3) Returns the number of rotation dimensions constrained (0-3) Saves the objective to a Klamp’t-native formatted string. setAxialRotConstraint(alocal, aworld) Manual: Sets an axial rotation constraint. setFixedPoint(link, plocal, pworld) Sets a fixed-point constraint. setFixedPoints(link, plocals, pworlds) Sets a multiple fixed-point constraint. setFixedPosConstraint(tlocal, tworld) Manual: Sets a fixed position constraint. Manual: Sets a fixed rotation constraint. setFixedTransform(link, R, t) Sets a fixed-transform constraint (R,t) Manual: Sets a free position constraint. Deprecated: use setFreePosConstraint. Manual: Sets a free rotation constraint. setLinearPosConstraint(tlocal, sworld, dworld) Manual: Sets a linear position constraint T(link)*tlocal = sworld + u*dworld for some real value u. setLinks(link[, link2]) Manual construction. setPlanarPosConstraint(tlocal, nworld, oworld) Manual: Sets a planar position constraint nworld^T T(link)*tlocal + oworld = 0. setRelativePoint(link1, link2, p1, p2) Sets a fixed-point constraint relative to link2. setRelativePoints(link1, link2, p1s, p2s) Sets a multiple fixed-point constraint relative to link2. setRelativeTransform(link, linkTgt, R, t) Sets a fixed-transform constraint (R,t) relative to linkTgt. transform(R, t) Tranforms the target position/rotation of this IK constraint by transform (R,t) transformLocal(R, t) Tranforms the local position/rotation of this IK constraint by transform (R,t)

Attributes:

 goal IKObjective_goal_get(IKObjective self) -> IKGoal positionScale IKObjective_positionScale_get(IKObjective self) -> float rotationScale IKObjective_rotationScale_get(IKObjective self) -> float
closestMatch(R, t)[source]

Gets the transform T that’s closest to the transform (R,t) and that satisfies the IK goal’s constraints.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

copy()[source]

Copy constructor.

Returns

Return type

IKObjective

The index of the destination link, or -1 if fixed to the world.

Returns

Return type

int

getPosition()[source]

Returns the local and global position of the position constraint.

getPositionDirection()[source]

For linear and planar constraints, returns the direction.

getRotation()[source]

For fixed rotation constraints, returns the orientation.

getRotationAxis()[source]

For axis rotation constraints, returns the local and global axes.

getTransform()[source]

For fixed-transform constraints, returns the transform (R,t)

property goal

IKObjective_goal_get(IKObjective self) -> IKGoal

The index of the robot link that is constrained.

Returns

Return type

int

loadString(str)[source]

Loads the objective from a Klamp’t-native formatted string. For a more readable but verbose format, try the JSON IO routines klampt.io.loader.toJson() / klampt.io.loader.fromJson()

Parameters

str (str) –

Returns

Return type

bool

matchDestination(R, t)[source]

Sets the destination coordinates of this constraint to fit the given target transform. In other words, if (R,t) is the current link transform, this sets the destination position / orientation so that this objective has zero error. The current position/rotation constraint types are kept.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

numPosDims()[source]

Returns the number of position dimensions constrained (0-3)

Returns

Return type

int

numRotDims()[source]

Returns the number of rotation dimensions constrained (0-3)

Returns

Return type

int

property positionScale

IKObjective_positionScale_get(IKObjective self) -> float

property rotationScale

IKObjective_rotationScale_get(IKObjective self) -> float

saveString()[source]

Saves the objective to a Klamp’t-native formatted string. For a more readable but verbose format, try the JSON IO routines klampt.io.loader.toJson() / klampt.io.loader.fromJson()

Returns

Return type

str

setAxialRotConstraint(alocal, aworld)[source]

Manual: Sets an axial rotation constraint.

Parameters
• alocal (list of 3 floats) –

• aworld (list of 3 floats) –

setFixedPoint(link, plocal, pworld)[source]

Sets a fixed-point constraint.

Parameters

• plocal (list of 3 floats) –

• pworld (list of 3 floats) –

setFixedPoints(link, plocals, pworlds)[source]

Sets a multiple fixed-point constraint.

Parameters

• plocals (object) –

• pworlds (object) –

setFixedPosConstraint(tlocal, tworld)[source]

Manual: Sets a fixed position constraint.

Parameters
• tlocal (list of 3 floats) –

• tworld (list of 3 floats) –

setFixedRotConstraint(R)[source]

Manual: Sets a fixed rotation constraint.

Parameters

R (list of 9 floats (so3 element)) –

setFixedTransform(link, R, t)[source]

Sets a fixed-transform constraint (R,t)

Parameters

• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

setFreePosConstraint()[source]

Manual: Sets a free position constraint.

setFreePosition()[source]

Deprecated: use setFreePosConstraint.

setFreeRotConstraint()[source]

Manual: Sets a free rotation constraint.

setLinearPosConstraint(tlocal, sworld, dworld)[source]

Manual: Sets a linear position constraint T(link)*tlocal = sworld + u*dworld for some real value u.

Parameters
• tlocal (list of 3 floats) –

• sworld (list of 3 floats) –

• dworld (list of 3 floats) –

Manual construction.

Parameters

• link2 (int, optional) – default value -1

setPlanarPosConstraint(tlocal, nworld, oworld)[source]

Manual: Sets a planar position constraint nworld^T T(link)*tlocal + oworld = 0.

Parameters
• tlocal (list of 3 floats) –

• nworld (list of 3 floats) –

• oworld (float) –

setRelativePoint(link1, link2, p1, p2)[source]

Sets a fixed-point constraint relative to link2.

Parameters

• p1 (list of 3 floats) –

• p2 (list of 3 floats) –

setRelativePoints(link1, link2, p1s, p2s)[source]

Sets a multiple fixed-point constraint relative to link2.

Parameters

• p1s (object) –

• p2s (object) –

setRelativeTransform(link, linkTgt, R, t)[source]

Sets a fixed-transform constraint (R,t) relative to linkTgt.

Parameters

• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

transform(R, t)[source]

Tranforms the target position/rotation of this IK constraint by transform (R,t)

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

transformLocal(R, t)[source]

Tranforms the local position/rotation of this IK constraint by transform (R,t)

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

class klampt.IKSolver(*args)[source]

Bases: object

An inverse kinematics solver based on the Newton-Raphson technique.

Typical calling pattern is:

s = IKSolver(robot)
s.setMaxIters(100)
s.setTolerance(1e-4)
res = s.solve()
if res:
print("IK solution:",robot.getConfig(),"found in",
s.lastSolveIters(),"iterations, residual",s.getResidual())
else:
print("IK failed:",robot.getConfig(),"found in",
s.lastSolveIters(),"iterations, residual",s.getResidual())


C++ includes: robotik.h

Initializes an IK solver. Given a RobotModel, an empty solver is created. Given an IK solver, acts as a copy constructor.

__init__ (robot): IKSolver

__init__ (solver): IKSolver

Parameters

Attributes:

 activeDofs IKSolver_activeDofs_get(IKSolver self) -> intVector biasConfig IKSolver_biasConfig_get(IKSolver self) -> doubleVector lastIters IKSolver_lastIters_get(IKSolver self) -> int maxIters IKSolver_maxIters_get(IKSolver self) -> int objectives IKSolver_objectives_get(IKSolver self) -> std::vector< IKObjective,std::allocator< IKObjective > > * qmax IKSolver_qmax_get(IKSolver self) -> doubleVector qmin IKSolver_qmin_get(IKSolver self) -> doubleVector robot IKSolver_robot_get(IKSolver self) -> RobotModel tol IKSolver_tol_get(IKSolver self) -> double useJointLimits IKSolver_useJointLimits_get(IKSolver self) -> bool

Methods:

 add(objective) Adds a new simultaneous objective. Clears objectives. Copy constructor. Gets the active degrees of freedom. Gets the solvers’ bias configuration. Returns a matrix describing the instantaneous derivative of the objective with respect to the active Dofs. Gets the limits on the robot’s configuration (by default this is the robot’s joint limits. Gets the max # of iterations. Returns a vector describing the error of the objective at the current configuration. Gets the constraint solve tolerance. Returns true if the current configuration residual is less than tol. Returns the number of Newton-Raphson iterations used in the last solve() call. Samples an initial random configuration. set(i, objective) Assigns an existing objective added by add. setActiveDofs(active) Sets the active degrees of freedom. setBiasConfig(biasConfig) Biases the solver to approach a given configuration. setJointLimits(qmin, qmax) Sets limits on the robot’s configuration. setMaxIters(iters) Sets the max # of iterations (default 100) Sets the constraint solve tolerance (default 1e-3) solve(*args) Old-style: will be deprecated.
property activeDofs

IKSolver_activeDofs_get(IKSolver self) -> intVector

add(objective)[source]

Parameters

objective (IKObjective) –

property biasConfig

IKSolver_biasConfig_get(IKSolver self) -> doubleVector

clear()[source]

Clears objectives.

copy()[source]

Copy constructor.

Returns

Return type

IKSolver

getActiveDofs()[source]

Gets the active degrees of freedom.

getBiasConfig()[source]

Gets the solvers’ bias configuration.

getJacobian()[source]

Returns a matrix describing the instantaneous derivative of the objective with respect to the active Dofs.

getJointLimits()[source]

Gets the limits on the robot’s configuration (by default this is the robot’s joint limits.

getMaxIters()[source]

Gets the max # of iterations.

Returns

Return type

int

getResidual()[source]

Returns a vector describing the error of the objective at the current configuration.

getTolerance()[source]

Gets the constraint solve tolerance.

Returns

Return type

float

isSolved()[source]

Returns true if the current configuration residual is less than tol.

Returns

Return type

bool

property lastIters

IKSolver_lastIters_get(IKSolver self) -> int

lastSolveIters()[source]

Returns the number of Newton-Raphson iterations used in the last solve() call.

Returns

Return type

int

property maxIters

IKSolver_maxIters_get(IKSolver self) -> int

property objectives

IKSolver_objectives_get(IKSolver self) -> std::vector< IKObjective,std::allocator< IKObjective > > *

property qmax

IKSolver_qmax_get(IKSolver self) -> doubleVector

property qmin

IKSolver_qmin_get(IKSolver self) -> doubleVector

property robot

IKSolver_robot_get(IKSolver self) -> RobotModel

sampleInitial()[source]

Samples an initial random configuration. More initial configurations can be sampled in case the prior configs lead to local minima.

set(i, objective)[source]

Parameters
setActiveDofs(active)[source]

Sets the active degrees of freedom.

Parameters

active (list of int) –

setBiasConfig(biasConfig)[source]

Biases the solver to approach a given configuration. Setting an empty vector clears the bias term.

Parameters

biasConfig (list of floats) –

setJointLimits(qmin, qmax)[source]

Sets limits on the robot’s configuration. If empty, this turns off joint limits.

Parameters
• qmin (list of floats) –

• qmax (list of floats) –

setMaxIters(iters)[source]

Sets the max # of iterations (default 100)

Parameters

iters (int) –

setTolerance(res)[source]

Sets the constraint solve tolerance (default 1e-3)

Parameters

res (float) –

solve(*args)[source]

Old-style: will be deprecated. Specify # of iterations and tolerance. Tries to find a configuration that satifies all simultaneous objectives up to the desired tolerance. Returns (res,iterations) where res is true if x converged.

solve (): bool

solve (iters,tol): object

Parameters
• iters (int, optional) –

• tol (float, optional) –

Returns

Return type

(object or bool)

property tol

IKSolver_tol_get(IKSolver self) -> double

property useJointLimits

IKSolver_useJointLimits_get(IKSolver self) -> bool

class klampt.Mass[source]

Bases: object

Stores mass information for a rigid body or robot link.

Note

Recommended to use the set/get functions rather than changing the members directly due to strangeness in SWIG’s handling of vectors.

mass

the actual mass (typically in kg)

Type

float

com

the center of mass position, in local coordinates. (Better to use setCom/getCom)

Type

SWIG-based list of 3 floats

inertia

the inertia matrix in local coordinates. If 3 floats, this is a diagonal matrix. If 9 floats, this gives all entries of the 3x3 inertia matrix (in column major or row major order, it doesn’t matter since inertia matrices are symmetric)

Type

SWIG-based list of 3 floats or 9 floats

C++ includes: robotmodel.h

Attributes:

 com Mass_com_get(Mass self) -> doubleVector inertia Mass_inertia_get(Mass self) -> doubleVector mass Mass_mass_get(Mass self) -> double

Methods:

 estimate(g, mass[, surfaceFraction]) Estimates the com and inertia of a geometry, with a given total mass. Returns the COM as a list of 3 floats. Returns the inertia matrix as a list of 3 floats or 9 floats. returns setCom(_com) param _com setInertia(_inertia) Sets an inertia matrix. setMass(_mass) param _mass
property com

Mass_com_get(Mass self) -> doubleVector

estimate(g, mass, surfaceFraction=0)[source]

Estimates the com and inertia of a geometry, with a given total mass.

estimate (g,mass,surfaceFraction=0)

estimate (g,mass)

Parameters

For TriangleMesh types, surfaceFraction dictates how much of the object’s mass is concentrated at the surface rather than the interior.

getCom()[source]

Returns the COM as a list of 3 floats.

getInertia()[source]

Returns the inertia matrix as a list of 3 floats or 9 floats.

getMass()[source]
Returns

Return type

float

property inertia

Mass_inertia_get(Mass self) -> doubleVector

property mass

Mass_mass_get(Mass self) -> double

setCom(_com)[source]
Parameters

_com (list of floats) –

setInertia(_inertia)[source]

Sets an inertia matrix.

Parameters

_inertia (list of floats) –

setMass(_mass)[source]
Parameters

_mass (float) –

class klampt.PointCloud[source]

Bases: object

A 3D point cloud class.

vertices

a list of vertices, given as a list [x1, y1, z1, x2, y2, … zn]

Type

SWIG vector of floats

properties

a list of vertex properties, given as a list [p11, p21, …, pk1, p12, p22, …, pk2, …, p1n, p2n, …, pkn] where each vertex has k properties. The name of each property is given by the propertyNames member.

Type

SWIG vector of floats

propertyNames

a list of the names of each property

Type

SWIG vector of strs

settings

a general property map .

Type

SWIG map of strs to strs

Note

Because the bindings are generated by SWIG, you can access the members via some automatically generated accessors / modifiers. In particular len(), append(), and indexing via [] are useful. Some other methods like resize() and iterators are also provided. However, you CANNOT set these items via assignment, i.e., pc.vertices = [0,0,0] is not allowed.

Property names are usually lowercase but follow PCL naming convention, and often include:

• normal_x, normal_y, normal_z: the outward normal

• rgb, rgba: integer encoding of RGB (24 bit int) or RGBA color (32 bit int)

• opacity: opacity, in range [0,1]

• c: opacity, in range [0,255]

• r,g,b,a: color channels, in range [0,1]

• u,v: texture coordinate

• radius: treats the point cloud as a collection of balls

Settings are usually lowercase but follow PCL naming convention, and often include:

• version: version of the PCL file, typically “0.7”

• id: integer id

• width: the width (in pixels) of a structured point cloud

• height: the height (in pixels) of a structured point cloud

• viewpoint: Camera position and orientation in the form ox oy oz qw qx qy qz, with (ox,oy,oz) the focal point and (qw,qx,qy,qz) the quaternion representation of the orientation (canonical representation, with X right, Y down, Z forward).

Examples:

pc = PointCloud()
pc.propertyNames.append('rgb')
#add 1 point with coordinates (0,0,0) and color #000000 (black)
pc.vertices.append(0)
pc.vertices.append(0)
pc.vertices.append(0)
pc.properties.append(0)
print(len(pc.vertices))  #prints 3
print(pc.numPoints())  #prints 1
#add another point with coordinates (1,2,3)
#this prints 2
print(pc.numPoints() )
#this prints 2, because there is 1 property category x 2 points
print(len(pc.properties.size()))
#this prints 0; this is the default value added when addPoint is called
print(pc.getProperty(1,0) )


To get all points as an n x 3 numpy array:

points = np.array(pc.vertices).reshape((pc.numPoints(),3))


To get all properties as a n x k numpy array:

properties =


np.array(pc.properties).reshape((p.numPoints(),p.numProperties()))

(Or use the convenience functions in klampt.io.numpy_convert)

C++ includes: geometry.h

Methods:

 Adds a point. addProperty(*args) Adds a new property with name pname, and sets values for this property to the given list (a n-list) getPoint(index) Retrieves the position of the point at the given index. getProperties(*args) Gets property named pindex of all points as an array. getProperty(*args) Gets the property named pname of point index. getSetting(key) Retrieves the given setting. join(pc) Adds the given point cloud to this one. Returns the number of points. Returns the number of properties. setPoint(index, p) Sets the position of the point at the given index to p. setPoints(num, plist) Sets all the points to the given list (a 3n-list) setProperties(*args) Sets property pindex of all points to the given list (a n-list) setProperty(*args) Sets the property named pname of point index to the given value. setSetting(key, value) Sets the given setting. transform(R, t) Transforms all the points by the rigid transform v=R*v+t. Translates all the points by v=v+t.

Attributes:

 properties PointCloud_properties_get(PointCloud self) -> doubleVector propertyNames PointCloud_propertyNames_get(PointCloud self) -> stringVector settings PointCloud_settings_get(PointCloud self) -> stringMap vertices PointCloud_vertices_get(PointCloud self) -> doubleVector
addPoint(p)[source]

Adds a point. Sets all its properties to 0. Returns the index.

Parameters

p (list of 3 floats) –

Returns

Return type

int

addProperty(*args)[source]

Adds a new property with name pname, and sets values for this property to the given list (a n-list)

Parameters
• pname (str) –

• properties (list of floats, optional) –

getPoint(index)[source]

Retrieves the position of the point at the given index.

Parameters

index (int) –

getProperties(*args)[source]

Gets property named pindex of all points as an array.

getProperties (pindex)

getProperties (pname)

Parameters
• pindex (int, optional) –

• pname (str, optional) –

getProperty(*args)[source]

Gets the property named pname of point index.

getProperty (index,pindex): float

getProperty (index,pname): float

Parameters
• index (int) –

• pindex (int, optional) –

• pname (str, optional) –

Returns

Return type

(float)

getSetting(key)[source]

Retrieves the given setting.

Parameters

key (str) –

Returns

Return type

str

join(pc)[source]

Adds the given point cloud to this one. They must share the same properties or else an exception is raised.

Parameters

pc (PointCloud) –

numPoints()[source]

Returns the number of points.

Returns

Return type

int

numProperties()[source]

Returns the number of properties.

Returns

Return type

int

property properties

PointCloud_properties_get(PointCloud self) -> doubleVector

property propertyNames

PointCloud_propertyNames_get(PointCloud self) -> stringVector

setPoint(index, p)[source]

Sets the position of the point at the given index to p.

Parameters
• index (int) –

• p (list of 3 floats) –

setPoints(num, plist)[source]

Sets all the points to the given list (a 3n-list)

Parameters
• num (int) –

• plist (list of floats) –

setProperties(*args)[source]

Sets property pindex of all points to the given list (a n-list)

setProperties (properties)

setProperties (pindex,properties)

Parameters
• properties (list of floats) –

• pindex (int, optional) –

setProperty(*args)[source]

Sets the property named pname of point index to the given value.

setProperty (index,pindex,value)

setProperty (index,pname,value)

Parameters
• index (int) –

• pindex (int, optional) –

• value (float) –

• pname (str, optional) –

setSetting(key, value)[source]

Sets the given setting.

Parameters
• key (str) –

• value (str) –

property settings

PointCloud_settings_get(PointCloud self) -> stringMap

transform(R, t)[source]

Transforms all the points by the rigid transform v=R*v+t.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

translate(t)[source]

Translates all the points by v=v+t.

Parameters

t (list of 3 floats) –

property vertices

PointCloud_vertices_get(PointCloud self) -> doubleVector

class klampt.RigidObjectModel[source]

Bases: object

A rigid movable object.

A rigid object has a name, geometry, appearance, mass, surface properties, and current transform / velocity.

State is retrieved/set using get/setTransform, and get/setVelocity

C++ includes: robotmodel.h

Methods:

 Returns a reference to the appearance associated with this object. drawGL([keepAppearance]) Draws the object’s geometry. Returns a reference to the geometry associated with this object. Returns a copy of the ContactParameters of this rigid object. Returns the ID of the rigid object in its world. Returns a copy of the Mass of this rigid object. returns Retrieves the rotation / translation of the rigid object (R,t) Retrieves the (angular velocity, velocity) of the rigid object. Loads the object from the file fn. saveFile(fn[, geometryName]) Saves the object to the file fn. setContactParameters(params) param params setMass(mass) param mass setName(name) param name setTransform(R, t) Sets the rotation / translation (R,t) of the rigid object. setVelocity(angularVelocity, velocity) Sets the (angular velocity, velocity) of the rigid object.

Attributes:

 index RigidObjectModel_index_get(RigidObjectModel self) -> int object RigidObjectModel_object_get(RigidObjectModel self) -> RigidObject * world RigidObjectModel_world_get(RigidObjectModel self) -> int
appearance()[source]

Returns a reference to the appearance associated with this object.

Returns

Return type

Appearance

drawGL(keepAppearance=True)[source]

Draws the object’s geometry. If keepAppearance=true, the current appearance is honored. Otherwise, only the raw geometry is drawn.

drawGL (keepAppearance=True)

drawGL ()

Parameters

keepAppearance (bool, optional) – default value True

PERFORMANCE WARNING: if keepAppearance is false, then this does not properly reuse OpenGL display lists. A better approach is to change the object’s Appearance directly.

geometry()[source]

Returns a reference to the geometry associated with this object.

Returns

Return type

Geometry3D

getContactParameters()[source]

Returns a copy of the ContactParameters of this rigid object.

Returns

Return type

ContactParameters

Note

To change the contact parameters, you should call p=object.getContactParameters(), change the desired properties in p, and then call object.setContactParameters(p)

getID()[source]

Returns the ID of the rigid object in its world.

Returns

Return type

int

Note: The world ID is not the same as the rigid object index.

getMass()[source]

Returns a copy of the Mass of this rigid object.

Returns

Return type

Mass

Note

To change the mass properties, you should call m=object.getMass(), change the desired properties in m, and then object.setMass(m)

getName()[source]
Returns

Return type

str

getTransform()[source]

Retrieves the rotation / translation of the rigid object (R,t)

Returns

a pair (R,t), with R a 9-list and t a 3-list of floats, giving the transform to world coordinates.

Return type

(se3 object)

getVelocity()[source]

Retrieves the (angular velocity, velocity) of the rigid object.

Returns

a pair of 3-lists (w,v) where w is the angular velocity vector and v is the translational velocity vector (both in world coordinates)

Return type

(tuple)

property index

RigidObjectModel_index_get(RigidObjectModel self) -> int

loadFile(fn)[source]

Loads the object from the file fn.

Parameters

fn (str) –

Returns

Return type

bool

property object

RigidObjectModel_object_get(RigidObjectModel self) -> RigidObject *

saveFile(fn, geometryName=None)[source]

Saves the object to the file fn. If geometryName is given, the geometry is saved to that file.

saveFile (fn,geometryName=None): bool

saveFile (fn): bool

Parameters
• fn (str) –

• geometryName (str, optional) – default value None

Returns

Return type

(bool)

setContactParameters(params)[source]
Parameters

params (ContactParameters) –

setMass(mass)[source]
Parameters

mass (Mass) –

setName(name)[source]
Parameters

name (str) –

setTransform(R, t)[source]

Sets the rotation / translation (R,t) of the rigid object.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

setVelocity(angularVelocity, velocity)[source]

Sets the (angular velocity, velocity) of the rigid object.

Parameters
• angularVelocity (list of 3 floats) –

• velocity (list of 3 floats) –

property world

RigidObjectModel_world_get(RigidObjectModel self) -> int

class klampt.RobotModel[source]

Bases: object

A model of a dynamic and kinematic robot.

Stores both constant information, like the reference placement of the links, joint limits, velocity limits, etc, as well as a current configuration and current velocity which are state-dependent. Several functions depend on the robot’s current configuration and/or velocity. To update that, use the setConfig() and setVelocity() functions. setConfig() also update’s the robot’s link transforms via forward kinematics. You may also use setDOFPosition and setDOFVelocity for individual changes, but this is more expensive because each call updates all of the affected the link transforms.

It is important to understand that changing the configuration of the model doesn’t actually send a command to the physical / simulated robot. Moreover, the model does not automatically get updated when the physical / simulated robot moves. In essence, the model maintains temporary storage for performing kinematics, dynamics, and planning computations, as well as for visualization.

The state of the robot is retrieved using getConfig/getVelocity calls, and is set using setConfig/setVelocity. Because many routines change the robot’s configuration, like IK and motion planning, a common design pattern is to save/restore the configuration as follows:

q = robot.getConfig()
do some stuff that may touch the robot's configuration...
robot.setConfig(q)


The model maintains configuration/velocity/acceleration/torque bounds. However, these are not enforced by the model, so you can happily set configurations outside must rather be enforced by the planner / simulator.

C++ includes: robotmodel.h

Methods:

 Computes the foward dynamics (using Recursive Newton Euler solver) configFromDrivers(driverValues) Converts a list of driver values (length numDrivers()) to a full configuration (length numLinks()). configToDrivers(config) Converts a full configuration (length numLinks()) to a list of driver values (length numDrivers()). distance(a, b) Computes a distance between two configurations, properly taking into account nonstandard joints. drawGL([keepAppearance]) Draws the robot geometry. driver(*args) Returns a reference to the driver by index or name. enableSelfCollision(link1, link2, value) Enables/disables self collisions between two links (depending on value) Retrieve the acceleration limit vector amax, the constraint is $$|ddq[i]| \leq amax[i]$$ Returns the 3D angular momentum vector. Returns the 3D center of mass at the current config. Returns the Jacobian matrix of the current center of mass. Returns the 3D velocity of the center of mass at the current config / velocity. Retrieves the current configuration of the robot model. Returns the Coriolis force matrix C(q,dq) for current config and velocity. Returns the Coriolis forces C(q,dq)*dq for current config and velocity. getDOFPosition(*args) Returns a single DOF’s position (by name) Returns the generalized gravity vector G(q) for the given workspace gravity vector g (usually (0,0,-9.8)). Returns the ID of the robot in its world. Retrieves a pair (qmin,qmax) of min/max joint limit vectors. getJointType(*args) Returns the joint type of the joint connecting the link to its parent, where the link is identified by index or by name. Returns the kinetic energy at the current config / velocity. Returns the 3D linear momentum vector. Returns the nxn mass matrix B(q). Returns the derivative of the nxn mass matrix with respect to q_i. Returns the inverse of the nxn mass matrix B(q)^-1. Returns the derivative of the nxn mass matrix with respect to t, given the robot’s current velocity. returns Retrieve the torque limit vector tmax, the constraint is $$|torque[i]| \leq tmax[i]$$ Calculates the 3x3 total inertia matrix of the robot. Retreives the current velocity of the robot model. Retrieve the velocity limit vector vmax, the constraint is $$|dq[i]| \leq vmax[i]$$ interpolate(a, b, u) Interpolates smoothly between two configurations, properly taking into account nonstandard joints. Returns the configuration derivative at a as you interpolate toward b at unit speed. link(*args) Returns a reference to the link by index or name. Loads the robot from the file fn. mount(link, subRobot, R, t) Mounts a sub-robot onto a link, with its origin at a given local transform (R,t). Returns the number of drivers. Returns the number of links = number of DOF’s. randomizeConfig([unboundedScale]) Samples a random configuration and updates the robot’s pose. reduce(robot) Sets self to a reduced version of robot, where all fixed DOFs are eliminated. saveFile(fn[, geometryPrefix]) Saves the robot to the file fn. Returns true if the robot is in self collision (faster than manual testing) selfCollisionEnabled(link1, link2) Queries whether self collisions between two links is enabled. sensor(*args) Returns a sensor by index or by name. Sets the acceleration limit vector amax, the constraint is $$|ddq[i]| \leq amax[i]$$ Sets the current configuration of the robot. setDOFPosition(*args) Sets a single DOF’s position (by index or by name). setJointLimits(qmin, qmax) Sets the min/max joint limit vectors (must have length numLinks()) setName(name) Sets the name of the robot. Sets the torque limit vector tmax, the constraint is $$|torque[i]| \leq tmax[i]$$ Sets the current velocity of the robot model. Sets the velocity limit vector vmax, the constraint is $$|dq[i]| \leq vmax[i]$$ Computes the inverse dynamics. velocityFromDrivers(driverVelocities) Converts a list of driver velocities (length numDrivers()) to a full velocity vector (length numLinks()). velocityToDrivers(velocities) Converts a full velocity vector (length numLinks()) to a list of driver velocities (length numDrivers()).

Attributes:

 dirty_dynamics RobotModel_dirty_dynamics_get(RobotModel self) -> bool index RobotModel_index_get(RobotModel self) -> int robot RobotModel_robot_get(RobotModel self) -> Robot * world RobotModel_world_get(RobotModel self) -> int
accelFromTorques(t)[source]

Computes the foward dynamics (using Recursive Newton Euler solver)

Parameters

t (list of floats) –

Note

Does not include gravity term G(q). getGravityForces(g) will need to be subtracted from the argument t.

Returns

the n-element joint acceleration vector that would result from joint torques t in the absence of external forces.

Return type

(list of floats)

configFromDrivers(driverValues)[source]

Converts a list of driver values (length numDrivers()) to a full configuration (length numLinks()).

Parameters

driverValues (list of floats) –

configToDrivers(config)[source]

Converts a full configuration (length numLinks()) to a list of driver values (length numDrivers()).

Parameters

config (list of floats) –

property dirty_dynamics

RobotModel_dirty_dynamics_get(RobotModel self) -> bool

distance(a, b)[source]

Computes a distance between two configurations, properly taking into account nonstandard joints.

Parameters
• a (list of floats) –

• b (list of floats) –

Returns

Return type

float

drawGL(keepAppearance=True)[source]

Draws the robot geometry. If keepAppearance=true, the current appearance is honored. Otherwise, only the raw geometry is drawn.

drawGL (keepAppearance=True)

drawGL ()

Parameters

keepAppearance (bool, optional) – default value True

PERFORMANCE WARNING: if keepAppearance is false, then this does not properly reuse OpenGL display lists. A better approach to changing the robot’s appearances is to set the link Appearance’s directly.

driver(*args)[source]

Returns a reference to the driver by index or name.

driver (index): RobotModelDriver

driver (name): RobotModelDriver

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type

(RobotModelDriver)

enableSelfCollision(link1, link2, value)[source]

Enables/disables self collisions between two links (depending on value)

Parameters

• value (bool) –

getAccelerationLimits()[source]

Retrieve the acceleration limit vector amax, the constraint is $$|ddq[i]| \leq amax[i]$$

getAngularMomentum()[source]

Returns the 3D angular momentum vector.

getCom()[source]

Returns the 3D center of mass at the current config.

getComJacobian()[source]

Returns the Jacobian matrix of the current center of mass.

Returns

a 3xn matrix J such that np.dot(J,dq) gives the COM velocity at the currene configuration

Return type

(list of 3 lists)

getComVelocity()[source]

Returns the 3D velocity of the center of mass at the current config / velocity.

getConfig()[source]

Retrieves the current configuration of the robot model.

getCoriolisForceMatrix()[source]

Returns the Coriolis force matrix C(q,dq) for current config and velocity. Takes O(n^2) time.

getCoriolisForces()[source]

Returns the Coriolis forces C(q,dq)*dq for current config and velocity. Takes O(n) time, which is faster than computing matrix and doing product. (“Forces” is somewhat of a misnomer; the result is a joint torque vector)

getDOFPosition(*args)[source]

Returns a single DOF’s position (by name)

getDOFPosition (i): float

getDOFPosition (name): float

Parameters
• i (int, optional) –

• name (str, optional) –

Returns

Return type

(float)

getGravityForces(g)[source]

Returns the generalized gravity vector G(q) for the given workspace gravity vector g (usually (0,0,-9.8)).

Parameters

g (list of 3 floats) –

Note

“Forces” is somewhat of a misnomer; the result is a vector of joint torques.

Returns

the n-element generalized gravity vector at the robot’s current configuration.

Return type

(list of floats)

getID()[source]

Returns the ID of the robot in its world.

Returns

Return type

int

Note: The world ID is not the same as the robot index.

getJointLimits()[source]

Retrieves a pair (qmin,qmax) of min/max joint limit vectors.

getJointType(*args)[source]

Returns the joint type of the joint connecting the link to its parent, where the link is identified by index or by name.

getJointType (index): str

getJointType (name): str

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type

(str)

getKineticEnergy()[source]

Returns the kinetic energy at the current config / velocity.

Returns

Return type

float

getLinearMomentum()[source]

Returns the 3D linear momentum vector.

getMassMatrix()[source]

Returns the nxn mass matrix B(q). Takes O(n^2) time.

getMassMatrixDeriv(i)[source]

Returns the derivative of the nxn mass matrix with respect to q_i. Takes O(n^3) time.

Parameters

i (int) –

getMassMatrixInv()[source]

Returns the inverse of the nxn mass matrix B(q)^-1. Takes O(n^2) time, which is much faster than inverting the result of getMassMatrix.

getMassMatrixTimeDeriv()[source]

Returns the derivative of the nxn mass matrix with respect to t, given the robot’s current velocity. Takes O(n^4) time.

getName()[source]
Returns

Return type

str

getTorqueLimits()[source]

Retrieve the torque limit vector tmax, the constraint is $$|torque[i]| \leq tmax[i]$$

getTotalInertia()[source]

Calculates the 3x3 total inertia matrix of the robot.

getVelocity()[source]

Retreives the current velocity of the robot model.

getVelocityLimits()[source]

Retrieve the velocity limit vector vmax, the constraint is $$|dq[i]| \leq vmax[i]$$

property index

RobotModel_index_get(RobotModel self) -> int

interpolate(a, b, u)[source]

Interpolates smoothly between two configurations, properly taking into account nonstandard joints.

Parameters
• a (list of floats) –

• b (list of floats) –

• u (float) –

Returns

The configuration that is u fraction of the way from a to b

Return type

(list of n floats)

interpolateDeriv(a, b)[source]

Returns the configuration derivative at a as you interpolate toward b at unit speed.

Parameters
• a (list of floats) –

• b (list of floats) –

Returns a reference to the link by index or name.

link (index): RobotModelLink

link (name): RobotModelLink

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type
loadFile(fn)[source]

Loads the robot from the file fn.

Parameters

fn (str) –

Returns

Return type

bool

mount(link, subRobot, R, t)[source]

Mounts a sub-robot onto a link, with its origin at a given local transform (R,t). The sub-robot’s links will be renamed to subRobot.getName() + ‘:’ + link.getName() unless subRobot.getName() is ‘’, in which case the link names are preserved.

Parameters

• subRobot (RobotModel) –

• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

numDrivers()[source]

Returns the number of drivers.

Returns

Return type

int

Returns the number of links = number of DOF’s.

Returns

Return type

int

randomizeConfig(unboundedScale=1.0)[source]

Samples a random configuration and updates the robot’s pose. Properly handles non-normal joints and handles DOFs with infinite bounds using a centered Laplacian distribution with the given scaling term. (Note that the python random seeding does not affect the result.)

randomizeConfig (unboundedScale=1.0)

randomizeConfig ()

Parameters

unboundedScale (float, optional) – default value 1.0

reduce(robot)[source]

Sets self to a reduced version of robot, where all fixed DOFs are eliminated. The return value is a map from the original robot DOF indices to the reduced DOFs.

Parameters

robot (RobotModel) –

Note that any geometries fixed to the world will disappear.

property robot

RobotModel_robot_get(RobotModel self) -> Robot *

saveFile(fn, geometryPrefix=None)[source]

Saves the robot to the file fn.

saveFile (fn,geometryPrefix=None): bool

saveFile (fn): bool

Parameters
• fn (str) –

• geometryPrefix (str, optional) – default value None

Returns

Return type

(bool)

If geometryPrefix == None (default), the geometry is not saved. Otherwise, the geometry of each link will be saved to files named geometryPrefix+name, where name is either the name of the geometry file that was loaded, or [link_name].off

selfCollides()[source]

Returns true if the robot is in self collision (faster than manual testing)

Returns

Return type

bool

selfCollisionEnabled(link1, link2)[source]

Queries whether self collisions between two links is enabled.

Parameters

Returns

Return type

bool

sensor(*args)[source]

Returns a sensor by index or by name. If out of bounds or unavailable, a null sensor is returned (i.e., SimRobotSensor.name() or SimRobotSensor.type()) will return the empty string.)

sensor (index): SimRobotSensor

sensor (name): SimRobotSensor

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type
setAccelerationLimits(amax)[source]

Sets the acceleration limit vector amax, the constraint is $$|ddq[i]| \leq amax[i]$$

Parameters

amax (list of floats) –

setConfig(q)[source]

Sets the current configuration of the robot. Input q is a vector of length numLinks(). This also updates forward kinematics of all links.

Parameters

q (list of floats) –

Again, it is important to realize that the RobotModel is not the same as a simulated robot, and this will not change the simulation world. Many functions such as IK and motion planning use the RobotModel configuration as a temporary variable, so if you need to keep the configuration through a robot-modifying function call, you should call q = robot.getConfig() before the call, and then robot.setConfig(q) after it.

setDOFPosition(*args)[source]

Sets a single DOF’s position (by index or by name).

setDOFPosition (i,qi)

setDOFPosition (name,qi)

Parameters
• i (int, optional) –

• qi (float) –

• name (str, optional) –

Note: if you are setting several joints at once, use setConfig because this function computes forward kinematics each time it is called.

setJointLimits(qmin, qmax)[source]

Sets the min/max joint limit vectors (must have length numLinks())

Parameters
• qmin (list of floats) –

• qmax (list of floats) –

setName(name)[source]

Sets the name of the robot.

Parameters

name (str) –

setTorqueLimits(tmax)[source]

Sets the torque limit vector tmax, the constraint is $$|torque[i]| \leq tmax[i]$$

Parameters

tmax (list of floats) –

setVelocity(dq)[source]

Sets the current velocity of the robot model. Like the configuration, this is also essentially a temporary variable.

Parameters

dq (list of floats) –

setVelocityLimits(vmax)[source]

Sets the velocity limit vector vmax, the constraint is $$|dq[i]| \leq vmax[i]$$

Parameters

vmax (list of floats) –

torquesFromAccel(ddq)[source]

Computes the inverse dynamics. Uses Recursive Newton Euler solver and takes O(n) time.

Parameters

ddq (list of floats) –

Note

Does not include gravity term G(q). getGravityForces(g) will need to be added to the result.

Returns

the n-element torque vector that would produce the joint accelerations ddq in the absence of external forces.

Return type

(list of floats)

velocityFromDrivers(driverVelocities)[source]

Converts a list of driver velocities (length numDrivers()) to a full velocity vector (length numLinks()).

Parameters

driverVelocities (list of floats) –

velocityToDrivers(velocities)[source]

Converts a full velocity vector (length numLinks()) to a list of driver velocities (length numDrivers()).

Parameters

velocities (list of floats) –

property world

RobotModel_world_get(RobotModel self) -> int

Bases: object

A reference to a link of a RobotModel.

The link stores many mostly-constant items (id, name, parent, geometry, appearance, mass, joint axes). There are two exceptions:

A RobotModelLink is not created by hand, but instead accessed using RobotModel.link() (index or name)

C++ includes: robotmodel.h

Methods:

 Returns a reference to the link’s appearance. drawLocalGL([keepAppearance]) Draws the link’s geometry in its local frame. drawWorldGL([keepAppearance]) Draws the link’s geometry in the world frame. Returns a reference to the link’s geometry. Returns the acceleration of the link origin given the robot’s current joint configuration and velocities, and the joint accelerations ddq. Returns the angular acceleration of the link given the robot’s current joint configuration and velocities, and the joint accelerations ddq. Returns the angular velocity of the link given the robot’s current joint configuration and velocities. Gets the local rotational / translational axis. Returns the ID of the robot link in its world. Returns the index of the link (on its robot). getJacobian(plocal) Returns the total jacobian of a point on this link w.r.t. getLocalDirection(vworld) Converts direction from world to local coordinates. getLocalPosition(pworld) Converts point from world to local coordinates. Retrieves the inertial properties of the link. Returns the name of the robot link. Returns the Hessians of each orientation component of the link w.r.t the robot’s configuration q. Returns the orientation jacobian of this link w.r.t. Returns the index of the link’s parent (on its robot). Gets transformation (R,t) to the parent link. getPointAcceleration(plocal, ddq) Returns the acceleration of the point given the robot’s current joint configuration and velocities, and the joint accelerations ddq. getPointVelocity(plocal) Returns the world velocity of a point attached to the link, given the robot’s current joint configuration and velocities. getPositionHessian(plocal) Returns the Hessians of each component of the position p w.r.t the robot’s configuration q. getPositionJacobian(plocal) Returns the position jacobian of a point on this link w.r.t. Gets the link’s current transformation (R,t) to the world frame. Returns the velocity of the link’s origin given the robot’s current joint configuration and velocities. getWorldDirection(vlocal) Converts direction from local to world coordinates. getWorldPosition(plocal) Converts point from local to world coordinates. Returns whether the joint is prismatic. Returns whether the joint is revolute. Returns a reference to the link’s parent, or a NULL link if it has no parent. Returns a reference to the link’s robot. setAxis(axis) Sets the local rotational / translational axis. setMass(mass) Sets the inertial proerties of the link. setName(name) Sets the name of the robot link. setParent(*args) Sets the link’s parent (must be on the same robot). Sets transformation (R,t) to the parent link. setPrismatic(prismatic) Changes a link from revolute to prismatic or vice versa. setTransform(R, t) Sets the link’s current transformation (R,t) to the world frame.

Attributes:

 index RobotModelLink_index_get(RobotModelLink self) -> int robotIndex RobotModelLink_robotIndex_get(RobotModelLink self) -> int robotPtr RobotModelLink_robotPtr_get(RobotModelLink self) -> Robot * world RobotModelLink_world_get(RobotModelLink self) -> int
appearance()[source]

Returns a reference to the link’s appearance.

Returns

Return type

Appearance

drawLocalGL(keepAppearance=True)[source]

Draws the link’s geometry in its local frame. If keepAppearance=true, the current Appearance is honored. Otherwise, just the geometry is drawn.

drawLocalGL (keepAppearance=True)

drawLocalGL ()

Parameters

keepAppearance (bool, optional) – default value True

drawWorldGL(keepAppearance=True)[source]

Draws the link’s geometry in the world frame. If keepAppearance=true, the current Appearance is honored. Otherwise, just the geometry is drawn.

drawWorldGL (keepAppearance=True)

drawWorldGL ()

Parameters

keepAppearance (bool, optional) – default value True

geometry()[source]

Returns a reference to the link’s geometry.

Returns

Return type

Geometry3D

getAcceleration(ddq)[source]

Returns the acceleration of the link origin given the robot’s current joint configuration and velocities, and the joint accelerations ddq.

Parameters

ddq (list of floats) –

ddq can be empty, which calculates the acceleration with acceleration 0, and is a little faster than setting ddq to [0]*n

Returns

the acceleration of the link’s origin, in world coordinates.

Return type

(list of 3 floats)

getAngularAcceleration(ddq)[source]

Returns the angular acceleration of the link given the robot’s current joint configuration and velocities, and the joint accelerations ddq.

Parameters

ddq (list of floats) –

Returns

the angular acceleration of the link, in world coordinates.

Return type

(list of 3 floats)

getAngularVelocity()[source]

Returns the angular velocity of the link given the robot’s current joint configuration and velocities.

Returns

the current angular velocity of the link, in world coordinates

Return type

(list of 3 floats)

getAxis()[source]

Gets the local rotational / translational axis.

getID()[source]

Returns the ID of the robot link in its world.

Returns

Return type

int

Note: The world ID is not the same as the link’s index, retrieved by getIndex.

getIndex()[source]

Returns the index of the link (on its robot).

Returns

Return type

int

getJacobian(plocal)[source]

Returns the total jacobian of a point on this link w.r.t. the robot’s configuration q.

Parameters

plocal (list of 3 floats) –

Returns

the 6xn total Jacobian matrix of the point given by local coordinates plocal. The matrix is row-major.

The orientation jacobian is given in the first 3 rows, and is stacked on the position jacobian, which is given in the last 3 rows.

Return type

(list of 6 lists of floats)

getLocalDirection(vworld)[source]

Converts direction from world to local coordinates.

Parameters

vworld (list of 3 floats) –

Returns

the local coordinates of the world direction vworld

Return type

(list of 3 floats)

getLocalPosition(pworld)[source]

Converts point from world to local coordinates.

Parameters

pworld (list of 3 floats) –

Returns

the local coordinates of the world point pworld

Return type

(list of 3 floats)

getMass()[source]

Retrieves the inertial properties of the link. (Note that the Mass is given with origin at the link frame, not about the COM.)

Returns

Return type

Mass

getName()[source]

Returns the name of the robot link.

Returns

Return type

str

getOrientationHessian()[source]

Returns the Hessians of each orientation component of the link w.r.t the robot’s configuration q.

Returns

a triple (Hx,Hy,Hz) of of nxn matrices corresponding, respectively, to the (wx,wy,wz) components of the Hessian.

Return type

(3-tuple)

getOrientationJacobian()[source]

Returns the orientation jacobian of this link w.r.t. the robot’s configuration q.

Returns

the 3xn orientation Jacobian matrix of the link. The matrix is row-major.

This matrix J gives the link’s angular velocity (in world coordinates) via np.dot(J,dq), where dq is the robot’s joint velocities.

Return type

(list of 3 lists of floats)

getParent()[source]

Returns the index of the link’s parent (on its robot).

Returns

Return type

int

getParentTransform()[source]

Gets transformation (R,t) to the parent link.

Returns

a pair (R,t), with R a 9-list and t a 3-list of floats, giving the local transform from this link to its parent, in the reference (zero) configuration.

Return type

(se3 object)

getPointAcceleration(plocal, ddq)[source]

Returns the acceleration of the point given the robot’s current joint configuration and velocities, and the joint accelerations ddq.

Parameters
• plocal (list of 3 floats) –

• ddq (list of floats) –

Returns

the acceleration of the point, in world coordinates.

Return type

(list of 3 floats)

getPointVelocity(plocal)[source]

Returns the world velocity of a point attached to the link, given the robot’s current joint configuration and velocities.

Parameters

plocal (list of 3 floats) –

Returns

the current velocity of the point, in world coordinates.

Return type

(list of 3 floats)

getPositionHessian(plocal)[source]

Returns the Hessians of each component of the position p w.r.t the robot’s configuration q.

Parameters

plocal (list of 3 floats) –

Returns

a triple (Hx,Hy,Hz) of of nxn matrices corresponding, respectively, to the (x,y,z) components of the Hessian.

Return type

(3-tuple)

getPositionJacobian(plocal)[source]

Returns the position jacobian of a point on this link w.r.t. the robot’s configuration q.

Parameters

plocal (list of 3 floats) –

Returns

the 3xn Jacobian matrix of the point given by local coordinates plocal. The matrix is row-major.

This matrix J gives the point’s velocity (in world coordinates) via np.dot(J,dq), where dq is the robot’s joint velocities.

Return type

(list of 3 lists of floats)

getTransform()[source]

Gets the link’s current transformation (R,t) to the world frame.

Returns

a pair (R,t), with R a 9-list and t a 3-list of floats.

Return type

(se3 object)

getVelocity()[source]

Returns the velocity of the link’s origin given the robot’s current joint configuration and velocities. Equivalent to getPointVelocity([0,0,0]).

Returns

the current velocity of the link’s origin, in world coordinates

Return type

(list of 3 floats)

getWorldDirection(vlocal)[source]

Converts direction from local to world coordinates.

Parameters

vlocal (list of 3 floats) –

Returns

the world coordinates of the local direction vlocal

Return type

(list of 3 floats)

getWorldPosition(plocal)[source]

Converts point from local to world coordinates.

Parameters

plocal (list of 3 floats) –

Returns

the world coordinates of the local point plocal

Return type

(list of 3 floats)

property index

isPrismatic()[source]

Returns whether the joint is prismatic.

Returns

Return type

bool

isRevolute()[source]

Returns whether the joint is revolute.

Returns

Return type

bool

parent()[source]

Returns a reference to the link’s parent, or a NULL link if it has no parent.

Returns

Return type

RobotModelLink

robot()[source]

Returns a reference to the link’s robot.

Returns

Return type

RobotModel

property robotIndex

property robotPtr

setAxis(axis)[source]

Sets the local rotational / translational axis.

Parameters

axis (list of 3 floats) –

setMass(mass)[source]

Sets the inertial proerties of the link. (Note that the Mass is given with origin at the link frame, not about the COM.)

Parameters

mass (Mass) –

setName(name)[source]

Sets the name of the robot link.

Parameters

name (str) –

setParent(*args)[source]

Sets the link’s parent (must be on the same robot).

setParent (p)

setParent (l)

Parameters
setParentTransform(R, t)[source]

Sets transformation (R,t) to the parent link.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

setPrismatic(prismatic)[source]

Changes a link from revolute to prismatic or vice versa.

Parameters

prismatic (bool) –

setTransform(R, t)[source]

Sets the link’s current transformation (R,t) to the world frame.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

Note

This does NOT perform inverse kinematics. The transform is overwritten when the robot’s setConfig() method is called.

property world

class klampt.SimBody[source]

Bases: object

A reference to a rigid body inside a Simulator (either a RigidObjectModel, TerrainModel, or a link of a RobotModel).

Can use this class to directly apply forces to or control positions / velocities of objects in the simulation.

Note: All changes are applied in the current simulation substep, not the duration provided to Simulation.simulate(). If you need fine-grained control, make sure to call Simulation.simulate() with time steps equal to the value provided to Simulation.setSimStep() (this is 0.001s by default).

Note: The transform of the object is centered at the object’s center of mass rather than the reference frame given in the RobotModelLink or RigidObjectModel.

C++ includes: robotsim.h

A reference to a rigid body inside a Simulator (either a RigidObjectModel, TerrainModel, or a link of a RobotModel).

Can use this class to directly apply forces to or control positions / velocities of objects in the simulation.

Note: All changes are applied in the current simulation substep, not the duration provided to Simulation.simulate(). If you need fine-grained control, make sure to call Simulation.simulate() with time steps equal to the value provided to Simulation.setSimStep() (this is 0.001s by default).

Note: The transform of the object is centered at the object’s center of mass rather than the reference frame given in the RobotModelLink or RigidObjectModel.

C++ includes: robotsim.h

Methods:

 applyForceAtLocalPoint(f, plocal) Applies a force at a given point (in local center-of-mass-centered coordinates) over the duration of the next Simulator.simulate(t) call. applyForceAtPoint(f, pworld) Applies a force at a given point (in world coordinates) over the duration of the next Simulator.simulate(t) call. applyWrench(f, t) Applies a force and torque about the COM over the duration of the next Simulator.simulate(t) call. enable([enabled]) Sets the simulation of this body on/off. enableDynamics([enabled]) Turns dynamic simulation of the body on/off. returns Returns the object ID that this body associated with. Gets the body’s transformation at the current simulation time step (in object- native coordinates). Gets (a copy of) the surface properties. Gets the body’s transformation at the current simulation time step (in center- of-mass centered coordinates). Returns the angular velocity and translational velocity. returns Returns true if this body is being simulated. setCollisionPadding(padding) Sets the collision padding used for contact generation. setCollisionPreshrink([shrinkVisualization]) If set, preshrinks the geometry so that the padded geometry better matches the original mesh. Sets the body’s transformation at the current simulation time step (in object- native coordinates) setSurface(params) Sets the surface properties. setTransform(R, t) Sets the body’s transformation at the current simulation time step (in center- of-mass centered coordinates). setVelocity(w, v) Sets the angular velocity and translational velocity at the current simulation time step.

Attributes:

 body SimBody_body_get(SimBody self) -> dBodyID geometry SimBody_geometry_get(SimBody self) -> ODEGeometry * objectID SimBody_objectID_get(SimBody self) -> int sim SimBody_sim_get(SimBody self) -> Simulator
applyForceAtLocalPoint(f, plocal)[source]

Applies a force at a given point (in local center-of-mass-centered coordinates) over the duration of the next Simulator.simulate(t) call.

Parameters
• f (list of 3 floats) –

• plocal (list of 3 floats) –

applyForceAtPoint(f, pworld)[source]

Applies a force at a given point (in world coordinates) over the duration of the next Simulator.simulate(t) call.

Parameters
• f (list of 3 floats) –

• pworld (list of 3 floats) –

applyWrench(f, t)[source]

Applies a force and torque about the COM over the duration of the next Simulator.simulate(t) call.

Parameters
• f (list of 3 floats) –

• t (list of 3 floats) –

property body

SimBody_body_get(SimBody self) -> dBodyID

enable(enabled=True)[source]

Sets the simulation of this body on/off.

enable (enabled=True)

enable ()

Parameters

enabled (bool, optional) – default value True

enableDynamics(enabled=True)[source]

Turns dynamic simulation of the body on/off. If false, velocities will simply be integrated forward, and forces will not affect velocity i.e., it will be pure kinematic simulation.

enableDynamics (enabled=True)

enableDynamics ()

Parameters

enabled (bool, optional) – default value True

property geometry

SimBody_geometry_get(SimBody self) -> ODEGeometry *

getCollisionPadding()[source]
Returns

Return type

float

getID()[source]

Returns the object ID that this body associated with.

Returns

Return type

int

getObjectTransform()[source]

Gets the body’s transformation at the current simulation time step (in object- native coordinates).

getSurface()[source]

Gets (a copy of) the surface properties.

Returns

Return type

ContactParameters

getTransform()[source]

Gets the body’s transformation at the current simulation time step (in center- of-mass centered coordinates).

getVelocity()[source]

Returns the angular velocity and translational velocity.

isDynamicsEnabled()[source]
Returns

Return type

bool

isEnabled()[source]

Returns true if this body is being simulated.

Returns

Return type

bool

property objectID

SimBody_objectID_get(SimBody self) -> int

setCollisionPadding(padding)[source]

Sets the collision padding used for contact generation. At 0 padding the simulation will be unstable for triangle mesh and point cloud geometries. A larger value is useful to maintain simulation stability for thin or soft objects. Default is 0.0025.

Parameters

setCollisionPreshrink(shrinkVisualization=False)[source]

If set, preshrinks the geometry so that the padded geometry better matches the original mesh. If shrinkVisualization=true, the underlying mesh is also shrunk (helps debug simulation artifacts due to preshrink)

setCollisionPreshrink (shrinkVisualization=False)

setCollisionPreshrink ()

Parameters

shrinkVisualization (bool, optional) – default value False

setObjectTransform(R, t)[source]

Sets the body’s transformation at the current simulation time step (in object- native coordinates)

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

setSurface(params)[source]

Sets the surface properties.

Parameters

params (ContactParameters) –

setTransform(R, t)[source]

Sets the body’s transformation at the current simulation time step (in center- of-mass centered coordinates).

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

setVelocity(w, v)[source]

Sets the angular velocity and translational velocity at the current simulation time step.

Parameters
• w (list of 3 floats) –

• v (list of 3 floats) –

property sim

SimBody_sim_get(SimBody self) -> Simulator

class klampt.SimJoint[source]

Bases: object

An interface to ODE’s hinge and slider joints. You may use this to create custom objects, e.g., drawers, doors, cabinets, etc. It can also be used to attach objects together, e.g., an object to a robot’s gripper.

C++ includes: robotsim.h

Attributes:

 a SimJoint_a_get(SimJoint self) -> SimBody b SimJoint_b_get(SimJoint self) -> SimBody joint SimJoint_joint_get(SimJoint self) -> dJointID type SimJoint_type_get(SimJoint self) -> int

Methods:

 addForce(force) Adds a torque for the hinge joint and a force for a slider joint. Removes the joint from the simulation. makeFixed(a, b) Creates a fixed joint between a and b. makeHinge(a,b,pt,axis) makeHinge (a,pt,axis) makeSlider(a,b,axis) makeSlider (a,axis) setFriction(friction) Sets the (dry) friction of the joint. setLimits(min, max) Sets the joint limits, relative to the initial configuration of the bodies. setVelocity(vel, fmax) Locks velocity of the joint, up to force fmax.
property a

SimJoint_a_get(SimJoint self) -> SimBody

addForce(force)[source]

Adds a torque for the hinge joint and a force for a slider joint.

Parameters

force (float) –

property b

SimJoint_b_get(SimJoint self) -> SimBody

destroy()[source]

Removes the joint from the simulation.

property joint

SimJoint_joint_get(SimJoint self) -> dJointID

makeFixed(a, b)[source]

Creates a fixed joint between a and b. (There’s no method to fix a to the world; just call a.enableDynamics(False))

Parameters
makeHinge(a, b, pt, axis)[source]

makeHinge (a,pt,axis)

Parameters
makeSlider(a, b, axis)[source]

makeSlider (a,axis)

Parameters
setFriction(friction)[source]

Sets the (dry) friction of the joint.

Parameters

friction (float) –

setLimits(min, max)[source]

Sets the joint limits, relative to the initial configuration of the bodies. Units are in radians for hinges and meters for sliders.

Parameters
• min (float) –

• max (float) –

setVelocity(vel, fmax)[source]

Locks velocity of the joint, up to force fmax. Can’t be used with setFriction.

Parameters
• vel (float) –

• fmax (float) –

property type

SimJoint_type_get(SimJoint self) -> int

class klampt.SimRobotController[source]

Bases: object

A controller for a simulated robot.

By default a SimRobotController has three possible modes:

• Motion queue + PID mode: the controller has an internal trajectory queue that may be added to and modified. This queue supports piecewise linear interpolation, cubic interpolation, and time-optimal move-to commands.

• PID mode: the user controls the motor’s PID setpoints directly

• Torque control: the user controlls the motor torques directly.

The “standard” way of using this is in move-to mode which accepts a milestone (setMilestone) or list of milestones (repeated calls to addMilestone) and interpolates dynamically from the current configuration/velocity. To handle disturbances, a PID loop is run automatically at the controller’s specified rate.

To get finer-grained control over the motion queue’s timing, you may use the setLinear/setCubic/addLinear/addCubic functions. In these functions it is up to the user to respect velocity, acceleration, and torque limits.

Whether in motion queue or PID mode, the constants of the PID loop are initially set in the robot file. You can programmatically tune these via the setPIDGains function.

Arbitrary trajectories can be tracked by using setVelocity over short time steps. Force controllers can be implemented using setTorque, again using short time steps.

If the setVelocity, setTorque, or setPID command are called, the motion queue behavior will be completely overridden. To reset back to motion queue control, setManualMode(False) must be called first.

Individual joints cannot be addressed with mixed motion queue mode and torque/PID mode. However, you can mix PID and torque mode between different joints with a workaround:

# setup by zeroing out PID constants for torque controlled joints
pid_joint_indices = [...]
torque_joint_indices = [...] # complement of pid_joint_indices
kp,ki,kp = controller.getPIDGains()
for i in torque_joint_indices:  #turn off PID gains here
kp[i] = ki[i] = kp[i] = 0

# to send PID command (qcmd,dqcmd) and torque commands tcmd, use
# a PID command with feedforward torques.  First we build a whole-robot
# command:
for i,k in enumerate(pid_joint_indices):
qcmd_whole[k],dqcmd_whole[i] = qcmd[i],dqcmd[i]
for i,k in enumerate(torque_joint_indices):
tcmd_whole[k] = tcmd[i]
# Then we send it to the controller
controller.setPIDCommand(qcmd_whole,dqcmd_whole,tcmd_whole)


C++ includes: robotsim.h

Methods:

 addCubic(q, v, dt) Same as setCubic but appends an interpolant onto the motion queue. addLinear(q, dt) Same as setLinear but appends an interpolant onto the motion queue. addMilestone(*args) Same as setMilestone, but appends an interpolant onto an internal motion queue starting at the current queued end state. Same as addMilestone, but enforces that the motion should move along a straight- line joint-space path. gets a custom command list Returns the current commanded configuration (size model().numLinks()) Returns the current commanded (feedforward) torque (size model().numDrivers()) Returns the current commanded velocity (size model().numLinks()) Returns the control type for the active controller. Gets the PID gains for the PID controller. Gets the current feedback control rate, in s. Returns the current “sensed” configuration from the simulator (size model().numLinks()) Returns the current “sensed” (feedback) torque from the simulator. Returns the current “sensed” velocity from the simulator (size model().numLinks()) getSetting(name) gets a setting of the controller Retrieves the robot model associated with this controller. Returns the remaining duration of the motion queue. sendCommand(name, args) sends a custom string command to the controller sensor(*args) Returns a sensor by index or by name. setCubic(q, v, dt) Uses cubic (Hermite) interpolation to get from the current configuration/velocity to the desired configuration/velocity after time dt. setLinear(q, dt) Uses linear interpolation to get from the current configuration to the desired configuration after time dt. setManualMode(enabled) Turns on/off manual mode, if either the setTorque or setPID command were previously set. setMilestone(*args) Uses a dynamic interpolant to get from the current state to the desired milestone (with optional ending velocity). setPIDCommand(*args) Sets a PID command controller. setPIDGains(kP, kI, kD) Sets the PID gains. Sets the current feedback control rate, in s. setSetting(name, val) sets a setting of the controller Sets a torque command controller. setVelocity(dq, dt) Sets a rate controller from the current commanded config to move at rate dq for time dt > 0. Returns all valid setting names.

Attributes:

 controller SimRobotController_controller_get(SimRobotController self) -> ControlledRobotSimulator * index SimRobotController_index_get(SimRobotController self) -> int sim SimRobotController_sim_get(SimRobotController self) -> Simulator
addCubic(q, v, dt)[source]

Same as setCubic but appends an interpolant onto the motion queue.

Parameters
• q (list of floats) –

• v (list of floats) –

• dt (float) –

addLinear(q, dt)[source]

Same as setLinear but appends an interpolant onto the motion queue.

Parameters
• q (list of floats) –

• dt (float) –

addMilestone(*args)[source]

Same as setMilestone, but appends an interpolant onto an internal motion queue starting at the current queued end state.

Parameters
• q (list of floats) –

• dq (list of floats, optional) –

addMilestoneLinear(q)[source]

Same as addMilestone, but enforces that the motion should move along a straight- line joint-space path.

Parameters

q (list of floats) –

commands()[source]

gets a custom command list

Returns

Return type

stringVector

property controller

SimRobotController_controller_get(SimRobotController self) -> ControlledRobotSimulator *

getCommandedConfig()[source]

Returns the current commanded configuration (size model().numLinks())

getCommandedTorque()[source]

Returns the current commanded (feedforward) torque (size model().numDrivers())

getCommandedVelocity()[source]

Returns the current commanded velocity (size model().numLinks())

getControlType()[source]

Returns the control type for the active controller.

Returns

Return type

str

Possible return values are:

• unknown

• off

• torque

• PID

• locked_velocity

getPIDGains()[source]

Gets the PID gains for the PID controller.

getRate()[source]

Gets the current feedback control rate, in s.

Returns

Return type

float

getSensedConfig()[source]

Returns the current “sensed” configuration from the simulator (size model().numLinks())

getSensedTorque()[source]

Returns the current “sensed” (feedback) torque from the simulator. (size model().numDrivers())

Note: a default robot doesn’t have a torque sensor, so this will be 0

getSensedVelocity()[source]

Returns the current “sensed” velocity from the simulator (size model().numLinks())

getSetting(name)[source]

gets a setting of the controller

Parameters

name (str) –

Returns

Return type

str

property index

SimRobotController_index_get(SimRobotController self) -> int

model()[source]

Retrieves the robot model associated with this controller.

Returns

Return type

RobotModel

remainingTime()[source]

Returns the remaining duration of the motion queue.

Returns

Return type

float

sendCommand(name, args)[source]

sends a custom string command to the controller

Parameters
• name (str) –

• args (str) –

Returns

Return type

bool

sensor(*args)[source]

Returns a sensor by index or by name. If out of bounds or unavailable, a null sensor is returned (i.e., SimRobotSensor.name() or SimRobotSensor.type()) will return the empty string.)

sensor (index): SimRobotSensor

sensor (name): SimRobotSensor

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type
setCubic(q, v, dt)[source]

Uses cubic (Hermite) interpolation to get from the current configuration/velocity to the desired configuration/velocity after time dt.

Parameters
• q (list of floats) –

• v (list of floats) –

• dt (float) –

q and v have size model().numLinks(). dt must be > 0.

setLinear(q, dt)[source]

Uses linear interpolation to get from the current configuration to the desired configuration after time dt.

Parameters
• q (list of floats) –

• dt (float) –

q has size model().numLinks(). dt must be > 0.

setManualMode(enabled)[source]

Turns on/off manual mode, if either the setTorque or setPID command were previously set.

Parameters

enabled (bool) –

setMilestone(*args)[source]

Uses a dynamic interpolant to get from the current state to the desired milestone (with optional ending velocity). This interpolant is time-optimal with respect to the velocity and acceleration bounds.

setMilestone (q)

setMilestone (q,dq)

Parameters
• q (list of floats) –

• dq (list of floats, optional) –

setPIDCommand(*args)[source]

Sets a PID command controller. If tfeedforward is provided, it is the feedforward torque vector.

setPIDCommand (qdes,dqdes)

setPIDCommand (qdes,dqdes,tfeedforward)

Parameters
• qdes (list of floats) –

• dqdes (list of floats) –

• tfeedforward (list of floats, optional) –

setPIDGains(kP, kI, kD)[source]

Sets the PID gains. Arguments have size model().numDrivers().

Parameters
• kP (list of floats) –

• kI (list of floats) –

• kD (list of floats) –

setRate(dt)[source]

Sets the current feedback control rate, in s.

Parameters

dt (float) –

setSetting(name, val)[source]

sets a setting of the controller

Parameters
• name (str) –

• val (str) –

Returns

Return type

bool

setTorque(t)[source]

Sets a torque command controller. t can have size model().numDrivers() or model().numLinks().

Parameters

t (list of floats) –

setVelocity(dq, dt)[source]

Sets a rate controller from the current commanded config to move at rate dq for time dt > 0. dq has size model().numLinks()

Parameters
• dq (list of floats) –

• dt (float) –

settings()[source]

Returns all valid setting names.

Returns

Return type

stringVector

property sim

SimRobotController_sim_get(SimRobotController self) -> Simulator

class klampt.SimRobotSensor(*args)[source]

Bases: object

A sensor on a simulated robot. Retrieve one from the controller using SimRobotController.getSensor(), or create a new one using SimRobotSensor(robotController,name,type)

Use getMeasurements() to get the currently simulated measurement vector.

Sensors are automatically updated through the Simulator.simulate() call, and getMeasurements() retrieves the updated values. As a result, you may get garbage measurements before the first Simulator.simulate call is made.

There is also a mode for doing kinematic simulation, which is supported (i.e., makes sensible measurements) for some types of sensors when just a robot / world model is given. This is similar to Simulation.fakeSimulate but the entire controller structure is bypassed. You can arbitrarily set the robot’s position, call kinematicReset(), and then call kinematicSimulate(). Subsequent calls assume the robot is being driven along a trajectory until the next kinematicReset() is called.

LaserSensor, CameraSensor, TiltSensor, AccelerometerSensor, GyroSensor, JointPositionSensor, JointVelocitySensor support kinematic simulation mode. FilteredSensor and TimeDelayedSensor also work. The force-related sensors (ContactSensor and ForceTorqueSensor) return 0’s in kinematic simulation.

To use get/setSetting, you will need to know the sensor attribute names and types as described in the Klampt sensor documentation (same as in the world or sensor XML file).

C++ includes: robotsim.h

__init__ (robot,sensor): SimRobotSensor

__init__ (robot,name,type): SimRobotSensor

Parameters
• sensor (SensorBase, optional) –

• name (str, optional) –

• type (str, optional) –

Methods:

 drawGL(*args) Draws a sensor indicator using OpenGL. Returns a list of measurements from the previous simulation (or kinematicSimulate) timestep. getSetting(name) Returns the value of the named setting (you will need to manually parse this) resets a kinematic simulation so that a new initial condition can be set kinematicSimulate(world,dt) kinematicSimulate (dt) Returns a list of names for the measurements (one per measurement). Returns the name of the sensor. Returns the model of the robot to which this belongs. setSetting(name, val) Sets the value of the named setting (you will need to manually cast an int/float/etc to a str) Returns all setting names. Returns the type of the sensor.

Attributes:

 robotModel SimRobotSensor_robotModel_get(SimRobotSensor self) -> RobotModel sensor SimRobotSensor_sensor_get(SimRobotSensor self) -> SensorBase *
drawGL(*args)[source]

Draws a sensor indicator using OpenGL. If measurements are given, the indicator is drawn as though these are the latest measurements, otherwise only an indicator is drawn.

drawGL ()

drawGL (measurements)

Parameters

measurements (list of floats, optional) –

getMeasurements()[source]

Returns a list of measurements from the previous simulation (or kinematicSimulate) timestep.

getSetting(name)[source]

Returns the value of the named setting (you will need to manually parse this)

Parameters

name (str) –

Returns

Return type

str

kinematicReset()[source]

resets a kinematic simulation so that a new initial condition can be set

kinematicSimulate(world, dt)[source]

kinematicSimulate (dt)

Parameters
measurementNames()[source]

Returns a list of names for the measurements (one per measurement).

Returns

Return type

stringVector

name()[source]

Returns the name of the sensor.

Returns

Return type

str

robot()[source]

Returns the model of the robot to which this belongs.

Returns

Return type

RobotModel

property robotModel

SimRobotSensor_robotModel_get(SimRobotSensor self) -> RobotModel

property sensor

SimRobotSensor_sensor_get(SimRobotSensor self) -> SensorBase *

setSetting(name, val)[source]

Sets the value of the named setting (you will need to manually cast an int/float/etc to a str)

Parameters
• name (str) –

• val (str) –

settings()[source]

Returns all setting names.

Returns

Return type

stringVector

type()[source]

Returns the type of the sensor.

Returns

Return type

str

class klampt.Simulator(model)[source]

Bases: object

A dynamics simulator for a WorldModel.

C++ includes: robotsim.h

Constructs the simulator from a WorldModel. If the WorldModel was loaded from an XML file, then the simulation setup is loaded from it.

Parameters

model (WorldModel) –

Attributes:

 STATUS_ADAPTIVE_TIME_STEPPING STATUS_CONTACT_UNRELIABLE STATUS_ERROR STATUS_NORMAL STATUS_UNSTABLE index Simulator_index_get(Simulator self) -> int initialState Simulator_initialState_get(Simulator self) -> std::string const & sim Simulator_sim_get(Simulator self) -> WorldSimulation * world Simulator_world_get(Simulator self) -> WorldModel

Methods:

 body(*args) Returns the SimBody corresponding to the given link, rigid object, or terrain. Checks if any objects are overlapping. contactForce(aid, bid) Returns the contact force on object a at the last time step. contactTorque(aid, bid) Returns the contact force on object a (about a’s origin) at the last time step. controller(*args) Returns a controller for the indicated robot, either by index or by RobotModel. enableContactFeedback(obj1, obj2) Call this to enable contact feedback between the two objects (arguments are indexes returned by object.getID()). Call this to enable contact feedback between all pairs of objects. Advances a faked simulation by time t, and updates the world model from the faked simulation state. getActualConfig(robot) Returns the current actual configuration of the robot from the simulator. getActualTorque(robot) Returns the current actual torques on the robot’s drivers from the simulator. getActualTorques(robot) Deprecated: renamed to getActualTorque to be consistent with SimRobotController methods. getActualVelocity(robot) Returns the current actual velocity of the robot from the simulator. getContactForces(aid, bid) Returns the list of contact forces on object a at the last time step. getContacts(aid, bid) Returns the list of contacts (x,n,kFriction) at the last time step. getJointForces(link) Returns the joint force and torque local to the link, as would be read by a force-torque sensor mounted at the given link’s origin. getSetting(name) Retrieves some simulation setting. Returns a Base64 string representing the binary data for the current simulation state, including controller parameters, etc. Returns an indicator code for the simulator status. Returns a string indicating the simulator’s status. Returns the simulation time. hadContact(aid, bid) Returns true if the objects had contact over the last simulate() call. hadPenetration(aid, bid) Returns true if the objects interpenetrated during the last simulate() call. hadSeparation(aid, bid) Returns true if the objects had ever separated during the last simulate() call. inContact(aid, bid) Returns true if the objects (indexes returned by object.getID()) are in contact on the current time step. meanContactForce(aid, bid) Returns the average contact force on object a over the last simulate() call. Resets to the initial state (same as setState(initialState)) Sets the overall gravity vector. setSetting(name, value) Sets some simulation setting. Sets the internal simulation substep. setState(str) Sets the current simulation state from a Base64 string returned by a prior getState call. Returns all setting names. Advances the simulation by time t, and updates the world model from the simulation state. Updates the world model from the current simulation state.
STATUS_ADAPTIVE_TIME_STEPPING = 1
STATUS_CONTACT_UNRELIABLE = 2
STATUS_ERROR = 4
STATUS_NORMAL = 0
STATUS_UNSTABLE = 3
body(*args)[source]

Returns the SimBody corresponding to the given link, rigid object, or terrain.

body (link): SimBody

body (object): SimBody

body (terrain): SimBody

Parameters
Returns

Return type
checkObjectOverlap()[source]

Checks if any objects are overlapping. Returns a pair of lists of integers, giving the pairs of object ids that are overlapping.

contactForce(aid, bid)[source]

Returns the contact force on object a at the last time step. You can set bid to -1 to get the overall contact force on object a.

Parameters
• aid (int) –

• bid (int) –

contactTorque(aid, bid)[source]

Returns the contact force on object a (about a’s origin) at the last time step. You can set bid to -1 to get the overall contact force on object a.

Parameters
• aid (int) –

• bid (int) –

controller(*args)[source]

Returns a controller for the indicated robot, either by index or by RobotModel.

controller (robot): SimRobotController

Parameters

robot (int or RobotModel) –

Returns

Return type
enableContactFeedback(obj1, obj2)[source]

Call this to enable contact feedback between the two objects (arguments are indexes returned by object.getID()). Contact feedback has a small overhead so you may want to do this selectively. This must be called before using inContact, getContacts, getContactForces, contactForce, contactTorque, hadContact, hadSeparation, hadPenetration, and meanContactForce.

Parameters
• obj1 (int) –

• obj2 (int) –

enableContactFeedbackAll()[source]

Call this to enable contact feedback between all pairs of objects. Contact feedback has a small overhead so you may want to do this selectively.

fakeSimulate(t)[source]

Advances a faked simulation by time t, and updates the world model from the faked simulation state.

Parameters

t (float) –

getActualConfig(robot)[source]

Returns the current actual configuration of the robot from the simulator.

Parameters

robot (int) –

getActualTorque(robot)[source]

Returns the current actual torques on the robot’s drivers from the simulator.

Parameters

robot (int) –

getActualTorques(robot)[source]

Deprecated: renamed to getActualTorque to be consistent with SimRobotController methods.

Parameters

robot (int) –

getActualVelocity(robot)[source]

Returns the current actual velocity of the robot from the simulator.

Parameters

robot (int) –

getContactForces(aid, bid)[source]

Returns the list of contact forces on object a at the last time step.

Parameters
• aid (int) –

• bid (int) –

getContacts(aid, bid)[source]

Returns the list of contacts (x,n,kFriction) at the last time step. Normals point into object a. The contact point (x,n,kFriction) is represented as a 7-element vector.

Parameters
• aid (int) –

• bid (int) –

getJointForces(link)[source]

Returns the joint force and torque local to the link, as would be read by a force-torque sensor mounted at the given link’s origin. The 6 entries are (fx,fy,fz,mx,my,mz)

Parameters

link (RobotModelLink) –

getSetting(name)[source]

Retrieves some simulation setting.

Parameters

name (str) –

Returns

Return type

str

Valid names are:

• gravity: the gravity vector (default “0 0 -9.8”)

• simStep: the internal simulation step (default “0.001”)

• autoDisable: whether to disable bodies that don’t move much between time steps (default “0”, set to “1” for many static objects)

• boundaryLayerCollisions: whether to use the Klampt inflated boundaries for contact detection’(default “1”, recommended)

• rigidObjectCollisions: whether rigid objects should collide (default “1”)

• robotSelfCollisions: whether robots should self collide (default “0”)

• robotRobotCollisions: whether robots should collide with other robots (default “1”)

• adaptiveTimeStepping: whether adaptive time stepping should be used to improve stability. Slower but more stable. (default “1”)

• minimumAdaptiveTimeStep: the minimum size of an adaptive time step before giving up (default “1e-6”)

• maxContacts: max # of clustered contacts between pairs of objects (default “20”)

• clusterNormalScale: a parameter for clustering contacts (default “0.1”)

• errorReductionParameter: see ODE docs on ERP (default “0.95”)

• dampedLeastSquaresParameter: see ODE docs on CFM (default “1e-6”)

• instabilityConstantEnergyThreshold: parameter c0 in instability correction (default “1”)

• instabilityLinearEnergyThreshold: parameter c1 in instability correction (default “1.5”)

• instabilityMaxEnergyThreshold: parameter cmax in instability correction (default “100000”)

• instabilityPostCorrectionEnergy: kinetic energy scaling parameter if instability is detected (default “0.8”)

Instability correction kicks in whenever the kinetic energy K(t) of an object exceeds min(c0*m + c1*K(t-dt),cmax). m is the object’s mass.

See Klampt/Simulation/ODESimulator.h for detailed descriptions of these parameters.

getState()[source]

Returns a Base64 string representing the binary data for the current simulation state, including controller parameters, etc.

Returns

Return type

str

getStatus()[source]

Returns an indicator code for the simulator status. The return result is one of the STATUS_X flags. (Technically, this returns the worst status over the last simulate() call)

Returns

Return type

int

getStatusString(s=- 1)[source]

Returns a string indicating the simulator’s status. If s is provided and >= 0, this function maps the indicator code s to a string.

getStatusString (s=-1): str

getStatusString (): str

Parameters

s (int, optional) – default value -1

Returns

Return type

(str)

getTime()[source]

Returns the simulation time.

Returns

Return type

float

hadContact(aid, bid)[source]

Returns true if the objects had contact over the last simulate() call. You can set bid to -1 to determine if object a had contact with any other object.

Parameters
• aid (int) –

• bid (int) –

Returns

Return type

bool

hadPenetration(aid, bid)[source]

Returns true if the objects interpenetrated during the last simulate() call. If so, the simulation may lead to very inaccurate results or artifacts.

Parameters
• aid (int) –

• bid (int) –

Returns

Return type

bool

You can set bid to -1 to determine if object a penetrated any object, or you can set aid=bid=-1 to determine whether any object is penetrating any other (indicating that the simulation will not be functioning properly in general).

hadSeparation(aid, bid)[source]

Returns true if the objects had ever separated during the last simulate() call. You can set bid to -1 to determine if object a had no contact with any other object.

Parameters
• aid (int) –

• bid (int) –

Returns

Return type

bool

inContact(aid, bid)[source]

Returns true if the objects (indexes returned by object.getID()) are in contact on the current time step. You can set bid=-1 to tell if object a is in contact with any object.

Parameters
• aid (int) –

• bid (int) –

Returns

Return type

bool

property index

Simulator_index_get(Simulator self) -> int

property initialState

Simulator_initialState_get(Simulator self) -> std::string const &

meanContactForce(aid, bid)[source]

Returns the average contact force on object a over the last simulate() call.

Parameters
• aid (int) –

• bid (int) –

reset()[source]

Resets to the initial state (same as setState(initialState))

setGravity(g)[source]

Sets the overall gravity vector.

Parameters

g (list of 3 floats) –

setSetting(name, value)[source]

Sets some simulation setting. Raises an exception if the name is unknown or the value is of improper format.

Parameters
• name (str) –

• value (str) –

setSimStep(dt)[source]

Sets the internal simulation substep. Values < 0.01 are recommended.

Parameters

dt (float) –

setState(str)[source]

Sets the current simulation state from a Base64 string returned by a prior getState call.

Parameters

str (str) –

settings()[source]

Returns all setting names.

Returns

Return type

stringVector

property sim

Simulator_sim_get(Simulator self) -> WorldSimulation *

simulate(t)[source]

Advances the simulation by time t, and updates the world model from the simulation state.

Parameters

t (float) –

updateWorld()[source]

Updates the world model from the current simulation state. This only needs to be called if you change the world model and want to revert back to the simulation state.

property world

Simulator_world_get(Simulator self) -> WorldModel

class klampt.TerrainModel[source]

Bases: object

Static environment geometry.

C++ includes: robotmodel.h

Methods:

 Returns a reference to the appearance associated with this object. drawGL([keepAppearance]) Draws the object’s geometry. Returns a reference to the geometry associated with this object. Returns the ID of the terrain in its world. returns Loads the terrain from the file fn. saveFile(fn[, geometryName]) Saves the terrain to the file fn. setFriction(friction) Changes the friction coefficient for this terrain. setName(name) param name

Attributes:

 index TerrainModel_index_get(TerrainModel self) -> int terrain TerrainModel_terrain_get(TerrainModel self) -> Terrain * world TerrainModel_world_get(TerrainModel self) -> int
appearance()[source]

Returns a reference to the appearance associated with this object.

Returns

Return type

Appearance

drawGL(keepAppearance=True)[source]

Draws the object’s geometry. If keepAppearance=true, the current appearance is honored. Otherwise, only the raw geometry is drawn.

drawGL (keepAppearance=True)

drawGL ()

Parameters

keepAppearance (bool, optional) – default value True

PERFORMANCE WARNING: if keepAppearance is false, then this does not properly reuse OpenGL display lists. A better approach is to change the object’s Appearance directly.

geometry()[source]

Returns a reference to the geometry associated with this object.

Returns

Return type

Geometry3D

getID()[source]

Returns the ID of the terrain in its world.

Returns

Return type

int

Note: The world ID is not the same as the terrain index.

getName()[source]
Returns

Return type

str

property index

TerrainModel_index_get(TerrainModel self) -> int

loadFile(fn)[source]

Loads the terrain from the file fn.

Parameters

fn (str) –

Returns

Return type

bool

saveFile(fn, geometryName=None)[source]

Saves the terrain to the file fn. If geometryName is given, the geometry is saved to that file.

saveFile (fn,geometryName=None): bool

saveFile (fn): bool

Parameters
• fn (str) –

• geometryName (str, optional) – default value None

Returns

Return type

(bool)

setFriction(friction)[source]

Changes the friction coefficient for this terrain.

Parameters

friction (float) –

setName(name)[source]
Parameters

name (str) –

property terrain

TerrainModel_terrain_get(TerrainModel self) -> Terrain *

property world

TerrainModel_world_get(TerrainModel self) -> int

class klampt.TriangleMesh[source]

Bases: object

A 3D indexed triangle mesh class.

vertices

a list of vertices, given as a flattened coordinate list [x1, y1, z1, x2, y2, …]

Type

SWIG vector of floats

indices

a list of triangle vertices given as indices into the vertices list, i.e., [a1,b1,c2, a2,b2,c2, …]

Type

SWIG vector of ints

Note: because the bindings are generated by SWIG, you can access the indices / vertices members via some automatically generated accessors / modifiers. In particular len(), append(), and indexing via [] are useful. Some other methods like resize() are also provided. However, you CANNOT set these items via assignment.

Examples:

m = TriangleMesh()
m.vertices.append(0)
m.vertices.append(0)
m.vertices.append(0)
print(len(m.vertices))  #prints 3
m.vertices = [0,0,0]   #this is an error
m.vertices += [1,2,3]   #this is also an error


To get all vertices as a numpy array:

verts = np.array(m.vertices).reshape((len(m.vertices)//3,3))


To get all indices as a numpy array:

inds = np.array(m.indices,dtype=np.int32).reshape((len(m.indices)//3,3))


(Or use the convenience functions in klampt.io.numpy_convert)

C++ includes: geometry.h

Attributes:

 indices TriangleMesh_indices_get(TriangleMesh self) -> intVector vertices TriangleMesh_vertices_get(TriangleMesh self) -> doubleVector

Methods:

 transform(R, t) Transforms all the vertices by the rigid transform v=R*v+t. Translates all the vertices by v=v+t.
property indices

TriangleMesh_indices_get(TriangleMesh self) -> intVector

transform(R, t)[source]

Transforms all the vertices by the rigid transform v=R*v+t.

Parameters
• R (list of 9 floats (so3 element)) –

• t (list of 3 floats) –

translate(t)[source]

Translates all the vertices by v=v+t.

Parameters

t (list of 3 floats) –

property vertices

TriangleMesh_vertices_get(TriangleMesh self) -> doubleVector

class klampt.VolumeGrid[source]

Bases: object

An axis-aligned volumetric grid, typically a signed distance transform with > 0 indicating outside and < 0 indicating inside. Can also store an occupancy grid with 1 indicating inside and 0 indicating outside.

bbox

contains min and max bounds (xmin,ymin,zmin),(xmax,ymax,zmax)

Type

SWIG vector of 6 doubles

dims

size of grid in each of 3 dimensions

Type

SWIG vector of of 3 ints

values

contains a 3D array of dims[0]*dims[1]*dims[1] values.

The cell index (i,j,k) is flattened to i*dims[1]*dims[2] + j*dims[2] + k.

The array index i is associated to cell index (i/(dims[1]*dims[2]), (i/dims[2]) % dims[1], i%dims[2])

Type

SWIG vector of doubles

C++ includes: geometry.h

Attributes:

 bbox VolumeGrid_bbox_get(VolumeGrid self) -> doubleVector dims VolumeGrid_dims_get(VolumeGrid self) -> intVector values VolumeGrid_values_get(VolumeGrid self) -> doubleVector

Methods:

 get(i, j, k) param i resize(sx, sy, sz) param sx set(value) set (i,j,k,value) setBounds(bmin, bmax) param bmin shift(dv) param dv
property bbox

VolumeGrid_bbox_get(VolumeGrid self) -> doubleVector

property dims

VolumeGrid_dims_get(VolumeGrid self) -> intVector

get(i, j, k)[source]
Parameters
• i (int) –

• j (int) –

• k (int) –

Returns

Return type

float

resize(sx, sy, sz)[source]
Parameters
• sx (int) –

• sy (int) –

• sz (int) –

set(value)[source]

set (i,j,k,value)

Parameters
• value (float) –

• i (int, optional) –

• j (int, optional) –

• k (int, optional) –

setBounds(bmin, bmax)[source]
Parameters
• bmin (list of 3 floats) –

• bmax (list of 3 floats) –

shift(dv)[source]
Parameters

dv (float) –

property values

VolumeGrid_values_get(VolumeGrid self) -> doubleVector

class klampt.WorldModel(*args)[source]

Bases: object

The main world class, containing robots, rigid objects, and static environment geometry.

Every robot/robot link/terrain/rigid object is given a unique ID in the world. This is potentially a source of confusion because some functions take IDs and some take indices. Only the WorldModel and Simulator classes use IDs when the argument has ‘id’ as a suffix, e.g., geometry(), appearance(), Simulator.inContact(). All other functions use indices, e.g. robot(0), terrain(0), etc.

To get an object’s ID, you can see the value returned by loadElement and/or object.getID(). states.

To save/restore the state of the model, you must manually maintain copies of the states of whichever objects you wish to save/restore.

C++ includes: robotmodel.h

Creates a WorldModel.

__init__ (): WorldModel

__init__ (ptrRobotWorld): WorldModel

__init__ (w): WorldModel

__init__ (fn): WorldModel

Parameters
• Given no arguments, creates a new world.

• Given another WorldModel instance, creates a reference to an existing world. (To create a copy, use the copy() method.)

• Given a string, loads from a file. A PyException is raised on failure.

• Given a pointer to a C++ RobotWorld structure, a reference to that structure is returned. (This is advanced usage, seen only when interfacing C++ and Python code)

Methods:

 add(*args) Adds a copy of the given robot, rigid object, or terrain to this world, either from this WorldModel or another. Retrieves an appearance for a given element ID. Creates a copy of the world model. Draws the entire world using OpenGL. enableGeometryLoading(enabled) If geometry loading is set to false, then only the kinematics are loaded from disk, and no geometry / visualization / collision detection structures will be loaded. enableInitCollisions(enabled) If collision detection is set to true, then collision acceleration data structures will be automatically initialized, with debugging information. Retrieves a geometry for a given element ID. Retrieves the name for a given element ID. Loads some element from a file, automatically detecting its type. Alias of readFile. Loads a rigid object from a .obj or a mesh file. Loads a robot from a .rob or .urdf file. Loads a rigid object from a mesh file. Creates a new empty rigid object. makeRobot(name) Creates a new empty robot. makeTerrain(name) Creates a new empty terrain. Returns the total number of world ids. Returns the number of rigid objects. numRobotLinks(robot) Returns the number of links on the given robot. Returns the number of robots. Returns the number of terrains. Reads from a world XML file. remove(*args) Removes a robot, rigid object, or terrain from the world. rigidObject(*args) Returns a RigidObjectModel in the world by index or name. robot(*args) Returns a RobotModel in the world by index or name. robotLink(*args) Returns a RobotModelLink of some RobotModel in the world by index or name. saveFile(fn[, elementDir]) Saves to a world XML file. terrain(*args) Returns a TerrainModel in the world by index or name.

Attributes:

 index WorldModel_index_get(WorldModel self) -> int
add(*args)[source]

Adds a copy of the given robot, rigid object, or terrain to this world, either from this WorldModel or another.

add (name,robot): RobotModel

add (name,obj): RigidObjectModel

add (name,terrain): TerrainModel

Parameters
Returns

Return type
appearance(id)[source]

Retrieves an appearance for a given element ID.

Parameters

id (int) –

Returns

Return type

Appearance

copy()[source]

Creates a copy of the world model. Note that geometries and appearances are shared, so this is very quick.

Returns

Return type

WorldModel

drawGL()[source]

Draws the entire world using OpenGL.

enableGeometryLoading(enabled)[source]

If geometry loading is set to false, then only the kinematics are loaded from disk, and no geometry / visualization / collision detection structures will be loaded. Useful for quick scripts that just use kinematics / dynamics of a robot.

Parameters

enabled (bool) –

enableInitCollisions(enabled)[source]

If collision detection is set to true, then collision acceleration data structures will be automatically initialized, with debugging information. Useful for scripts that do planning and for which collision initialization may take a long time.

Parameters

enabled (bool) –

Note that even when this flag is off, the collision acceleration data structures will indeed be initialized the first time that geometry collision, distance, or ray-casting routines are called.

geometry(id)[source]

Retrieves a geometry for a given element ID.

Parameters

id (int) –

Returns

Return type

Geometry3D

getName(id)[source]

Retrieves the name for a given element ID.

Parameters

id (int) –

Returns

Return type

str

property index

WorldModel_index_get(WorldModel self) -> int

loadElement(fn)[source]

Loads some element from a file, automatically detecting its type. Meshes are interpreted as terrains. The ID is returned, or -1 if loading failed.

Parameters

fn (str) –

Returns

Return type

int

loadFile(fn)[source]

Parameters

fn (str) –

Returns

Return type

bool

loadRigidObject(fn)[source]

Loads a rigid object from a .obj or a mesh file. An empty rigid object is returned if loading fails.

Parameters

fn (str) –

Returns

Return type

RigidObjectModel

loadRobot(fn)[source]

Loads a robot from a .rob or .urdf file. An empty robot is returned if loading fails.

Parameters

fn (str) –

Returns

Return type

RobotModel

loadTerrain(fn)[source]

Parameters

fn (str) –

Returns

Return type

TerrainModel

makeRigidObject(name)[source]

Creates a new empty rigid object.

Parameters

name (str) –

Returns

Return type

RigidObjectModel

makeRobot(name)[source]

Creates a new empty robot. (Not terribly useful now since you can’t resize the number of links yet)

Parameters

name (str) –

Returns

Return type

RobotModel

makeTerrain(name)[source]

Creates a new empty terrain.

Parameters

name (str) –

Returns

Return type

TerrainModel

numIDs()[source]

Returns the total number of world ids.

Returns

Return type

int

numRigidObjects()[source]

Returns the number of rigid objects.

Returns

Return type

int

Returns the number of links on the given robot.

Parameters

robot (int) –

Returns

Return type

int

numRobots()[source]

Returns the number of robots.

Returns

Return type

int

numTerrains()[source]

Returns the number of terrains.

Returns

Return type

int

readFile(fn)[source]

Reads from a world XML file.

Parameters

fn (str) –

Returns

Return type

bool

remove(*args)[source]

Removes a robot, rigid object, or terrain from the world. It must be in this world or an exception is raised.

remove (robot)

remove (object)

remove (terrain)

Parameters

Important

All other RobotModel, RigidObjectModel, and TerrainModel references will be

invalidated.

rigidObject(*args)[source]

Returns a RigidObjectModel in the world by index or name.

rigidObject (index): RigidObjectModel

rigidObject (name): RigidObjectModel

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type
robot(*args)[source]

Returns a RobotModel in the world by index or name.

robot (index): RobotModel

robot (name): RobotModel

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type

Returns a RobotModelLink of some RobotModel in the world by index or name.

robotLink (robot,index): RobotModelLink

robotLink (robot,name): RobotModelLink

Parameters
• robot (str or int) –

• index (int, optional) –

• name (str, optional) –

Returns

Return type
saveFile(fn, elementDir=None)[source]

Saves to a world XML file. If elementDir is provided, then robots, terrains, etc. will be saved there. Otherwise they will be saved to a folder with the same base name as fn (without the trailing .xml)

saveFile (fn,elementDir=None): bool

saveFile (fn): bool

Parameters
• fn (str) –

• elementDir (str, optional) – default value None

Returns

Return type

(bool)

terrain(*args)[source]

Returns a TerrainModel in the world by index or name.

terrain (index): TerrainModel

terrain (name): TerrainModel

Parameters
• index (int, optional) –

• name (str, optional) –

Returns

Return type
klampt.robotsim.setRandomSeed(seed)[source]

Sets the random seed used by the configuration sampler.

Parameters

seed (int) –

klampt.robotsim.SampleTransform(*args)[source]

Returns a transformation (R,t) from link relative to link2, sampled at random from the space of transforms that satisfies the objective obj.

SampleTransform (obj)

Parameters

obj (GeneralizedIKObjective or IKObjective) –

klampt.robotsim.comEquilibrium(*args)[source]

Tests whether the given COM com is stable for the given contacts and the given external force fext.

comEquilibrium (contacts,fext,com): object

comEquilibrium (contactPositions,frictionCones,fext,com): object

The 2-argument version is a “fancy” version that allows more control over the constraint planes.

Args: contacts (list of 7-float lists or tuples): the list of contacts, each specified as a 7-list or tuple [x,y,z,nx,ny,nz,k], with:

• (x,y,z): the contact position

• (nx,ny,nz): the contact normal

• k: the coefficient of friction (>= 0)

contactPositions (list of 3-float lists or tuples): the list of contact point positions. frictionCones (list of lists): Each item of this list specifies linear inequalities that must be met of the force at the corresponding contact point. The item must have length k*4 where k is an integer, and each inequality gives the entries (ax,ay,az,b) of a constraint ax*fx+ay*fy+az*fz <= b that limits the contact force (fx,fy,fz) at the i’th contact. Each of the k 4-tuples is laid out sequentially per-contact. fext (3-tuple or list): the external force vector. com (3-tuple or list, or None): the center of mass coordinates. If None, assumes that you want to test whether ANY COM may be in equilibrium for the given contacts.

Returns

if com is given, and there are feasible

equilibrium forces, this returns a list of 3 tuples giving equilibrium forces at each of the contacts. None is returned if no such forces exist.

If com = None, the result is True or False.

Return type

(bool, None, or list)

klampt.robotsim.comEquilibrium2D(*args)[source]

Tests whether the given COM com is stable for the given contacts and the given external force fext.

comEquilibrium2D (contacts,fext,com): object

comEquilibrium2D (contactPositions,frictionCones,fext,com): object

The 2-argument version is a “fancy” version that allows more control over the constraint planes.

Parameters
• contacts (list of 4-float lists or tuples) –

the list of contacts, each specified as a 4-list or tuple [x,y,theta,k], with:

• (x,y,z): the contact position

• theta: is the normal angle (in radians, CCW to the x axis)

• k: the coefficient of friction (>= 0)

• contactPositions (list of 2-float lists or tuples) – the list of contact point positions.

• frictionCones (list of lists) – The i’th element in this list has length k*3 (for some integer k), and gives the contact force constraints (ax,ay,b) where ax*fx+ay*fy <= b limits the contact force (fx,fy) at the i’th contact. Each of the k 3-tuples is laid out sequentially per-contact.

• fext (2-tuple or list) – the external force vector.

• com (2-tuple or list, or None) – the center of mass coordinates. If None, assumes that you want to test whether ANY COM may be in equilibrium for the given contacts.

Returns

if com is given, and there are feasible

equilibrium forces, this returns a list of 2-tuples giving equilibrium forces at each of the contacts. None is returned if no such forces exist.

If com = None, the result is True or False.

Return type

(bool, None, or list)

klampt.robotsim.equilibriumTorques(*args)[source]

Solves for the torques / forces that keep the robot balanced against gravity.

equilibriumTorques (robot,contacts,links,fext,norm=0): object

equilibriumTorques (robot,contacts,links,fext): object

equilibriumTorques (robot,contacts,links,fext,internalTorques,norm=0): object

equilibriumTorques (robot,contacts,links,fext,internalTorques): object

The problem being solved is

$$min_{t,f_1,...,f_N} \|t\|_p$$

$$s.t. t_{int} + G(q) = t + sum_{i=1}^N J_i(q)^T f_i$$

$$|t| \leq t_{max}$$

$$f_i \in FC_i$$

Parameters
• robot (RobotModel) – the robot, posed in its current configuration

• contacts (list of N 7-lists) – a list of contact points, given as 7-lists [x,y,z,nx,ny,nz,kFriction]

• links (list of N ints) – a list of the links on which those contact points lie

• fext (list of 3 floats) – the external force (e.g., gravity)

• norm (double) –

the torque norm to minimize.

• If 0, minimizes the l-infinity norm (default)

• If 1, minimizes the l-1 norm.

• If 2, minimizes the l-2 norm (experimental, may not get good results).

• internalTorques (list of robot.numLinks() floats, optional) –

allows you to solve for dynamic situations, e.g., with coriolis forces taken into account. These are added to the RHS of the torque balance equation. If not given, t_int is assumed to be zero.

To use dynamics, set the robot’s joint velocities dq, calculate then calculate the torques via robot.torquesFromAccel(ddq), and pass the result into internalTorques.

Returns

a pair (torque,force) if a solution exists,

giving valid joint torques t and frictional contact forces (f1,…,fn).

None is returned if no solution exists.

Return type

(pair of lists, optional)

klampt.robotsim.forceClosure(*args)[source]

Returns true if the list of contact points has force closure.

forceClosure (contacts): bool

forceClosure (contactPositions,frictionCones): bool

Returns

Return type

(bool)

In the 1-argument version, each contact point is specified by a list of 7 floats, [x,y,z,nx,ny,nz,k] where (x,y,z) is the position, (nx,ny,nz) is the normal, and k is the coefficient of friction.

The 2-argument version is a “fancy” version that allows more control over the constraint planes.

Parameters
• contacts (list of 7-float lists or tuples) –

the list of contacts, each specified as a 7-list or tuple [x,y,z,nx,ny,nz,k], with:

• (x,y,z): the contact position

• (nx,ny,nz): the contact normal

• k: the coefficient of friction (>= 0)

• contactPositions (list of 3-float lists or tuples) – the list of contact point positions.

• frictionCones (list of lists) – Each item of this list specifies linear inequalities that must be met of the force at the corresponding contact point. The item must have length k*4 where k is an integer, and each inequality gives the entries (ax,ay,az,b) of a constraint ax*fx+ay*fy+az*fz <= b that limits the contact force (fx,fy,fz) at the i’th contact. Each of the k 4-tuples is laid out sequentially per-contact.

klampt.robotsim.forceClosure2D(*args)[source]

Returns true if the list of 2D contact points has force closure.

forceClosure2D (contacts): bool

forceClosure2D (contactPositions,frictionCones): bool

Returns

Return type

(bool)

In the 1-argument version, each contact point is given by a list of 4 floats, [x,y,theta,k] where (x,y) is the position, theta is the normal angle, and k is the coefficient of friction

The 2-argument version is a “fancy” version that allows more control over the constraint planes.

Parameters
• contacts (list of 4-float lists or tuples) –

the list of contacts, each specified as a 4-list or tuple [x,y,theta,k], with:

• (x,y): the contact position

• theta: is the normal angle (in radians, CCW to the x axis)

• k: the coefficient of friction (>= 0)

• contactPositions (list of 2-float lists or tuples) – the list of contact point positions.

• frictionCones (list of lists) – The i’th element in this list has length k*3 (for some integer k), and gives the contact force constraints (ax,ay,b) where ax*fx+ay*fy <= b limits the contact force (fx,fy) at the i’th contact. Each of the k 3-tuples is laid out sequentially per-contact.

klampt.robotsim.setFrictionConeApproximationEdges(numEdges)[source]

Globally sets the number of edges used in the friction cone approximation. The default value is 4.

Parameters

numEdges (int) –

klampt.robotsim.supportPolygon(*args)[source]

Calculates the support polygon for a given set of contacts and a downward external force (0,0,-g).

supportPolygon (contacts): object

supportPolygon (contactPositions,frictionCones): object

In the 1-argument version, a contact point is given by a list of 7 floats, [x,y,z,nx,ny,nz,k] as usual. The 2-argument version is a “fancy” version that allows more control over the constraint planes.

Parameters
• contacts (list of 7-float lists or tuples) –

the list of contacts, each specified as a 7-list or tuple [x,y,z,nx,ny,nz,k], with:

• (x,y,z): the contact position

• (nx,ny,nz): the contact normal

• k: the coefficient of friction (>= 0)

• contactPositions (list of 3-float lists or tuples) – the list of contact point positions.

• frictionCones (list of lists) – Each item of this list specifies linear inequalities that must be met of the force at the corresponding contact point. The item must have length k*4 where k is an integer, and each inequality gives the entries (ax,ay,az,b) of a constraint ax*fx+ay*fy+az*fz <= b that limits the contact force (fx,fy,fz) at the i’th contact. Each of the k 4-tuples is laid out sequentially per-contact.

Returns

The sorted plane boundaries of the support

polygon. The format of a plane is (nx,ny,ofs) where (nx,ny) are the outward facing normals, and ofs is the offset from 0. In other words to test stability of a com with x-y coordinates [x,y], you can test whether dot([nx,ny],[x,y]) <= ofs for all planes.

Hint: with numpy, you can do:

Ab = np.array(supportPolygon(args))
A=Ab[:,0:2]
b=Ab[:,2]
myComEquilibrium = lambda x: np.all(np.dot(A,x)<=b)


Return type

(list of 3-tuples)

klampt.robotsim.supportPolygon2D(*args)[source]

Calculates the support polygon (interval) for a given set of contacts and a downward external force (0,-g).

supportPolygon2D (contacts): object

supportPolygon2D (contacts,frictionCones): object

The 2-argument version is a “fancy” version that allows more control over the constraint planes.

Parameters
• contacts (list of 4-float lists or tuples) –

the list of contacts, each specified as a 4-list or tuple [x,y,theta,k], with:

• (x,y,z): the contact position

• theta: is the normal angle (in radians, CCW to the x axis)

• k: the coefficient of friction (>= 0)

• contactPositions (list of 2-float lists or tuples) –

the list of contact

point positions.

frictionCones (list of lists): The i’th element in this list has length

k*3 (for some integer k), and gives the contact force constraints (ax,ay,b) where ax*fx+ay*fy <= b limits the contact force (fx,fy) at the i’th contact. Each of the k 3-tuples is laid out sequentially per-contact.

Returns

gives the min/max extents of the support polygon.

If the support interval is empty, (inf,inf) is returned.

Return type

(2-tuple)

class klampt.robotsim.Widget[source]
class klampt.robotsim.WidgetSet[source]
class klampt.robotsim.ObjectPoser(object)[source]
Parameters

object (RigidObjectModel) –

class klampt.robotsim.RobotPoser(robot)[source]
Parameters

robot (RobotModel) –

class klampt.robotsim.PointPoser[source]
class klampt.robotsim.TransformPoser[source]
class klampt.robotsim.Viewport[source]