klampt.model.contact module

Primary API

ContactPoint([x, n, kFriction])	A point contact between two rigid bodies, object1 and object2.
Hold()	A container for both contact points and an IK constraint on a robot's link.
force_closure(contactOrHoldList)	Given a list of ContactPoints or Holds, tests for force closure.
com_equilibrium(contactOrHoldList[, fext, com])	Given a list of ContactPoints or Holds, an external gravity force, and a COM, tests for the existence of an equilibrium solution.
support_polygon(contactOrHoldList)	Given a list of ContactPoints or Holds, returns the support polygon.
equilibrium_torques(robot, holdList[, fext, ...])	Solves for the torques / forces that keep the robot balanced against gravity.
contact_map(contacts[, fixed])	Given an unordered list of ContactPoints, computes a canonical dict from (obj1,obj2) pairs to a list of contacts on those objects.
world_contact_map(world, padding[, ...])	Given a WorldModel, returns a contact map representing all current contacts (distance >= 0 and <= padding).
sim_contact_map(sim)	Given a Simulation, returns a contact map representing all current contacts (among bodies with collision feedback enabled).
contact_map_ik_objectives(contactmap)	Given a contact map, computes a set of non-conflicting IKObjective's or GeneralizedIKObjective's that enforce all simultaneous contact constraints.
contact_map_holds(contactmap)	Given a contact map, computes a set of non-conflicting Holds that enforce all simultaneous contact constraints.
geometry_contacts(geom1, geom2, padding1[, ...])	Similar to geom1.contacts(geom2,padding1,padding2,maxcontacts), but returns a list of ContactPoint, where the point (x) of each contact is placed in the center of the overlap region.
wrench_matrices(contactMap)	Returns a map from contact pairs (o1,o2) to pairs of rigid-body wrench matrices (W1,W2) corresponding to each pair of objects in the contact map.

Helpers

idToObject(*args, **kwargs)	Deprecated in a future version of Klampt.
skew(x)	Returns the skew-symmetric cross-product matrix corresponding to the matrix x
invMassMatrix(*args, **kwargs)	Deprecated in a future version of Klampt.

Definitions of frictional point contacts, contact maps, basic wrench space calculation subroutines, and performing equilibrium testing.

klampt.model.contact.id_to_object(world, ID)

Helper: takes a WorldModel ID and converts it into an object.

class klampt.model.contact.ContactPoint(x=None, n=None, kFriction=0.0)

Bases: object

A point contact between two rigid bodies, object1 and object2.

x

the contact point in world coordinates.

Type:

list of 3 floats

n

the normal pointing from object1 into object 2.

Type:

list of 3 floats

kFriction

the friction coefficient.

Type:

float

object1, object2

the objects in contact.

Type:

optional

reflect()

Flips the contact point to switch the base object from object1 to object2

transform(xform)

Given a rigid transform xform given as an se3 element, transforms the contact point.

tolist()

Returns a 7-list representing this contact point, for use in the stability testing routines.

fromlist(v)

Reads the values x,n, and kFriction from the 7-list v.

class klampt.model.contact.Hold

Bases: object

A container for both contact points and an IK constraint on a robot’s link. Can represent face-face, point-point, or edge contact.

Similar to the Hold class in the C++ RobotSim library. Hypothetically, can also represent sliding contact.

link

the link index

Type:

int

ikConstraint

the constraint (see klampt.robotsim.IKObjective or klampt.ik.objective)

Type:

IKObjective

contacts

the contacts used in the hold. (see klampt.contact.ContactPoint)

Type:

list of ContactPoint

setFixed(link, contacts)

Creates this hold such that it fixes a robot link to match a list of contacts (in world space) at its current transform.

Parameters:

• link – a robot link or rigid object, currently contacting the environment / object at contacts

• contacts (list of ContactPoint) – the fixed contact points, given in world coordinates.

transform(xform)

Given a rigid transform xform given as an se3 element, transforms the hold’s contacts and ik constraint in-place.

klampt.model.contact.force_closure(contactOrHoldList)

Given a list of ContactPoints or Holds, tests for force closure. Return value is True or False.

Formulates the wrench matrix W of all contacts, including edges of the friction cones. If Hull(W) contains the zero vector in its interior, then the contacts are said to be in force closure.

klampt.model.contact.com_equilibrium(contactOrHoldList, fext=(0, 0, -1), com=None)

Given a list of ContactPoints or Holds, an external gravity force, and a COM, tests for the existence of an equilibrium solution.

Specifically, an equilibrium exists when the following equations are met:

\[\begin{split} \begin{eqnarray} fext & = \sum_i f_i \\ 0 &= \sum_i f_i \times (p_i - com) \\ f_i & \in FC_i \end{eqnarray}\end{split}\]

where \(f_i\), \(p_i\), and \(FC_i\) are the force, position, and the friction cone of the i’th contact point.

If com == None, this tests whether there exists any equilibrium com, and returns True/False.

If com != None, this returns either None if there is no solution, or otherwise returns a list of contact forces.

klampt.model.contact.support_polygon(contactOrHoldList)

Given a list of ContactPoints or Holds, returns the support polygon. The support polygon is given by list of tuples (ax,ay,b) such that the contraint ax*x+ay*y <= c holds for all (x,y) in the support polygon.

An empty support polygon is given by the result [(0,0,-1)]. A complete support polygon is given by the result [].

klampt.model.contact.equilibrium_torques(robot, holdList, fext=(0, 0, -9.8), internalTorques=None, norm=0)

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

Specifically, solves for \((\tau,f)\) in the equation:

\[G(q;fext) + \tau_{int} = \tau + \sum_i J_i(q)^T f_i\]

where \(G(q;fext)\) is the generalized gravity, \(\tau_{int}\) are the internal torques, and \(J_i(q)\) is the Jacobian of the i’th contact point. All forces are required to be in their friction cones.

Parameters:

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

• holdList (list of Hold) – a list of Holds.

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

• internalTorques (list, optional) – if given, a list of length robot.numDofs giving internal torques. For example, setting this to robot.accelToTorques(ddq) can incorporate dynamics into the solver.

• norm (float, optional) – 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)

Returns:

A pair (t,f) giving the joint torques and a list of frictional contact forces, if a solution exists. The return value may be None if no solution exists.

Return type:

tuple or None

klampt.model.contact.contact_map(contacts, fixed=None)

Given an unordered list of ContactPoints, computes a canonical dict from (obj1,obj2) pairs to a list of contacts on those objects. The resulting dict is also regularized so that objects are sorted in increasing getID(), so that (obj1,obj2) is not duplicated as (obj2,obj1).

If fixed is provided, all objects for which fixed(x) returns true will be set to None. The most common example, which fixes terrains, is:

lambda x: x is None or isinstance(x,TerrainModel)

klampt.model.contact.geometry_contacts(geom1, geom2, padding1, padding2=0, maxcontacts=0, kFriction=1)

Similar to geom1.contacts(geom2,padding1,padding2,maxcontacts), but returns a list of ContactPoint, where the point (x) of each contact is placed in the center of the overlap region.

The friction coefficient of each contact (kFriction) is set to kFriction.

Note

Contact normals may be set to (0,0,0) if they cannot be computed properly, such as when two meshes intersect.

klampt.model.contact.world_contact_map(world, padding, kFriction=1, collider=None)

Given a WorldModel, returns a contact map representing all current contacts (distance >= 0 and <= padding).

Parameters:

• world (WorldModel) – the world

• padding (float or function) – if float, a constant padding, otherwise a function f(object) that returns the padding for an object.

• kFriction (float or function, optional) – if float, a constant friction. Otherwise, a function f(object1,object2) that returns the friction for a pair of objects.

• collider (WorldCollider, optional) – if given, only the pairs of objects whose collisions are enabled will be checked.

Note

Contact normals may be set to (0,0,0) if they cannot be computed properly, such as when two meshes intersect.

klampt.model.contact.sim_contact_map(sim)

Given a Simulation, returns a contact map representing all current contacts (among bodies with collision feedback enabled).

klampt.model.contact.contact_map_ik_objectives(contactmap)

Given a contact map, computes a set of non-conflicting IKObjective’s or GeneralizedIKObjective’s that enforce all simultaneous contact constraints.

Usually called in conjunction with contact_map() with the following sequence:

objectives = contact_map_ik_objectives(contact_map(contacts,lambda x:x==None or isinstance(x,TerrainModel)))

klampt.model.contact.contact_map_holds(contactmap)

Given a contact map, computes a set of non-conflicting Holds that enforce all simultaneous contact constraints.

Usually called in conjunction with contact_map() with the following sequence:

objectives = contact_map_holds(contact_map(contacts,lambda x:x==None or isinstance(x,TerrainModel)))

klampt.model.contact.skew(x)

Returns the skew-symmetric cross-product matrix corresponding to the matrix x

klampt.model.contact.inv_mass_matrix(obj)

Returns the inverse of obj’s generalized mass matrix:

[H 0 ]-1
[0 mI]

about the origin.

klampt.model.contact.wrench_matrices(contactMap)

Returns a map from contact pairs (o1,o2) to pairs of rigid-body wrench matrices (W1,W2) corresponding to each pair of objects in the contact map.

Let the pair (o1,o2) have n contacts. The matrix W1 is a 6 x 3n mapping from contact force vectors applied on o2 to torque and force at the origin of o1. The columns [3i,…,3i+2] correspond to the force at the i’th point in contactMap[(o1,o2)]. Rows 0-2 correspond to torque about the origin, while 3-5 correspond to force.

W2 is similar, but is the jacobian regarding the force on o1.

klampt.model.contact.comEquilibrium(*args, **kwargs)

Deprecated in a future version of Klampt. Use com_equilibrium instead

klampt.model.contact.contactMap(*args, **kwargs)

Deprecated in a future version of Klampt. Use contact_map instead

klampt.model.contact.contactMapHolds(*args, **kwargs)

Deprecated in a future version of Klampt. Use contact_map_holds instead

klampt.model.contact.contactMapIKObjectives(*args, **kwargs)

Deprecated in a future version of Klampt. Use contact_map_ik_objectives instead

klampt.model.contact.equilibriumTorques(*args, **kwargs)

Deprecated in a future version of Klampt. Use equilibrium_torques instead

klampt.model.contact.forceClosure(*args, **kwargs)

Deprecated in a future version of Klampt. Use force_closure instead

klampt.model.contact.geometryContacts(*args, **kwargs)

Deprecated in a future version of Klampt. Use geometry_contacts instead

klampt.model.contact.idToObject(*args, **kwargs)

Deprecated in a future version of Klampt. Use id_to_object instead

klampt.model.contact.invMassMatrix(*args, **kwargs)

Deprecated in a future version of Klampt. Use inv_mass_matrix instead

klampt.model.contact.simContactMap(*args, **kwargs)

Deprecated in a future version of Klampt. Use sim_contact_map instead

klampt.model.contact.supportPolygon(*args, **kwargs)

Deprecated in a future version of Klampt. Use support_polygon instead

klampt.model.contact.worldContactMap(*args, **kwargs)

Deprecated in a future version of Klampt. Use world_contact_map instead

klampt.model.contact.wrenchMatrices(*args, **kwargs)

Deprecated in a future version of Klampt. Use wrench_matrices instead