def _assemble_forces_equations(self):
node = 'ground'
nrows = 2
F_applied = sm.MutableSparseMatrix(nrows, 1, None)
in_edges = self.forces_graph.in_edges([node], data='obj')
if len(in_edges) == 0:
Q_in_R = zero_matrix(3, 1)
Q_in_P = zero_matrix(4, 1)
else:
Q_in_R = sm.MatAdd(*[e[-1].Qj.blocks[0] for e in in_edges])
Q_in_P = sm.MatAdd(*[e[-1].Qj.blocks[1] for e in in_edges])
out_edges = self.forces_graph.out_edges([node], data='obj')
if len(out_edges) == 0:
Q_out_R = zero_matrix(3, 1)
Q_out_P = zero_matrix(4, 1)
else:
Q_out_R = sm.MatAdd(*[e[-1].Qi.blocks[0] for e in out_edges])
Q_out_P = sm.MatAdd(*[e[-1].Qi.blocks[1] for e in out_edges])
Q_t_R = Q_in_R + Q_out_R
Q_t_P = Q_in_P + Q_out_P
F_applied[0] = Q_t_R
F_applied[1] = Q_t_P
self.frc_equations = F_applied
def _assemble_forces_equations(self):
graph = self.forces_graph
nodes = self.bodies
nrows = 2 * len(nodes)
F_applied = sm.MutableSparseMatrix(nrows, 1, None)
for i, n in enumerate(nodes):
if self._is_virtual_node(n):
continue
in_edges = graph.in_edges([n], data='obj')
if len(in_edges) == 0:
Q_in_R = zero_matrix(3, 1)
Q_in_P = zero_matrix(4, 1)
else:
Q_in_R = sm.MatAdd(*[e[-1].Qj.blocks[0] for e in in_edges])
Q_in_P = sm.MatAdd(*[e[-1].Qj.blocks[1] for e in in_edges])
out_edges = graph.out_edges([n], data='obj')
if len(out_edges) == 0:
Q_out_R = zero_matrix(3, 1)
Q_out_P = zero_matrix(4, 1)
else:
Q_out_R = sm.MatAdd(*[e[-1].Qi.blocks[0] for e in out_edges])
Q_out_P = sm.MatAdd(*[e[-1].Qi.blocks[1] for e in out_edges])
Q_t_R = Q_in_R + Q_out_R
Q_t_P = Q_in_P + Q_out_P
F_applied[i * 2] = Q_t_R
F_applied[i * 2 + 1] = Q_t_P
self.frc_equations = F_applied
def _assemble_mass_matrix(self):
nodes = self.nodes
bodies = self.bodies
n = 2 * len(bodies)
matrix = sm.MutableSparseMatrix(n, n, None)
mass_matricies = [[nodes[i]['obj'].M, nodes[i]['obj'].J]
for i in bodies]
mass_matricies = sum(mass_matricies, [])
for i, m in enumerate(mass_matricies):
matrix[i, i] = m
self.mass_equations = matrix
def _assemble_constraints_equations(self):
nodelist = self.nodes
cols = 2 * len(nodelist)
nve = 2
equations = sm.MutableSparseMatrix(nve, 1, None)
vel_rhs = sm.MutableSparseMatrix(nve, 1, None)
acc_rhs = sm.MutableSparseMatrix(nve, 1, None)
jacobian = sm.MutableSparseMatrix(nve, cols, None)
row_ind = 0
b = self.nodes['ground']['obj']
i = self.nodes_indicies['ground']
jacobian[row_ind:row_ind + 2,
i * 2:i * 2 + 2] = b.normalized_jacobian.blocks
equations[row_ind:row_ind + 2, 0] = b.normalized_pos_equation.blocks
vel_rhs[row_ind:row_ind + 2, 0] = b.normalized_vel_equation.blocks
acc_rhs[row_ind:row_ind + 2, 0] = b.normalized_acc_equation.blocks
self.pos_equations = equations
self.vel_equations = vel_rhs
self.acc_equations = acc_rhs
self.jac_equations = jacobian
def smith_normal_form(matrix, augment=None):
"""Computes the Smith normal form of the given matrix.
Args:
matrix:
augment:
Returns:
n x n smith normal form of the matrix.
Particularly for projection onto the nullspace of M and the orthogonal
complement that is, for a matrix M,
P = _smith_normal_form(M) is a projection operator onto the
nullspace of M
"""
if augment is not None:
M = matrix.row_join(augment)
k = augment.cols
else:
M = matrix
k = 0
m, n = M.shape
M = augmented_rref(M, k)
Mp = sympy.MutableSparseMatrix(n - k, n, {})
constraints = []
for row in range(m):
leading_coeff = -1
for col in range(row, n - k):
if M[row, col] != 0:
leading_coeff = col
break
if leading_coeff < 0:
if not M[row, n - k:].is_zero_matrix:
constraints.append(sum(M[row, :]))
else:
Mp[leading_coeff, :] = M[row, :]
if augment:
return Mp[:, :-k], Mp[:, -k:], constraints
else:
return Mp, sympy.SparseMatrix(m, k, {}), constraints
def smith_normal_form(matrix, augment=None):
"""Computes the Smith normal form of the given matrix.
Args:
matrix:
augment:
Returns:
n x n smith normal form of the matrix.
Particularly for projection onto the nullspace of M and the orthogonal
complement that is, for a matrix M,
P = _smith_normal_form(M) is a projection operator onto the nullspace of M
"""
# M, _ = matrix.rref()
# m, n = M.shape
# M = sympy.SparseMatrix(m, n, M)
# m_dict = {}
# current_row = 0
#
# row_map = {}
#
# current_row = 0
#
# for row, c_idx, entry in M.RL:
# if row not in row_map:
# row_map[row] = c_idx
# r_idx = c_idx
#
# else:
# r_idx = row_map[row]
#
# m_dict[(r_idx, c_idx)] = entry
#
# return sympy.SparseMatrix(n, n, m_dict)
if augment:
M = matrix.row_join(augment)
k = augment.cols
else:
M = matrix
k = 0
m, n = M.shape
M = augmented_rref(M, k)
Mp = sympy.MutableSparseMatrix(n - k, n, {})
constraints = []
for row in range(m):
leading_coeff = -1
for col in range(row, n - k):
if M[row, col] != 0:
leading_coeff = col
break
if leading_coeff < 0:
if not M[row, n - k:].is_zero:
constraints.append(sum(M[row, :]))
else:
Mp[leading_coeff, :] = M[row, :]
if augment:
return Mp[:, :-k], Mp[:, -k:], constraints
else:
return Mp, sympy.SparseMatrix(m, k, {}), constraints
def _assemble_mass_matrix(self):
ground_obj = self.nodes['ground']['obj']
matrix = sm.MutableSparseMatrix(2, 2, None)
matrix[0, 0] = ground_obj.M
matrix[1, 1] = ground_obj.J
self.mass_equations = matrix
def _assemble_constraints_equations(self):
edges = self.constraints_graph.edges
nodes = self.nodes
node_index = self.nodes_indicies
cols = 2 * len(nodes)
nve = self.nve
equations = sm.MutableSparseMatrix(nve, 1, None)
vel_rhs = sm.MutableSparseMatrix(nve, 1, None)
acc_rhs = sm.MutableSparseMatrix(nve, 1, None)
jacobian = sm.MutableSparseMatrix(nve, cols, None)
row_ind = 0
for e in edges:
if self._is_virtual_edge(e):
continue
eo = edges[e]['obj']
u, v = e[:-1]
# tracker of row index based on the current joint type and the history
# of the loop
eo_nve = eo.nve + row_ind
ui = node_index[u]
vi = node_index[v]
# assigning the joint jacobians to the propper index in the system jacobian
# on the "constraint vector equations" level.
jacobian[row_ind:eo_nve, ui * 2:ui * 2 + 2] = eo.jacobian_i.blocks
jacobian[row_ind:eo_nve, vi * 2:vi * 2 + 2] = eo.jacobian_j.blocks
equations[row_ind:eo_nve, 0] = eo.pos_level_equations.blocks
vel_rhs[row_ind:eo_nve, 0] = eo.vel_level_equations.blocks
acc_rhs[row_ind:eo_nve, 0] = eo.acc_level_equations.blocks
row_ind += eo.nve
for i, n in enumerate(nodes):
if self._is_virtual_node(n):
continue
b = nodes[n]['obj']
if isinstance(b, bodies.ground):
jacobian[row_ind:row_ind + 2,
i * 2:i * 2 + 2] = b.normalized_jacobian.blocks
equations[row_ind:row_ind + 2,
0] = b.normalized_pos_equation.blocks
vel_rhs[row_ind:row_ind + 2,
0] = b.normalized_vel_equation.blocks
acc_rhs[row_ind:row_ind + 2,
0] = b.normalized_acc_equation.blocks
else:
jacobian[row_ind, i * 2 + 1] = b.normalized_jacobian[1]
equations[row_ind, 0] = b.normalized_pos_equation
vel_rhs[row_ind, 0] = b.normalized_vel_equation
acc_rhs[row_ind, 0] = b.normalized_acc_equation
row_ind += b.nve
self.pos_equations = equations
self.vel_equations = vel_rhs
self.acc_equations = acc_rhs
self.jac_equations = jacobian