# 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, 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 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. RobotModel() A model of a dynamic and kinematic robot. RobotModelLink() A reference to a link of a RobotModel. RobotModelDriver() A reference to a driver of a RobotModel. RigidObjectModel() A rigid movable object. TerrainModel() Static environment geometry. Mass() Stores mass information for a rigid body or robot link. ContactParameters() 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) A three-D geometry. Appearance(*args) Geometry appearance information. GeometricPrimitive() A geometric primitive. TriangleMesh() A 3D indexed triangle mesh class. PointCloud() A 3D point cloud class. VolumeGrid() An axis-aligned volumetric grid, typically a signed distance transform with > 0 indicating outside and < 0 indicating inside. DistanceQuerySettings() Configures the _ext distance queries of Geometry3D. DistanceQueryResult() The result from a “fancy” distance 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. GeneralizedIKObjective(*args) An inverse kinematics target for matching points between two robots and/or objects. GeneralizedIKSolver(world) 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. SimBody() A reference to a rigid body inside a Simulator (either a RigidObjectModel, TerrainModel, or a link of a RobotModel). SimRobotController() A controller for a simulated robot. SimRobotSensor(robot, sensor) A sensor on a simulated robot.

## Equilibrium testing¶

See also the aliases in the klampt.model.contact module.

 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. setFrictionConeApproximationEdges(numEdges) 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. 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.

## Visualization¶

For use in GLWidgetProgram.

Widget()
WidgetSet()
ObjectPoser(object)
RobotPoser(robot)
PointPoser()
TransformPoser()
Viewport()

## Module contents¶

class klampt.WorldModel(*args)[source]

Bases: object

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

Note that this is just a model and can be changed at will – in fact planners and simulators will make use of a model to “display” computed

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: ptrRobotWorld (void, optional) – w (WorldModel, optional) – fn (str, optional) –
• 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)
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: name (str) – robot (RobotModel, optional) – obj (RigidObjectModel, optional) – terrain (TerrainModel, optional) –
appearance(id)[source]

Retrieves an appearance for a given element ID.

Parameters: id (int) – (Appearance)
copy()[source]

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

Returns: (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) – (Geometry3D)
getName(id)[source]

Retrieves the name for a given element ID.

Parameters: id (int) – (str)
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) – (int)
loadFile(fn)[source]

Parameters: fn (str) – (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) – (RigidObjectModel)
loadRobot(fn)[source]

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

Parameters: fn (str) – (RobotModel)
loadTerrain(fn)[source]

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

Parameters: fn (str) – (TerrainModel)
makeRigidObject(name)[source]

Creates a new empty rigid object.

Parameters: name (str) – (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) – (RobotModel)
makeTerrain(name)[source]

Creates a new empty terrain.

Parameters: name (str) – (TerrainModel)
numIDs()[source]
Returns: (int)
numRigidObjects()[source]
Returns: (int)
Parameters: robot (int) – (int)
numRobots()[source]
Returns: (int)
numTerrains()[source]
Returns: (int)
readFile(fn)[source]

Reads from a world XML file.

Parameters: fn (str) – (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: robot (RobotModel, optional) – object (RigidObjectModel, optional) – terrain (TerrainModel, optional) –

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) –
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 a RobotModelLink of some RobotModel in the world by index or name.

robotLink (robot,index): RobotModelLink

robotLink (robot,name): RobotModelLink

Parameters: robot (int or str) – index (int, optional) – name (str, optional) –
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 (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) –
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

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. (list of floats)
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) – (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) – (RobotModelDriver)
enableSelfCollision(link1, link2, value)[source]

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

Parameters: link1 (int) – link2 (int) – 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 (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) – (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. (list of floats)
getID()[source]

Returns the ID of the robot in its world.

Returns: (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) – (str)
getKineticEnergy()[source]

Returns the kinetic energy at the current config / velocity.

Returns: (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: (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]$$

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) – The configuration that is u fraction of the way from a to b (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) –
loadFile(fn)[source]

Loads the robot from the file fn.

Parameters: fn (str) – (bool)
numDrivers()[source]

Returns the number of drivers.

Returns: (int)

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

Returns: (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
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 (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: (bool)
selfCollisionEnabled(link1, link2)[source]

Queries whether self collisions between two links is enabled.

Parameters: link1 (int) – link2 (int) – (bool)
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]
Parameters: name (str) –
setTorqueLimits(tmax)[source]

Sets the torque limit vector tmax, the constraint is $$|torque[i]| <\leqtmax[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. (list of floats)
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

appearance()[source]

Returns a reference to the link’s appearance.

Returns: (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: (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. (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) – the angular acceleration of the link, in world coordinates. (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 (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: (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: (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) – 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. (list of 6 lists of floats)
getLocalDirection(vworld)[source]

Converts direction from world to local coordinates.

Parameters: vworld (list of 3 floats) – the local coordinates of the world direction vworld (list of 3 floats)
getLocalPosition(pworld)[source]

Converts point from world to local coordinates.

Parameters: pworld (list of 3 floats) – the local coordinates of the world point pworld (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: (Mass)
getName()[source]

Returns the name of the robot link.

Returns: (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. (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. (list of 3 lists of floats)
getParent()[source]

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

Returns: (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. (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) – the acceleration of the point, in world coordinates. (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) – the current velocity of the point, in world coordinates. (list of 3 floats)
getPositionHessian(p)[source]

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

Parameters: p (list of 3 floats) – a triple (Hx,Hy,Hz) of of nxn matrices corresponding, respectively, to the (x,y,z) components of the Hessian. (3-tuple)
getPositionJacobian(p)[source]

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

Parameters: p (list of 3 floats) – 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. (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. (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 (list of 3 floats)
getWorldDirection(vlocal)[source]

Converts direction from local to world coordinates.

Parameters: vlocal (list of 3 floats) – the world coordinates of the local direction vlocal (list of 3 floats)
getWorldPosition(plocal)[source]

Converts point from local to world coordinates.

Parameters: plocal (list of 3 floats) – the world coordinates of the local point plocal (list of 3 floats)
index

parent()[source]

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

Returns: (RobotModelLink)
robot()[source]

Returns a reference to the link’s robot.

Returns: (RobotModel)
robotIndex

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: p (int, optional) – l (RobotModelLink, optional) –
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) –
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.

world

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

appearance()[source]

Returns a reference to the appearance associated with this object.

Returns: (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: (Geometry3D)
getContactParameters()[source]

Returns a copy of the ContactParameters of this rigid object.

Returns: (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: (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: (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: (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. (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) (tuple)
index

RigidObjectModel_index_get(RigidObjectModel self) -> int

loadFile(fn)[source]

Loads the object from the file fn.

Parameters: fn (str) – (bool)
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 (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) –
world

RigidObjectModel_world_get(RigidObjectModel self) -> int

class klampt.TerrainModel[source]

Bases: object

Static environment geometry.

C++ includes: robotmodel.h

appearance()[source]

Returns a reference to the appearance associated with this object.

Returns: (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: (Geometry3D)
getID()[source]

Returns the ID of the terrain in its world.

Returns: (int)

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

getName()[source]
Returns: (str)
index

TerrainModel_index_get(TerrainModel self) -> int

loadFile(fn)[source]

Loads the terrain from the file fn.

Parameters: fn (str) – (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 (bool)
setFriction(friction)[source]

Changes the friction coefficient for this terrain.

Parameters: friction (float) –
setName(name)[source]
Parameters: name (str) –
terrain

TerrainModel_terrain_get(TerrainModel self) -> Terrain *

world

TerrainModel_world_get(TerrainModel self) -> int

class klampt.Mass[source]

Bases: object

Stores mass information for a rigid body or robot link.

Note

You should 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

com

Mass_com_get(Mass self) -> doubleVector

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: (float)
inertia

Mass_inertia_get(Mass self) -> doubleVector

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.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

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

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 setVelocity, setTorque, or setPID command are called, the motion queue behavior will be completely overridden. To reset back to motion queue control, the function setManualMode(False) must be called.

C++ includes: robotsim.h

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 command list

Returns: (stringVector)
controller

SimRobotController_controller_get(SimRobotController self) -> ControlledRobotSimulator *

getCommandedConfig()[source]

Returns the current commanded configuration.

getCommandedVelocity()[source]

Returns the current commanded velocity.

getControlType()[source]

Returns the control type for the active controller.

Returns: (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.

Returns: (float)
getSensedConfig()[source]

Returns the current “sensed” configuration from the simulator.

getSensedVelocity()[source]

Returns the current “sensed” velocity from the simulator.

getSetting(name)[source]

gets a setting of the controller

Parameters: name (str) – (str)
index

SimRobotController_index_get(SimRobotController self) -> int

model()[source]

Retrieves the robot model associated with this controller.

Returns: (RobotModel)
remainingTime()[source]

Returns the remaining duration of the motion queue.

Returns: (float)
sendCommand(name, args)[source]

sends a command to the controller

Parameters: name (str) – args (str) – (bool)
sensor(*args)[source]

Returns a sensor by index or by name. If out of bounds or unavailable, a null sensor is returned.

sensor (index): SimRobotSensor

sensor (name): SimRobotSensor

Parameters: index (int, optional) – name (str, optional) – (SimRobotSensor)
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) –
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) –
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.

Parameters: kP (list of floats) – kI (list of floats) – kD (list of floats) –
setRate(dt)[source]

Sets the current feedback control rate.

Parameters: dt (float) –
setSetting(name, val)[source]

sets a setting of the controller

Parameters: name (str) – val (str) – (bool)
setTorque(t)[source]

Sets a torque command controller.

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.

Parameters: dq (list of floats) – dt (float) –
sim

SimRobotController_sim_get(SimRobotController self) -> Simulator

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

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) –
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]

Sets the 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
geometry

SimBody_geometry_get(SimBody self) -> ODEGeometry *

getCollisionPadding()[source]
Returns: (float)
getID()[source]

Returns the object ID that this body associated with.

Returns: (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: (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: (bool)
isEnabled()[source]

Returns true if this body is being simulated.

Returns: (bool)
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: padding (float) –
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) –
sim

SimBody_sim_get(SimBody self) -> Simulator

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) –
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: link (RobotModelLink, optional) – object (RigidObjectModel, optional) – terrain (TerrainModel, optional) –
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) – (SimRobotController)
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) –
getActualTorques(robot)[source]

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

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) – (str)

Valid names are:

• gravity
• simStep
• boundaryLayerCollisions
• rigidObjectCollisions
• robotSelfCollisions
• robotRobotCollisions
• maxContacts
• clusterNormalScale
• errorReductionParameter
• dampedLeastSquaresParameter
• instabilityConstantEnergyThreshold
• instabilityLinearEnergyThreshold
• instabilityMaxEnergyThreshold
• instabilityPostCorrectionEnergy

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: (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: (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 (str)
getTime()[source]

Returns the simulation time.

Returns: (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) – (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) – (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) – (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) – (bool)
index

Simulator_index_get(Simulator self) -> int

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) –
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.

world

Simulator_world_get(Simulator self) -> WorldModel

class klampt.Geometry3D(*args)[source]

Bases: object

A three-D geometry. Can either be a reference to a world item’s geometry, in which case modifiers change the world item’s geometry, or it can be a standalone geometry.

There are five currently supported types of geometry:

• primitives (GeometricPrimitive)
• triangle meshes (TriangleMesh)
• point clouds (PointCloud)
• volumetric grids (VolumeGrid)
• groups (Group)

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

Each geometry stores a “current” transform, which is automatically updated for world items’ geometries. The proximity queries are performed with respect to the transformed geometries (note the underlying geometry is not changed, which could be computationally expensive. The query is performed, however, as though they were).

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 any setX() functions, translate(), and transform().

Proximity queries include collides(), withinDistance(), distance(), closestPoint(), and rayCast().

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. By default it is zero. (Note that this is NOT the same thing as simulation body collision padding!)

C++ includes: geometry.h

__init__ (): Geometry3D

__init__ (arg2): Geometry3D

Parameters: arg2 (VolumeGrid or TriangleMesh or GeometricPrimitive or PointCloud or Geometry3D, optional) –
clone()[source]

Creates a standalone geometry from this geometry.

Returns: (Geometry3D)
collides(other)[source]

Returns true if this geometry collides with the other.

Parameters: other (Geometry3D) – (bool)
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

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.
• PointCloud -> TriangleMesh. Available if the point cloud is structured. param is the threshold for splitting triangles by depth discontinuity. param is by default infinity.
• 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.
distance(other)[source]

Returns the the distance and closest points between the given geometries.

Parameters: other (Geometry3D) – (DistanceQueryResult)

If the objects are penetrating, some combinations of geometry types allow calculating penetration depths (GeometricPrimitive-GeometricPrimitive, GeometricPrimitive-TriangleMesh (surface only), GeometricPrimitive-PointCloud, GeometricPrimitive-VolumeGrid, TriangleMesh (surface only)- GeometricPrimitive, PointCloud-VolumeGrid). In this case, a negative value is returned and cp1,cp2 are the deepest penetrating points.

Same comments as the distance_point function

distance_ext(other, settings)[source]

A customizable version of 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: other (Geometry3D) – settings (DistanceQuerySettings) –
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) – (DistanceQueryResult)

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

distance_point_ext(pt, settings)[source]

A customizable version of 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: pt (list of 3 floats) – settings (DistanceQuerySettings) –
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 (float)

Returns the distance from this geometry to the other. If either geometry contains volume information, this value may be negative to indicate penetration.

empty()[source]

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

Returns: (bool)
free()[source]

Frees the data associated with this geometry, if standalone.

geomPtr

Geometry3D_geomPtr_get(Geometry3D self) -> void *

getBB()[source]

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

getBBTight()[source]

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

getCollisionMargin()[source]

Returns the padding around the base geometry. Default 0.

Returns: (float)
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) – (Geometry3D)
getGeometricPrimitive()[source]

Returns a GeometricPrimitive if this geometry is of type GeometricPrimitive.

Returns: (GeometricPrimitive)
getPointCloud()[source]

Returns a PointCloud if this geometry is of type PointCloud.

Returns: (PointCloud)
getTriangleMesh()[source]

Returns a TriangleMesh if this geometry is of type TriangleMesh.

Returns: (TriangleMesh)
getVolumeGrid()[source]

Returns a VoumeGrid if this geometry is of type VolumeGrid.

Returns: (VolumeGrid)
id

Geometry3D_id_get(Geometry3D self) -> int

isStandalone()[source]

Returns true if this is a standalone geometry.

Returns: (bool)
loadFile(fn)[source]

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

Parameters: fn (str) – (bool)
numElements()[source]

Returns the number of sub-elements in this geometry.

Returns: (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) – (bool)
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) – (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) –
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: element (int) – data (Geometry3D) –
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) –
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, or GeometricPrimitive.

Returns: (str)
withinDistance(other, tol)[source]

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

Parameters: other (Geometry3D) – tol (float) – (bool)
world

Geometry3D_world_get(Geometry3D self) -> int

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) –
ALL = 0
EDGES = 2
FACES = 3
VERTICES = 1
appearancePtr

Appearance_appearancePtr_get(Appearance self) -> void *

clone()[source]

Creates a standalone appearance from this appearance.

Returns: (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()[source]

getColor (feature)

Parameters: feature (int, optional) –
getDraw(*args)[source]

Returns whether this object or feature is visible.

getDraw (): bool

getDraw (feature): bool

Parameters: feature (int, optional) – (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) –
id

Appearance_id_get(Appearance self) -> int

isStandalone()[source]

Returns true if this is a standalone appearance.

Returns: (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, or FACES.

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

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) –
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 most significant byte
• argb8: unsigned byte RGBA colors with alpha in the most significant byte
• l8: unsigned byte grayscale colors
setTexture2D(w, h, format, bytes)[source]

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

Parameters: w (int) – h (int) – format (str) – char (std::vector< unsigned) – bytes (std::allocator) –
world

Appearance_world_get(Appearance self) -> int

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

absErr

DistanceQuerySettings_absErr_get(DistanceQuerySettings self) -> double

relErr

DistanceQuerySettings_relErr_get(DistanceQuerySettings self) -> double

upperBound

DistanceQuerySettings_upperBound_get(DistanceQuerySettings self) -> double

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.

Type: bool
hasGradients

f true, distance gradient information is given in grad0 and grad1.

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, 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

C++ includes: geometry.h

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.

Type: bool
hasGradients

f true, distance gradient information is given in grad0 and grad1.

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, 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

C++ includes: geometry.h

cp1

DistanceQueryResult_cp1_get(DistanceQueryResult self) -> doubleVector

cp2

DistanceQueryResult_cp2_get(DistanceQueryResult self) -> doubleVector

d

DistanceQueryResult_d_get(DistanceQueryResult self) -> double

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

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


C++ includes: geometry.h

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


C++ includes: geometry.h

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) –
vertices

TriangleMesh_vertices_get(TriangleMesh self) -> doubleVector

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 vertices/properties/propertyName 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.

Properties 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

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

• version: version of the PCL file, typically “0.7”
• id: integer id
• viewpoint: “ox oy oz qw qx qy qz”

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)


C++ includes: geometry.h

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 vertices/properties/propertyName 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.

Properties 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

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

• version: version of the PCL file, typically “0.7”
• id: integer id
• viewpoint: “ox oy oz qw qx qy qz”

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)


C++ includes: geometry.h

addPoint(p)[source]

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

Parameters: p (list of 3 floats) – (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) –
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) – (float)
getSetting(key)[source]

Retrieves the given setting.

Parameters: key (str) – (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: (int)
numProperties()[source]

Returns the number of properties.

Returns: (int)
properties

PointCloud_properties_get(PointCloud self) -> doubleVector

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) –
settings

PointCloud_settings_get(PointCloud self) -> std::map< std::string,std::string > *

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) –
vertices

PointCloud_vertices_get(PointCloud self) -> doubleVector

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.

C++ includes: geometry.h

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

C++ includes: geometry.h

loadString(str)[source]
Parameters: str (str) – (bool)
properties

GeometricPrimitive_properties_get(GeometricPrimitive self) -> doubleVector

saveString()[source]
Returns: (str)
setAABB(bmin, bmax)[source]
Parameters: bmin (list of 3 floats) – bmax (list of 3 floats) –
setPoint(pt)[source]
Parameters: pt (list of 3 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) –
type

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

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.

C++ includes: geometry.h

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

C++ includes: geometry.h

bbox

VolumeGrid_bbox_get(VolumeGrid self) -> doubleVector

dims

VolumeGrid_dims_get(VolumeGrid self) -> intVector

get(i, j, k)[source]
Parameters: i (int) – j (int) – k (int) – (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) –
values

VolumeGrid_values_get(VolumeGrid self) -> doubleVector

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.

Currently only fixed-point constraints and fixed-transform constraints are implemented in the Python API.

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) –
copy()[source]

Copy constructor.

Returns: (IKObjective)

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

Returns: (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)

goal

IKObjective_goal_get(IKObjective self) -> IKGoal

The index of the robot link that is constrained.

Returns: (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 loader.toJson/fromJson()

Parameters: str (str) – (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: (int)
numRotDims()[source]

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

Returns: (int)
saveString()[source]

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

Returns: (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: link (int) – plocal (list of 3 floats) – pworld (list of 3 floats) –
setFixedPoints(link, plocals, pworlds)[source]

Sets a multiple fixed-point constraint.

Parameters: link (int) – 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: link (int) – R (list of 9 floats (so3 element)) – t (list of 3 floats) –
setFreePosition()[source]

Manual: Sets a free position constraint.

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: link (int) – 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: link1 (int) – link2 (int) – 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: link1 (int) – link2 (int) – p1s (object) – p2s (object) –
setRelativeTransform(link, linkTgt, R, t)[source]

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

Parameters: link (int) – linkTgt (int) – 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: robot (RobotModel, optional) – solver (IKSolver, optional) –
activeDofs

IKSolver_activeDofs_get(IKSolver self) -> intVector

add(objective)[source]

Adds a new simultaneous objective.

Parameters: objective (IKObjective) –
biasConfig

IKSolver_biasConfig_get(IKSolver self) -> doubleVector

clear()[source]

Clears objectives.

copy()[source]

Copy constructor.

Returns: (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: (int)
getResidual()[source]

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

getTolerance()[source]

Gets the constraint solve tolerance.

Returns: (float)
isSolved()[source]

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

Returns: (bool)
lastIters

IKSolver_lastIters_get(IKSolver self) -> int

lastSolveIters()[source]

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

Returns: (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

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]

Assigns an existing objective added by add.

Parameters: i (int) – objective (IKObjective) –
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) – (object or bool)
tol

IKSolver_tol_get(IKSolver self) -> double

useJointLimits

IKSolver_useJointLimits_get(IKSolver self) -> bool

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: obj (GeneralizedIKObjective or RigidObjectModel, optional) – link (RobotModelLink, optional) – link2 (RobotModelLink, optional) – obj2 (RigidObjectModel, optional) –
goal

GeneralizedIKObjective_goal_get(GeneralizedIKObjective self) -> IKGoal

isObj1

GeneralizedIKObjective_isObj1_get(GeneralizedIKObjective self) -> bool

isObj2

GeneralizedIKObjective_isObj2_get(GeneralizedIKObjective self) -> bool

link1

link2

obj1

GeneralizedIKObjective_obj1_get(GeneralizedIKObjective self) -> RigidObjectModel

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) –
add(objective)[source]

Adds a new simultaneous objective.

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.

maxIters

GeneralizedIKSolver_maxIters_get(GeneralizedIKSolver self) -> int

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.

tol

GeneralizedIKSolver_tol_get(GeneralizedIKSolver self) -> double

useJointLimits

GeneralizedIKSolver_useJointLimits_get(GeneralizedIKSolver self) -> bool

world

GeneralizedIKSolver_world_get(GeneralizedIKSolver self) -> WorldModel

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. (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. 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. (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. 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. (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: (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: (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. 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)  (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. gives the min/max extents of the support polygon. If the support interval is empty, (inf,inf) is returned. (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]