def deblock(B):
""" Flatten a BlockMatrix of BlockMatrices """
if not isinstance(B, BlockMatrix) or not B.blocks.has(BlockMatrix):
return B
wrap = lambda x: x if isinstance(x, BlockMatrix) else BlockMatrix([[x]])
bb = B.blocks.applyfunc(wrap) # everything is a block
try:
MM = Matrix(0, sum(bb[0, i].blocks.shape[1] for i in range(bb.shape[1])), [])
for row in range(0, bb.shape[0]):
M = Matrix(bb[row, 0].blocks)
for col in range(1, bb.shape[1]):
M = M.row_join(bb[row, col].blocks)
MM = MM.col_join(M)
return BlockMatrix(MM)
except ShapeError:
return B
def deblock(B):
""" Flatten a BlockMatrix of BlockMatrices """
if not isinstance(B, BlockMatrix) or not B.blocks.has(BlockMatrix):
return B
wrap = lambda x: x if isinstance(x, BlockMatrix) else BlockMatrix([[x]])
bb = B.blocks.applyfunc(wrap) # everything is a block
from sympy import Matrix
try:
MM = Matrix(0, sum(bb[0, i].blocks.shape[1] for i in range(bb.shape[1])), [])
for row in range(0, bb.shape[0]):
M = Matrix(bb[row, 0].blocks)
for col in range(1, bb.shape[1]):
M = M.row_join(bb[row, col].blocks)
MM = MM.col_join(M)
return BlockMatrix(MM)
except ShapeError:
return B
# Rotations performed on individual Frames for A->C->D->E
Rc_a = rot_x(0)
Rd_c = rot_x(90 * dtr)
Re_d = rot_z(90 * dtr)
# Define Translations between frames.
ta = Matrix([[0], [0], [0], [1]])
tb_a = Matrix([[-2], [2], [4], [1]])
te_b = Matrix([[0], [2], [0], [1]])
tc_a = Matrix([[4], [4], [0], [1]])
td_c = Matrix([[-3], [3], [2], [1]])
te_d = Matrix([[-3], [2], [3], [1]])
# Define homogenous transformation matrices
# HINT: Check out sympy's documentation for functions row_join and col_join
Ta = Ra.col_join(Matrix([[0, 0, 0]])).row_join(ta)
Tb_a = Rb_a.col_join(Matrix([[0, 0, 0]])).row_join(tb_a)
Te_b = Re_b.col_join(Matrix([[0, 0, 0]])).row_join(te_b)
Tc_a = Rc_a.col_join(Matrix([[0, 0, 0]])).row_join(tc_a)
Td_c = Rd_c.col_join(Matrix([[0, 0, 0]])).row_join(td_c)
Te_d = Re_d.col_join(Matrix([[0, 0, 0]])).row_join(te_d)
# Composition of Transformations
Te_a_1 = simplify(Ta * Tb_a * Te_b)
Te_a_2 = simplify(Ta * Tc_a * Td_c * Te_d)
# Calculate orientation and position for E
E_1 = Te_a_1.evalf(subs={q1: 0, q2: 0}, chop=True)
E_2 = Te_a_2.evalf(subs={q3: 0, q4: 0}, chop=True)
print("Transformation Matrix for A->B->E:")
