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

KinematicError

Bases: Exception

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

Kinematic(joints=[], content=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

Examples:

>>> # 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)

Attributes:

Name	Type	Description
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
increment		the maximum change of joint pose before changing monotony of gradient
bounds		a tuple of (min, max) joint poses
scale		the reference distance scale used to adimension residuals

Source code in madcad/kinematic/solver.py

def __init__(self, joints:list=[], content:dict=None, ground=None, inputs=None, outputs=None):
	if ground is None:
		if joints:	ground = joints[0].solids[0]
		if inputs:  ground = inputs[0].solids[0]
	if (inputs is None) ^ (outputs is None):
		raise TypeError("inputs and outputs must be both provided or undefined")
	if inputs and outputs:
		outputs = [Free((ground, out))  for out in outputs]
		joints = inputs + joints + outputs
		self.inputs = inputs
		self.outputs = outputs
	else:
		self.inputs = None
		self.outputs = None

	self.content = content
	self.joints = joints
	self.ground = ground
	self.default = [joint.default  for joint in self.joints]
	self.increment = [joint.increment  for joint in self.joints]
	self.bounds = (
		[joint.bounds[0]  for joint in self.joints],
		[joint.bounds[1]  for joint in self.joints],
		)

	# collect the joint graph as a connectivity and the solids
	conn = {}  # connectivity {node: [node]}  with nodes being joints and solids
	for joint in joints:
		conn[joint] = joint.solids
		for solid in joint.solids:
			if solid not in conn:
				conn[solid] = []
			conn[solid].append(joint)

	# number of scalar variables
	dims = {joint: flatten_state(joint.default).size
		for joint in conn
		if isinstance(joint, Joint)}

	self.dim = sum(dims.values())
	if inputs and outputs:
		self.inputs_dim = sum(dims[joint]  for joint in self.inputs)
		self.outputs_dim = sum(dims[joint]  for joint in self.outputs)

	# reversed joints using the tree
	tree = depthfirst(conn, starts=[ground])
	self.rev = {}

	# joint computation order, for direct kinematic
	self.order = []
	for parent, child in tree:
		if child in dims:
			joint = child
			if joint.solids[0] != parent:
				if joint not in self.rev:
					self.rev[joint] = joint = Reverse(joint)
			self.order.append(joint)

	# decompose into cycles, for inverse kinematic
	self.cycles = []
	for cycle in shortcycles(conn, dims, branch=False):
		# cycles may not begin with a solid, change this
		if isinstance(cycle[0], Joint):
			cycle.pop(0)
			cycle.append(cycle[0])
		# # keep minimum number of reversed joints
		# inversions = sum(cycle[i-1] != cycle[i].solids[0]
						# for i in range(1, len(cycle), 2))
		# if inversions > len(cycle)//2:
			# cycle.reverse()
		chain = []
		for i in range(1, len(cycle), 2):
			joint = cycle[i]
			if cycle[i-1] != joint.solids[0]:
				if joint not in self.rev:
					self.rev[joint] = Reverse(joint)
				joint = self.rev[joint]
			chain.append(joint)
		self.cycles.append(chain)

	# reference distance, useful to adimension the problem
	self.scale = max(joint.scale  for joint in self.joints) or 1

normalize(state)

inplace normalize the joint coordinates in the given state

Source code in madcad/kinematic/solver.py

def normalize(self, state) -> list:
	''' inplace normalize the joint coordinates in the given state '''
	for i, joint in enumerate(self.joints):
		state[i] = joint.normalize(state[i])
	return state

direct(parameters, close=None)

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

Source code in madcad/kinematic/solver.py

def direct(self, 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
	'''
	fixed = dict(zip(self.inputs, parameters))
	result = self.solve(fixed, close)[-len(self.outputs):]
	return [joint.direct(x)  for joint, x in zip(self.outputs, result)]

inverse(parameters, close=None)

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

Source code in madcad/kinematic/solver.py

def inverse(self, 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
	'''
	fixed = {joint: joint.inverse(x)  for joint, x in zip(self.outputs, parameters)}
	result = self.solve(fixed, close)[:len(self.inputs)]
	return result[:len(self.inputs)]

grad(state, freedom=None)

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

Source code in madcad/kinematic/solver.py

def grad(self, state, freedom=None) -> list:
	'''
	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`
	'''
	if freedom is None:
		freedom = self.freedom(state, precision=1e-3)
	free = freedom.T
	inputs, outputs = free[:self.inputs_dim], free[-self.outputs_dim:]
	# pseudo inverse the input directions to get a jacobian matrix with input space being the defined input space
	try:
		# this version fails when some inputs do not have effects on any degree of freedom (and not only outputs)
		# if len(inputs) >= len(freedom):
			# jac = outputs @ la.inv(inputs.T @ inputs) @ inputs.T
		# else:
			# jac = outputs @ inputs.T @ la.inv(inputs @ inputs.T)
		# this version is robust but involves a SVD
		jac = outputs @ la.pinv(inputs, 1e-4)
	except la.LinAlgError as err:
		raise KinematicError("the defined degrees of freedom cannot move") from err
	# get matrix derivatives for each output joint
	result = []
	dmats = [j.grad(s)  for j,s in zip(self.joints[-len(self.outputs):], state[-len(self.outputs):])]
	for i,djoint in enumerate(jac.T):
		for dmat in dmats:
			result.append(sum( dm * dj   for dm,dj in zip(dmat,djoint) ))
	return result

cycles()

return a list of minimal cycles decomposing the gkinematic graph

Examples:

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

Source code in madcad/kinematic/solver.py

def cycles(self) -> list:
	'''
	return a list of minimal cycles decomposing the gkinematic graph

	Examples:
		>>> len(kinematic.cycles())
		5
	'''
	return self.cycles[:]

solve(fixed={}, close=None, precision=1e-06, maxiter=None, strict=0.0)

compute the joint positions for the given fixed solids positions

Parameters:

Name	Type	Description	Default
fixed	dict	list of mat4 poses of the fixed solids in the same order as given to __init__	{}
close	list	the joint positions we want the result the closest to. If not provided, self.default will be used	None
precision		the desired precision tolerance for kinematic loops closing. this function returns once reached	1e-06
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	0.0
maxiter		maximum number of iterations allowed, or None if not limit	None

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

Examples:

>>> # 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)})
[...]

Source code in madcad/kinematic/solver.py

def solve(self, fixed:dict={}, close:list=None, precision=1e-6, maxiter=None, strict=0.) -> list:
	'''
	compute the joint positions for the given fixed solids positions

	Args:
		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

	Examples:
		>>> # 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)})
		[...]
	'''
	if close is None:
		close = self.default
	if len(self.cycles) == 0:
		return close

	# state when some joints are fixed
	joints = []
	state = []
	increment = []
	for joint, p in zip(self.joints, close):
		if joint not in fixed:
			joints.append(joint)
			state.append(p)
			increment.append(joint.increment)

	prec = 1e-6
	max_increment = flatten_state(increment)

	k = 0
	while True:
		k += 1
		if maxiter and k > maxiter and strict==inf:
			break

		move = -self.cost_residuals(state, fixed)
		error = np.abs(move).max()
		if error <= precision:
			break
		elif maxiter and k > maxiter:
			if error <= strict:
				break
			else:
				raise KinematicError('convergence failed after maximum iterations')

		# newton method with a pseudo inverse
		jac = self.cost_jacobian(state, fixed).transpose()
		increment = la.lstsq(jac.T, move, prec)[0]
		state = structure_state(
			flatten_state(state)
			+ increment * min(1, np.abs(max_increment / increment).max()),
			state)

		for i, joint in enumerate(joints):
			state[i] = joint.normalize(state[i])
	print('acheived in iteration', k, 'with error', error)

	# structure results
	result = iter(state)
	final = []
	for joint, default in zip(self.joints, close):
		if joint in fixed:
			final.append(fixed[joint])
		else:
			final.append(next(result))
	return final

freedom(state, precision=1e-06)

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

Source code in madcad/kinematic/solver.py

def freedom(self, state, precision=1e-6) -> 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
	'''
	return null_space(self.cost_jacobian(state), precision).transpose()

parts(state, precision=1e-06)

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

Parameters:

Name	Type	Description	Default
precision		error tolerance for kinematic loop closing, if a loop is above this threshold this function will raise KinematicError	1e-06

Source code in madcad/kinematic/solver.py

def parts(self, state, precision=1e-6) -> 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`
	'''
	if isinstance(state, dict):
		state = [state.get(joint) or joint.default  for joint in self.joints]
	transforms = {}
	# collect transformations
	for joint, p in zip(self.joints, state):
		transforms[joint] = joint.direct(p)
		if joint in self.rev:
			transforms[self.rev[joint]] = affineInverse(transforms[joint])
	# chain transformations
	poses = {self.order[0].solids[0]: mat4()}
	for joint in self.order:
		base = poses.get(joint.solids[0]) or mat4()
		tip = base * transforms[joint]
		if joint.solids[-1] in poses:
			# check that the given state is consistent, meaning kinematic loops are closed
			error = poses[joint.solids[-1]] - tip
			error[3] /= self.scale
			if np.abs(error).max() > precision:
				raise KinematicError('position out of reach: kinematic cycles not closed')
		else:
			poses[joint.solids[-1]] = tip
	return poses

display(scene)

display allowing manipulation of kinematic

Source code in madcad/kinematic/solver.py

def display(self, scene):
	''' display allowing manipulation of kinematic '''
	from .displays import KinematicManip
	return KinematicManip(scene, self)

to_chain()

Source code in madcad/kinematic/solver.py

def to_chain(self) -> 'Chain':
	if self.cycles:
		raise ValueError("this kinematic has cycles and do not form a chain")
	return Chain(self.cycles[0])

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 :math:(q_i)
• by a start-end matrix :math:T_{ab}

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

Attributes:

Name	Type	Description
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

Source code in madcad/kinematic/solver.py

def __init__(self, *args, default=0, **kwargs):
	if isinstance(args[0], Solid) and isinstance(args[1], Solid):
		self.solids = args[:2]
		args = args[2:]
	self.default = default
	self.init(*args, **kwargs)

normalize(state)

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.

Source code in madcad/kinematic/solver.py

def normalize(self, 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.
	'''
	return state

direct(state)

direct kinematic computation

Parameters:

Name	Type	Description	Default
state		the parameters defining the joint state It can be any type accepted by numpy.array	required

Returns:

Type	Description
'mat4'	the transfer matrix from solids self.stop to self.start

Source code in madcad/kinematic/solver.py

def direct(self, state) -> 'mat4':
	''' 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`
	'''
	raise NotImplemented

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

inverse kinematic computation

the default implementation is using a newton method to nullify the error between the given and acheived matrices. For precision and efficiency it is better to implement an exact solution whenever possible especially if self.grad is using finite differentiation.

Parameters:

Name	Type	Description	Default
matrix		the transfer matrix from solids self.stop to self.start we want the parameters for	required
close		a know solution we want the result the closest to. if not specified, it defaults to self.default	None
precision		desired error tolerance on the given matrix, this function returns once reached	1e-06
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	0.0
maxiter		maximum number of iterations allowed, or None if not limit	None

Returns:

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

Source code in madcad/kinematic/solver.py

def inverse(self, matrix, close=None, precision=1e-6, maxiter=None, strict=0.) -> list:
	''' inverse kinematic computation

	the default implementation is using a newton method to nullify the error between the given and acheived matrices. For precision and efficiency it is better to implement an exact solution whenever possible especially if `self.grad` is using finite differentiation.

	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)
	'''
	if close is None:
		close = self.default

	max_increment = flatten_state(self.increment)

	state = close
	k = 0
	while True:
		k += 1
		if maxiter and k > maxiter and not strict:
			break

		move = matrix - self.direct(state)
		error = np.abs(move).max()
		if error <= precision:
			break
		elif maxiter and k > maxiter:
			if error <= strict:
				break
			else:
				raise KinematicError('convergence failed after maximum iterations')

		# newton method with a pseudo inverse
		jac = self.grad(state)
		increment = la.solve(jac @ jac.transpose() + np.eye(len(jac))*self.prec, jac @ move)
		# apply correction to pose
		state = self.normalize(structure_state(
						flatten_state(state)
						+ increment * min(1, np.abs(max_increment / increment).max()), 
						state))
	return state

grad(state, delta=1e-06)

compute the gradient of the direct kinematic

The default implementation is using a finite differentiation. For precision it is better to implement an exact solution in every joint

Parameters:

Name	Type	Description	Default
state		anything accepted by self.direct() including singularities	required
delta		finite differentiation interval	1e-06

Returns:

Type	Description
'[mat4]'	a list of the matrix derivatives of self.direct(), one each parameter

Source code in madcad/kinematic/solver.py

def grad(self, state, delta=1e-6) -> '[mat4]':
	''' 
	compute the gradient of the direct kinematic 

	The default implementation is using a finite differentiation. For precision it is better to implement an exact solution in every joint

	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
	'''
	flattened = flatten_state(state)
	grad = []
	base = self.direct(state)
	for i in range(len(flattened)):
		grad.append(_partial_difference(self.direct, state, flattened, base, i, delta))
	return grad

scheme(size, junc=None)

generate the scheme elements to render the joint

Source code in madcad/kinematic/solver.py

def scheme(self, size: float, junc: vec3=None) -> '[Scheme]':
	''' generate the scheme elements to render the joint '''
	raise NotImplemented

display(scene)

display showing the schematics of this joint, without interaction

Source code in madcad/kinematic/solver.py

def display(self, scene):
	''' display showing the schematics of this joint, without interaction '''
	display = scene.display(self.scheme(dict(zip(self.solids, range(2))), inf, None, None))
	display.spacegens[0].pose = fmat4()
	display.spacegens[1].pose = fmat4(self.direct(self.default))
	return display

Weld(solids, transform=None)

Bases: Joint

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.

Source code in madcad/kinematic/solver.py

def __init__(self, solids, transform: mat4=None):
	self.solids = solids
	self.transform = transform or mat4()

bounds = ((), ()) class-attribute instance-attribute

increment = () class-attribute instance-attribute

default = () class-attribute instance-attribute

solids = solids instance-attribute

transform = transform or mat4() instance-attribute

direct(parameters)

Source code in madcad/kinematic/solver.py

def direct(self, parameters):
	assert len(parameters) == 0
	return self.transform

inverse(matrix, close=None)

Source code in madcad/kinematic/solver.py

def inverse(self, matrix, close=None):
	return ()

grad(parameters, delta=1e-06)

Source code in madcad/kinematic/solver.py

def grad(self, parameters, delta=1e-6):
	return ()

__repr__()

Source code in madcad/kinematic/solver.py

def __repr__(self):
	return '{}({}, {})'.format(self.__class__.__name__, self.solids, self.transform)

Free(solids)

Bases: Joint

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.

Source code in madcad/kinematic/solver.py

def __init__(self, solids):
	self.solids = solids

solids = solids instance-attribute

bounds = (np.array([-2] * 4 + [-inf] * 3, float), np.array([+2] * 4 + [+inf] * 3, float)) class-attribute instance-attribute

increment = np.array([0.5] * 4 + [inf] * 3, float) class-attribute instance-attribute

default = np.array([1, 0, 0, 0, 0, 0, 0], float) class-attribute instance-attribute

normalize(parameters)

Source code in madcad/kinematic/solver.py

def normalize(self, parameters):
	return np.concatenate([normalize(quat(parameters[:4])), parameters[4:]])

direct(parameters)

Source code in madcad/kinematic/solver.py

def direct(self, parameters):
	m = mat4(quat(parameters[:4]))
	m[3] = vec4(parameters[4:],1)
	return m

inverse(matrix, close=None)

Source code in madcad/kinematic/solver.py

def inverse(self, matrix, close=None):
	return np.concatenate([quat(matrix), matrix[3].xyz])

grad(parameters)

Source code in madcad/kinematic/solver.py

def grad(self, parameters):
	w,x,y,z,*_ = parameters
	return (
		# derivatives of quaternion based rotation matrix (column major of course)
		mat4(
			0,    2*z, -2*y, 0,
			-2*z,  0,    2*x, 0,
			2*y, -2*x,  0,   0,
			0,    0,    0,   0,
			),
		mat4(
			0,   2*y,  2*z, 0,
			2*y, -4*x,  2*w, 0,
			2*z, -2*w, -4*x, 0,
			0,   0,    0,   0,
			),
		mat4(
			-4*y,  2*x, -2*w, 0,
			2*x,  0,    2*z, 0,
			2*w,  2*z, -4*y, 0,
			0,    0,    0,   0,
			),
		mat4(
			-4*z,  2*w,  2*x, 0,
			-2*w, -4*z,  2*y, 0,
			2*x,  2*y,  0,   0,
			0,    0,    0,   0,
			),
		# derivatives of translation (column major of course)
		mat4(
			0,    0,    0,   0,
			0,    0,    0,   0,
			0,    0,    0,   0,
			1,    0,    0,   0,
			),
		mat4(
			0,    0,    0,   0,
			0,    0,    0,   0,
			0,    0,    0,   0,
			0,    1,    0,   0,
			),
		mat4(
			0,    0,    0,   0,
			0,    0,    0,   0,
			0,    0,    0,   0,
			0,    0,    1,   0,
			),
		)

__repr__()

Source code in madcad/kinematic/solver.py

def __repr__(self):
	return '{}({})'.format(self.__class__.__name__, self.solids)

Reverse(joint)

Bases: Joint

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

Source code in madcad/kinematic/solver.py

def __init__(self, joint):
	self.joint = joint
	self.solids = joint.solids[::-1]
	if hasattr(self.joint, 'position'):
		self.position = self.joint.position

joint = joint instance-attribute

solids = joint.solids[::(-1)] instance-attribute

position = self.joint.position instance-attribute

default property

bounds property

direct(parameters)

Source code in madcad/kinematic/solver.py

def direct(self, parameters):
	return affineInverse(self.joint.direct(parameters))

inverse(matrix, close=None)

Source code in madcad/kinematic/solver.py

def inverse(self, matrix, close=None):
	return self.joint.inverse(hinverse(matrix), close)

grad(parameters)

Source code in madcad/kinematic/solver.py

def grad(self, parameters):
	if hasattr(self.joint.default, '__len__'):
		f = self.joint.direct(parameters)
		return [- affineInverse(f) * df * affineInverse(f)
			for f, df in self.joint.grad(parameters)]
	else:
		f, df = self.joint.direct(parameters), self.joint.grad(parameters)
		return - affineInverse(f) * df * affineInverse(f)

__repr__()

Source code in madcad/kinematic/solver.py

def __repr__(self):
	return '{}({})'.format(self.__class__.__name__, self.joint)

scheme(index, maxsize, attach_start, attach_end)

Source code in madcad/kinematic/solver.py

def scheme(self, index, maxsize, attach_start, attach_end):
	return self.joint.scheme(index, maxsize, attach_end, attach_start)

Chain(joints, content=None, default=None)

Bases: Joint

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.

Attributes:

Name	Type	Description
joints	list	joints in the chaining order it must satisfy joints[i].solids[-1] == joints[i+1].solids[0]
content	list	displayables matching solids, its length must be len(joints)+1
solids	tuple	the (start,end) joints of the chain, as defined in Joint
default		the default joints positions
increment		the maximum change of joint positions before changin monotony of gradient
bounds		the (min,max) joints positions

Source code in madcad/kinematic/solver.py

def __init__(self, joints, content:list=None, default=None):
	if not all(joints[i-1].solids[-1] == joints[i].solids[0]
			for i in range(1, len(joints))):
		raise ValueError('joints do not form a direct chain, joints are not ordered or badly oriented')
	self.joints = joints
	self.content = content
	self.solids = (joints[0].solids[0], joints[-1].solids[-1])
	self.default = default or [joint.default  for joint in self.joints]
	self.increment = [joint.increment for joint in self.joints]
	self.bounds = (
		[joint.bounds[0]  for joint in self.joints],
		[joint.bounds[1]  for joint in self.joints],
		)
	self.scale = max(joint.scale  for joint in self.joints)

normalize(state)

inplace normalize the joint coordinates in the given state

Source code in madcad/kinematic/solver.py

def normalize(self, state) -> list:
	''' inplace normalize the joint coordinates in the given state '''
	for i, joint in enumerate(self.joints):
		state[i] = joint.normalize(state[i])
	return state

direct(state)

Source code in madcad/kinematic/solver.py

def direct(self, state) -> mat4:
	b = mat4()
	for x, joint in zip(state, self.joints):
		b *= joint.direct(x)
	return b

grad(state)

the jacobian of the flattened parameters list

Source code in madcad/kinematic/solver.py

def grad(self, state) -> list:
	''' the jacobian of the flattened parameters list '''
	# built the left side of the gradient product of the direct joint
	directs = []
	grad = []
	b = mat4(1)
	for x, joint in zip(state, self.joints):
		f = joint.direct(x)
		g = regularize_grad(joint.grad(x))
		directs.append((f, len(grad)))
		for df in g:
			grad.append(b*df)
		b = b*f
	# build the right side of the product
	b = mat4(1)
	i = len(grad)
	for f,n in reversed(directs):
		for i in reversed(range(n, i)):
			grad[i] = grad[i]*b
		b = f*b
	return grad

parts(state)

return the pose of each solid in the chain

Source code in madcad/kinematic/solver.py

def parts(self, state) -> list:
	''' return the pose of each solid in the chain '''
	solids = [mat4()] * (len(self.joints)+1)
	for i in range(len(self.joints)):
		solids[i+1] = solids[i] * self.joints[i].direct(state[i])
	return solids

to_kinematic()

Source code in madcad/kinematic/solver.py

def to_kinematic(self) -> 'Kinematic':
	return Kinematic(inputs=self.joints, outputs=[self.solids[-1]], ground=self.solids[0])

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

Source code in madcad/kinematic/solver.py

def to_dh(self) -> '(dh, transforms)':
	'''
	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
	'''
	raise NotImplementedError("In development")

from_dh(dh, transforms=None)

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

Source code in madcad/kinematic/solver.py

def from_dh(dh, transforms=None) -> 'Self':
	''' build a kinematic chain from a denavit-hartenberge representation, and eventual base definitions relative to the denavit-hartenberg convention '''
	raise NotImplementedError("In development")

display(scene)

display allowing manipulation of the chain

Source code in madcad/kinematic/solver.py

def display(self, scene):
	''' display allowing manipulation of the chain '''
	from .displays import ChainManip
	return ChainManip(scene, self)

arcs(conn)

find ars in the given graph

Source code in madcad/kinematic/solver.py

def arcs(conn: '{node: [node]}') -> '[[node]]':
	''' find ars in the given graph '''
	suites = []
	empty = ()
	edges = set()
	reached = set()
	for start in conn:
		if start in reached or len(conn.get(start, empty)) > 2:
			continue
		suite = [start]
		reached.add(start)
		def propagate():
			while True:
				node = suite[-1]
				reached.add(node)
				for child in conn.get(node, empty):
					if (child, node) not in edges:
						suite.append(child)
						edges.add((node, child))
						edges.add((child, node))
						break
				else:
					break
		propagate()
		suite.reverse()
		propagate()
		suites.append(suite)
	return suites

depthfirst(conn, starts=())

generate a depth-first traversal of the givne graph

Source code in madcad/kinematic/solver.py

def depthfirst(conn: '{node: [node]}', starts=()) -> '[(parent, child)]':
	'''
	generate a depth-first traversal of the givne graph

	![depthfirst](../schemes/kinematic-depthfirst.svg)
	'''
	edges = set()
	reached = set()
	ordered = []
	for start in itertools.chain(starts, conn):
		if start in reached:
			continue
		front = [(None, start)]
		while front:
			parent, node = front.pop()
			if (parent, node) in edges:
				continue
			reached.add(node)
			if parent is not None:
				edges.add((parent, node))
				edges.add((node, parent))
				ordered.append((parent, node))
			for child in conn[node]:
				if (node, child) not in reached:
					front.append((node, child))
	return ordered

cycles(conn)

extract a set of any-length cycles decomposing the graph

Source code in madcad/kinematic/solver.py

def cycles(conn: '{node: [node]}') -> '[[node]]':
	''' extract a set of any-length cycles decomposing the graph '''
	todo

shortcycles(conn, costs, branch=True)

extract a set of minimal cycles decompsing the graph

Source code in madcad/kinematic/solver.py

def shortcycles(conn: '{node: [node]}', costs: '{node: float}', branch=True) -> '[[node]]':
	'''
	extract a set of minimal cycles decompsing the graph

	![cycles](../schemes/kinematic-cycles.svg)
	'''
	# TODO: debug this because now it is not producing the shortest cycles
	# orient the graph in a depth-first way, and search for fusion points
	distances = {}
	merges = []
	tree = {}
	for parent, child in depthfirst(conn):
		if child in distances:
			merges.append((parent, child))
		else:
			tree[child] = parent
			if parent not in tree:
				distances[parent] = 0
			distances[child] = distances.get(parent, 0) + costs.get(child, 0)
	# sort merge points with
	#  - the first distance being the merge point distance to the root node
	#  - the second distance being the secondary branch bigest distance to the root node
	key = lambda edge: (distances[edge[1]], -distances[edge[0]])
	merges = sorted(merges, key=key)

	cycles = []
	c = len(merges)
	while c > 0:
		del merges[c:]
		c -= 1
		# collect candidates to next cycle
		# merge node that have the same distance may be concurrent cycles, so they must be sorted out
		while c > 0 and distances[merges[c][1]] == distances[merges[c-1][1]]:
			c -= 1
		# recompute distance to root
		# only the second distance can have changed since the depthfirst tree ensure that the first is already optimal
		for i in range(c, len(merges)):
			parent, child = merges[i]
			node, cost = parent, 0
			while node in tree:
				cost += costs.get(node, 0)
				node = tree[node]
			distances[parent] = cost
		# process all the candidates
		# the second distances cannot swap during this process, so any change will keep the same order
		for parent, child in sorted(merges[c:], key=key):
			# unroll from parent and child each on their own until union
			parenthood, childhood = [parent], [child]
			while parent != child:
				if distances[parent] >= distances[child]:
					parent = tree[parent]
					parenthood.append(parent)
				elif distances[parent] <= distances[child]:
					child = tree[child]
					childhood.append(child)
			# assemble cycle
			cycles.append(childhood[::-1] + parenthood)
			# report simplifications in the graph
			for i in range(1, len(parenthood)):
				parent, child = parenthood[i-1], parenthood[i]
				dist = distances[child] + costs.get(parent, 0)
				if dist >= distances[parent]:		break
				tree[parent] = child
				distances[parent] = dist
	return cycles

displays

__all__ = ['ChainManip', 'KinematicManip', 'scale_solid', 'world_solid'] module-attribute

index_toolcenter = 10000 module-attribute

kinematic_color_names = ['annotation', 'schematic'] module-attribute

ChainManip(scene, chain, pose=None, toolcenter=None)

Bases: Group

object to display and interact with a robot in the 3d view

Attributes:

Name	Type	Description
chain		the kinematic chain this display is rendering
pose		joints poses in the last rendered frame
parts		solids poses in the last rendered frame
toolcenter	vec3	current end-solid rotation point in rotation mode, relative to last solid

Source code in madcad/kinematic/displays.py

def __init__(self, scene, chain, pose=None, toolcenter=None):
	super().__init__(scene)
	self.chain = chain
	self.toolcenter = toolcenter or chain.joints[-1].position[1]
	self.pose = pose or chain.default
	self.parts = self.chain.parts(self.pose)
	self.defered = DeferedSolving()

	if chain.content:
		for key, solid in enumerate(chain.content):
			if scene.displayable(solid):
				self.displays[key] = scene.display(solid)

	scheme, index = kinematic_scheme(chain.joints)
	scheme += kinematic_toolcenter(self.toolcenter)
	self.displays['scheme'] = scene.display(scheme)

min_colinearity = 0.01 class-attribute instance-attribute

maxiter = 3 class-attribute instance-attribute

prec = 1e-06 class-attribute instance-attribute

chain = chain instance-attribute

toolcenter = toolcenter or chain.joints[-1].position[1] instance-attribute

pose = pose or chain.default instance-attribute

parts = self.chain.parts(self.pose) instance-attribute

defered = DeferedSolving() instance-attribute

stack(scene)

rendering stack requested by the madcad rendering system

Source code in madcad/kinematic/displays.py

def stack(self, scene):
	''' rendering stack requested by the madcad rendering system '''
	yield (self, 'screen', -1, self._place_solids)
	yield from super().stack(scene)

control(view, key, sub, evt)

user event manager, optional part of the madcad rendering system

Source code in madcad/kinematic/displays.py

def control(self, view, key, sub, evt):
	''' user event manager, optional part of the madcad rendering system '''
	if evt.type() == QEvent.Type.MouseButtonPress and evt.button() == Qt.MouseButton.LeftButton:
		evt.accept()

	if evt.type() == QEvent.Type.MouseMove and evt.buttons() & Qt.MouseButton.LeftButton:
		evt.accept()
		# put the tool into the view, to handle events
		# TODO: allow changing mode during move
		if sub == ('scheme', index_toolcenter):
			generator = self._move_tool(view, sub, evt)
		else:
			generator = getattr(self, '_move_'+view.scene.options['kinematic_manipulation'])(view, sub, evt)
		view.callbacks.append(receiver(generator))

KinematicManip(scene, kinematic, pose=None, toolcenter=None)

Bases: Group

Display that holds a kinematic structure and allows the user to move it

Attributes:

Name	Type	Description
kinematic		the kinematic this display is rendering
pose		joints poses in the last rendered frame
parts		solids poses in the last rendered frame
toolcenter		current solid rotation point in rotation mode, relative to kinematic ground

Source code in madcad/kinematic/displays.py

def __init__(self, scene, kinematic, pose=None, toolcenter=None):
	super().__init__(scene)
	self.kinematic = kinematic
	self.defered = DeferedSolving()
	self.toolcenter = toolcenter or vec3(0)
	self.pose = pose or self.kinematic.default
	try:
		self.pose = self.kinematic.solve(
			close=pose or self.kinematic.default, 
			maxiter=self.stay_maxiter, 
			precision=self.move_precision, 
			strict=self.tolerated_precision,
			)
	except KinematicError:
		warnings.warn("could not solve kinematic {} for displaying, falling back to infinite tolerance".format(self))
		self.pose = self.kinematic.default
		self.tolerated_precision = inf
	self.parts = self.kinematic.parts(self.pose, precision=self.tolerated_precision)

	if self.kinematic.content:
		for key, solid in self.kinematic.content.items():
			if scene.displayable(solid):
				self.displays[key] = scene.display(solid)

	scheme, self.index = kinematic_scheme(kinematic.joints)
	scheme += kinematic_toolcenter(self.toolcenter)
	self.displays['scheme'] = scene.display(scheme)

prec = 1e-06 class-attribute instance-attribute

move_maxiter = 5 class-attribute instance-attribute

stay_maxiter = 10000 class-attribute instance-attribute

move_precision = 0.0001 class-attribute instance-attribute

stay_precision = 1e-06 class-attribute instance-attribute

tolerated_precision = 0.1 class-attribute instance-attribute

damping = 0.9 class-attribute instance-attribute

kinematic = kinematic instance-attribute

defered = DeferedSolving() instance-attribute

toolcenter = toolcenter or vec3(0) instance-attribute

pose = self.kinematic.solve(close=(pose or self.kinematic.default), maxiter=(self.stay_maxiter), precision=(self.move_precision), strict=(self.tolerated_precision)) instance-attribute

parts = self.kinematic.parts(self.pose, precision=(self.tolerated_precision)) instance-attribute

stack(scene)

rendering stack requested by the madcad rendering system

Source code in madcad/kinematic/displays.py

def stack(self, scene):
	''' rendering stack requested by the madcad rendering system '''
	self.prepare(scene)
	yield (self, 'screen', -1, self._place_solids)
	for key, display in self.displays.items():
		display.world = self.world
		display.key = (*self.key, key)
		if key == 'scheme' and not scene.options['display_annotations'] and not self.selected:
			continue
		yield from display.stack(scene)

control(view, key, sub, evt)

user event manager, optional part of the madcad rendering system

Source code in madcad/kinematic/displays.py

def control(self, view, key, sub, evt):
	''' user event manager, optional part of the madcad rendering system '''
	# give priority to sub elements
	disp = self.displays
	for i in range(1,len(sub)):
		disp = disp[sub[i-1]]
		disp.control(view, sub[:i], sub[i:], evt)
		if evt.isAccepted(): return

	if evt.type() == QEvent.Type.MouseButtonPress and evt.button() == Qt.MouseButton.LeftButton:
		# accept mouse pressing to tell we are interested in mouse movements
		evt.accept()

	if evt.type() == QEvent.Type.MouseMove and evt.buttons() & Qt.MouseButton.LeftButton:
		evt.accept()
		# put the tool into the view, to handle events
		# TODO: allow changing mode during move
		if sub == ('scheme', index_toolcenter):
			generator = self._move_tool(view, sub, evt)
		else:
			generator = getattr(self, '_move_'+view.scene.options['kinematic_manipulation'])(view, sub, evt)
		view.callbacks.append(receiver(generator))

DeferedSolving()

helper executing solver iterations following ticks of a QTimer

Source code in madcad/kinematic/displays.py

def __init__(self):
	self.timer = None
	self.problem = None

timer = None instance-attribute

problem = None instance-attribute

set(prepare, solve, iterations)

schedule solving steps starting now for the given count of iterations

Source code in madcad/kinematic/displays.py

def set(self, prepare, solve, iterations):
	''' schedule solving steps starting now for the given count of iterations '''
	if not self.timer:
		self.timer = QTimer()
		self.timer.timeout.connect(self._step)
		self.timer.setInterval(10)
	self.problem = DeferedProblem(prepare, solve, iterations)
	if not self.timer.isActive():
		self.timer.start()

stop()

stop iterations

Source code in madcad/kinematic/displays.py

def stop(self):
	''' stop iterations '''
	self.timer.stop()

DeferedProblem(prepare, solve, iterations) dataclass

prepare instance-attribute

solve instance-attribute

iterations instance-attribute

scale_solid(solid, center, size, pose=fmat4()) dataclass

scheme space scaling around a point in a given solid

solid instance-attribute

center instance-attribute

size instance-attribute

pose = fmat4() class-attribute instance-attribute

__call__(view)

Source code in madcad/kinematic/displays.py

def __call__(self, view):
	m = view.uniforms['view'] * view.uniforms['world'] * self.pose
	e = view.uniforms['proj'] * fvec4(1,1,(m*self.center).z,1)
	e /= e[3]
	return m * translate(self.center) * scale(fvec3(min(self.size, 2 / (e[1]*view.target.height))))

world_solid(solid, pose=fmat4()) dataclass

solid instance-attribute

pose = fmat4() class-attribute instance-attribute

__call__(view)

Source code in madcad/kinematic/displays.py

def __call__(self, view):
	return view.uniforms['view'] * view.uniforms['world'] * self.pose

kinematic_toolcenter(toolcenter)

create a scheme for drawing the toolcenter in kinematic manipulation

Source code in madcad/kinematic/displays.py

def kinematic_toolcenter(toolcenter):
	''' create a scheme for drawing the toolcenter in kinematic manipulation '''
	color = settings.colors['annotation']
	angle = 2
	radius = 60
	size = 3
	tend = -sin(angle)*X + cos(angle)*Y
	rend = cos(angle)*X + sin(angle)*Y
	sch = Scheme(
		space=halo_screen(fvec3(toolcenter)), 
		layer=1e-4,
		shader='line',
		track=index_toolcenter,
		)
	sch.add([size*cos(t)*X + size*sin(t)*Y   for t in linrange(0, 2*pi, step=0.2)], color=fvec4(color,1))
	sch.add([radius*cos(t)*X + radius*sin(t)*Y  for t in linrange(0, angle, step=0.1)], color=fvec4(color,1))
	sch.add([radius*cos(t)*X + radius*sin(t)*Y  for t in linrange(angle, 2*pi, step=0.1)], color=fvec4(color,0.2))
	sch.add(
		revolution(web([radius*rend, radius*rend-14*tend-3.6*rend]), Axis(radius*rend, tend), resolution=('div', 8)), 
		shader='fill',
		color=fvec4(color,0.8),
		)
	return sch

kinematic_scheme(joints)

create a kinematic scheme for the given joints

Source code in madcad/kinematic/displays.py

def kinematic_scheme(joints) -> '(Scheme, index)':
	''' create a kinematic scheme for the given joints '''
	index = {}
	centers = {}
	for joint in joints:
		if hasattr(joint, 'position'):
			for solid, position in zip(joint.solids, joint.position):
				centers[solid] = centers.get(solid, 0) + vec4(position, 1)
				index[solid] = index.get(solid, len(index))
	for solid, center in centers.items():
		centers[solid] = center.xyz / center.w  if center.w > 1 else None

	scheme = Scheme()
	for joint in joints:
		if hasattr(joint, 'position'):
			size = vec2(sum(
				vec2(distance(position, centers[solid]), 1)
				for solid, position in zip(joint.solids, joint.position)
				if centers[solid]))
			if size.y:
				size = 1.5*size.x / size.y
			else:
				size = inf

			sch = joint.scheme(index, size, centers[joint.solids[0]], centers[joint.solids[-1]])
			if sch is not NotImplemented:
				scheme += sch

	return scheme, {v: k  for k, v in index.items()}

kinematic_color(i)

return the scheme color vector for solid i in a kinematic

Source code in madcad/kinematic/displays.py

def kinematic_color(i):
	''' return the scheme color vector for solid `i` in a kinematic '''
	return fvec4(settings.colors[kinematic_color_names[i%2]], 1)

qtpos(qtpos, view)

convert qt position in the widget to opengl screen coords in range (-1, 1)

Source code in madcad/kinematic/displays.py

def qtpos(qtpos, view):
	''' convert qt position in the widget to opengl screen coords in range (-1, 1) '''
	return vec2(
		+ (qtpos.x()/view.width() *2 -1),
		- (qtpos.y()/view.height() *2 -1),
		)

normsq(x)

Source code in madcad/kinematic/displays.py

def normsq(x):
	x = x.ravel()
	return np.dot(x,x)