kinematic - Kinematic solver/constraint system

This module defines the types and functions for kinematic manimulation and computation.

Kinematics are a conceptual approach of mechanisms. It sort parts in groups called solids (in solids all parts have the same movement), and links the solids to each other using constraints named joints. That way no matter what are the parts, or what are their shape, or how parts interact - solids movements can be deduced only from joints.

This allows designing the mechanisms before designing its parts. This also allows visualizing the mechanism whether it is complete or not.

As parts in the same solid all have the same movement, solids are considered to be undeformable. This allows the to use the Screw theory to represent the force and movement variables (see https://en.wikipedia.org/wiki/Screw_theory). In this module, screws are called Screw.

This module mainly features:

• Joint - the base class for all joints, instances of joints define a kinematic
• Kinematic - the general kinematic solver
• Chain - a joint and kinematic solver dedicated to kinematic chains

joints are defined in madcad.joints

class KinematicError

raised when a kinematic problem cannot be solved because the constraints cannot be satisfied or the solver cannot satisfy it

class Kinematic(joints: list = [], content: Optional[dict] = None, ground=None, inputs=None, outputs=None)

This class allows resolving direct and inverse kinematic problems with any complexity. It is not meant to be a data format for kinematic, since the whole kinematic definition holds in joints. This class builds appropriate internal data structures on instanciation so that calls to solve() are fast and reproducible. Realtime (meaning fixed time resolution) is not a target, but reliability and convenience to compute any sort of mechanical interactions between solids.

A kinematic is defined by its joints:

• each joint works using position variables, the list of all joint positions is called the state of the kinematic
• each joint is a link between 2 solids (start, stop)
• each joint can provide a transformation matrix from its start solid to stop solid deduced from the joint position, as well as a gradient of this matrix

A kinematic problem is defined by

• the joints we fix (or solids we fix, but fixing a solid can be done using a joint)
• the joints who stay free, whose positions need to be deduced from the fixed joints

A list of joints can be seen as a graph of links between solids. The complexity of the kinematic probleme depends on

• the number cycles
• the degree of freedom

Example

>>> # keep few joints apart, so we can use them as dict keys, for calling `solve`
>>> motor1 = Revolute((0,2), Axis(...))
>>> motor2 = Revolute((0,7), Axis(...))
>>> free = Free((7,4))
>>> # the kinematic solver object
>>> kinematic = Kinematic([
...     Revolute((0,1), Axis(...)),
...     motor1,
...     motor2,
...     Planar((7,3), ...),
...     Cylindrical((1,3), ...),
...     Ball((3,2), ...),
...     Prismatic((4,3), ...),
...     Planar((1,5), ...),
...     Planar((1,6), ...),
...     Weld((7,5), mat4(...)),
...     Weld((7,6), mat4(...)),
...     free,
...     ], ground=7)

defines a kinematic with the following graph

one can also define a kinematic with notions of direct and inverse transformations. The notion of direct and inverse is based on an input/output relation that we define as such:

• inputs is selection of joint coordinates
• outputs is a selection of solids poses

>>> kinematic = Kinematic([
...     Revolute((0,1), Axis(...)),
...     Planar((7,3), ...),
...     Cylindrical((1,3), ...),
...     Ball((3,2), ...),
...     Prismatic((4,3), ...),
...     Planar((1,5), ...),
...     Planar((1,6), ...),
...     Weld((7,5), mat4(...)),
...     Weld((7,6), mat4(...)),
...     ],
...     ground = 7,
...     inputs = [motor1, motor2],
...     outputs = [4,5,6],
...     )

Tip

If your kinematic is a chain of joints, then prefer using Chain to reduce the overhead of the genericity.

Note

A Kinematic doesn’t tolerate modifications of the type of its joints once instanciated. joints positions could eventually be modified at the moment it doesn’t affect the hash of the joints (See Joint)

joints

a list of Joint, defining the kinematic these joints could for a connex graph or not, with any number of cycles

each joint is a link between 2 solids, which are represented by a hashable object (it is common to designate these solids by integers, strings, or objects hashable by their id)

content

display object for each solid, this can be anything implementing the display protocol, and will be used only when this kinematic is displayed

ground

the reference solid, all other solids positions will be relative to it

inputs

a list of joints to fix when calling direct()

outputs

a list of solids to fix when calling inverse()

default

the default joint pose of the kinematic

bounds

a tuple of (min, max) joint poses

normalize(state) → list

inplace normalize the joint coordinates in the given state

direct(parameters: list, close=None) → list

shorthand to self.solve(self.inputs) and computation of desired transformation matrices it only works when direct and inverse constraining joints have been set

inverse(parameters: list, close=None) → list

shorthand to self.solve(self.outputs) and extraction of desired joints it only works when direct and inverse constraining joints have been set

grad(state) → dict

return a gradient of the all the solids poses at the given joints position

Note

this function will ignore any degree of freedom of the kinematic that is not defined in inputs

cycles() → list

return a list of minimal cycles decomposing the gkinematic graph

Example

>>> len(kinematic.cycles())
5

solve(fixed: dict = {}, close: Optional[list] = None, precision=1e-06, maxiter=None, strict=0.0) → list

compute the joint positions for the given fixed solids positions

Parameters:

• fixed – list of mat4 poses of the fixed solids in the same order as given to __init__
• close – the joint positions we want the result the closest to. If not provided, self.default will be used
• precision – the desired precision tolerance for kinematic loops closing. this function returns once reached
• strict – expected error tolerance on the kinematic loops closing, if not reached after maxiter iterations, this function will raise KinematicError. Leave it to 0 to raise when precision has not been reached
• maxiter – maximum number of iterations allowed, or None if not limit

Return: the joint positions allowing the kinematic to have the fixed solids in the given poses Raise: KinematicError if no joint position can satisfy the fixed positions

Example

>>> # solve with no constraints
>>> kinematic.solve()
[...]
>>> # solve by fixing solid 4
>>> kinematic.solve({free: mat4(...)})
[...]
>>> # solve by fixing some joints
>>> kinematic.solve({motor1: radians(90)})
[...]
>>> kinematic.solve({motor1: radians(90), motor2: radians(15)})
[...]

freedom(state, precision=1e-06) → list

list of free movement joint directions. the length of the list is the degree of freedom .

Note

When state is a singular position in the kinematic, the degree of freedom is locally smaller or bigger than in other positions

parts(state, precision=1e-06) → dict

return the pose of all solids in the kinematic for the given joints positions The arguments are the same as for self.direct()

Parameters:precision – error tolerance for kinematic loop closing, if a loop is above this threshold this function will raise KinematicError

display(scene)

display allowing manipulation of kinematic

to_chain() → Chain

class Joint(*args, default=0, **kwargs)

A Joint constraints the relative position of two solids.

In this library, relative positioning is provided by transformation matrices which need a start-end convention, so every joint is directed and the order of self.solids matters.

There is two ways of defining the relative positioning of solids

• by a joint position \((q_i)\)
• by a start-end matrix \(T_{ab}\)

we can switch from one representation to the other using the direct and inverse methods.

solids

a tuple (start, end) or hashable objects representing the solids the joint is linking

default

a default joint position

bounds

a tuple of (min, max) joint positions

normalize(state) → params

make the given joint coordinates consistent. For most joint is is a no-op or just coordinates clipping

for complex joints coordinates containing quaternions or directions or so, it may need to perform normalizations or orthogonalizations or so on. This operation may be implemented in-place or not.

direct(state) → dmat4x4

direct kinematic computation

Parameters:state – the parameters defining the joint state It can be any type accepted by numpy.array Returns:the transfer matrix from solids self.stop to self.start

inverse(matrix, close=None, precision=1e-06, maxiter=None, strict=0.0) → list

inverse kinematic computation

the default implementation is using a newton method to nullify the error between the given and acheived matrices

Parameters:

• matrix – the transfer matrix from solids self.stop to self.start we want the parameters for
• close – a know solution we want the result the closest to. if not specified, it defaults to self.default
• precision – desired error tolerance on the given matrix, this function returns once reached
• strict – expected error tolerance on the given matrix, if not reached after maxiter iterations, this function will raise KinematicError. Leave it to 0 to raise when precision has not been reached
• maxiter – maximum number of iterations allowed, or None if not limit

Returns:

the joint parameters so that self.direct(self.inverse(x)) == x (modulo numerical precision)

grad(state, delta=1e-06) → [mat4]

compute the gradient of the direct kinematic

The default implementation is using a finite differentiation

Parameters:

• state – anything accepted by self.direct() including singularities
• delta – finite differentiation interval

Returns:

a list of the matrix derivatives of self.direct(), one each parameter

transmit(force: Screw, state=None, velocity=None) → Screw

compute the force transmited by the kinematic chain in free of its moves

The default implementation uses the direct kinematic gradient to compute the moving directions of the chain

Parameters:

• force – force sent by self.start in its coordinate system
• state – the joint position in which the joint is at the force transmision instant. If not specified it defaults to self.default (many joints transmit the same regardless of their position)
• velocity – current derivative of state at the transmision instant Perfect joints are transmiting the same regardless of their velocity

Returns:

force received by self.stop in its coordinate system

scheme(size: float, junc: dvec3 = None) → [Scheme]

generate the scheme elements to render the joint

display(scene)

display showing the schematics of this joint, without interaction

class Weld(solids, transform: Optional[dmat4x4] = None)

joint with no degree of freedom, simply welding a solid to an other with a transformation matrix to place one relatively to the other

It is useful to fix solids between each other without actually making it the same solid in a kinematic.

class Free(solids)

joint of complete freedom. it adds no effective constraint to the start and end solids. its parameter is its transformation matrix.

it is useful to control the explicit relative pose of solids in a kinematic.

class Reverse(joint)

that joint behaves like its wrapped joint but with swapped start and stop solids

class Chain(joints, content: Optional[list] = None, default=None)

Kinematic chain, This chain of joints acts like one only joint The new formed joint has as many degrees of freedom as its enclosing joints.

This class is often used instead of Kinematic when possible, because having more efficient inverse() and direct() methods dedicated to kinematics with one only cycle. It also has simpler in/out parameters since a chain has only two ends where a random kinematic may have many

A Chain doesn’t tolerate modifications of the type of its joints once instanciated. a joint placement can be modified as long as it doesn’t change its hash.

joints

joints in the chaining order it must satisfy joints[i].solids[-1] == joints[i+1].solids[0]

Type:list

content

displayables matching solids, its length must be len(joints)+1

Type:list

solids

the (start,end) joints of the chain, as defined in Joint

Type:tuple

default

the default joints positions

bounds

the (min,max) joints positions

normalize(state) → list

inplace normalize the joint coordinates in the given state

direct(state) → dmat4x4

direct kinematic computation

Parameters:state – the parameters defining the joint state It can be any type accepted by numpy.array Returns:the transfer matrix from solids self.stop to self.start

inverse(matrix, close=None, precision=1e-06, maxiter=None, strict=0.0) → list

inverse kinematic computation

the default implementation is using a newton method to nullify the error between the given and acheived matrices

Parameters:

• matrix – the transfer matrix from solids self.stop to self.start we want the parameters for
• close – a know solution we want the result the closest to. if not specified, it defaults to self.default
• precision – desired error tolerance on the given matrix, this function returns once reached
• strict – expected error tolerance on the given matrix, if not reached after maxiter iterations, this function will raise KinematicError. Leave it to 0 to raise when precision has not been reached
• maxiter – maximum number of iterations allowed, or None if not limit

Returns:

the joint parameters so that self.direct(self.inverse(x)) == x (modulo numerical precision)

grad(state) → list

the jacobian of the flattened parameters list

parts(state) → list

return the pose of each solid in the chain

to_kinematic() → Kinematic

to_dh()

denavit-hartenberg representation of this kinematic chain.

it also returns the solids base definitions relative to the denavit-hartenberg convention, it the joints already follows the conventions, these should be eye matrices

from_dh(transforms=None) → Self

build a kinematic chain from a denavit-hartenberge representation, and eventual base definitions relative to the denavit-hartenberg convention

display(scene)

display allowing manipulation of the chain

arcs(conn: {node: [node]}) → [[node]]

find ars in the given graph

depthfirst(conn: {node: [node]}, starts=()) → [parent, child]

generate a depth-first traversal of the givne graph

cycles(conn: {node: [node]}) → [[node]]

extract a set of any-length cycles decomposing the graph

shortcycles(conn: {node: [node]}, costs: {node: float}, branch=True, tree=None) → [[node]]

extract a set of minimal cycles decompsing the graph

class Solid(pose=1, **content)

Solid for objects display

A Solid is also a way to group objects and move it anywhere without modifying them, as the objects contained in a solid are considered to be in solid local coordinates. A Solid is just like a dictionary with a pose.

pose

placement matrix, it defines a base in which the solid’s content is displayed

Type:mat4

content

objects to display using the solid’s pose

Type:dict/list

Example

>>> mypart = icosphere(vec3(0), 1)
>>> s = Solid(part=mypart, anything=vec3(0))   # create a solid with whatever inside

>>> s.transform(vec3(1,2,3))   # make a new translated solid, keeping the same content without copy

>>> # put any content in as a dict
>>> s['part']
<Mesh ...>
>>> s['whatever'] = vec3(5,2,1)

transform(value) → Solid

Displace the solid by the transformation

place(*args, **kwargs) → Solid

Strictly equivalent to .transform(placement(...)), see placement for parameters specifications.

loc(*args)

deloc(*args)

set(**objs)

Convenient method to set many elements in one call. Equivalent to self.content.update(objs)

append(value)

Add an item in self.content, a key is automatically created for it and is returned

__getitem__(key)

Shorthand to self.content

__setitem__(key, value)

Shorthand to self.content

display(scene)

placement(*pairs, precision=0.001)

Return a transformation matrix that solved the placement constraints given by the surface pairs

Parameters:

• pairs –

  a list of pairs to convert to kinematic joints

  • items can be couples of surfaces to convert to joints using guessjoint
  • tuples (joint_type, a, b) to build joints joint_type(solida, solidb, a, b)
• precision – surface guessing and kinematic solving precision (distance)

Each pair define a joint between the two assumed solids (a solid for the left members of the pairs, and a solid for the right members of the pairs). Placement will return the pose of the first relatively to the second, satisfying the constraints.

Example

>>> # get the transformation for the pose
>>> pose = placement(
...             (screw['part'].group(0), other['part'].group(44)),  # two cylinder surfaces: Cylindrical joint
...             (screw['part'].group(4), other['part'].group(25)),    # two planar surfaces: Planar joint
...             )  # solve everything to get solid's pose
>>> # apply the transformation to the solid
>>> screw.pose = pose

>>> # or
>>> screw.place(
...             (screw['part'].group(0), other['part'].group(44)),
...             (screw['part'].group(4), other['part'].group(25)),
...             )

>>> screw.place(
...             (Revolute, screw['axis'], other['screw_place']),
...             )

suppose we have those parts to assemble and it’s hard to guess the precise pose transform between them

placement gives the pose for the screw to make the selected surfaces coincide

explode(solids, factor=1, offsets=None) -> (solids:list, graph:Mesh)

Move the given solids away from each other in the way of an exploded view. It makes easier to seen the details of an assembly . See explode_offsets for the algorithm.

Parameters:

• solids – a list of solids (copies of each will be made before displacing)
• factor – displacement factor, 0 for no displacement, 1 for normal displacement
• offsets – if given, must be the result of explode_offsets(solids)

Example

>>> # pick some raw model and separate parts
>>> imported = read(folder+'/some_assembly.stl')
>>> imported.mergeclose()
>>> parts = []
>>> for part in imported.islands():
...     part.strippoints()
...     parts.append(Solid(part=segmentation(part)))
...
>>> # explode the assembly to look into it
>>> exploded = explode(parts)

before operation

after operation

explode_offsets(solids) → [solid_index, parent_index, offset, barycenter]

Build a graph of connected objects, ready to create an exploded view or any assembly animation. See explode() for an example. The exploded view is computed using the meshes contained in the given solids, so make sure there everything you want in their content.

Complexity is O(m * n) where m = total number of points in all meshes, n = number of solids

Note

Despite the hope that this function will be helpful, it’s (for computational cost reasons) not a perfect algorithm for complex assemblies (the example above is at the limit of a simple one). The current algorithm will work fine for any simple enough assembly but may return unexpected results for more complex ones.