def test_mass_matrix(self):
thetalist = np.array([0.1, 0.1, 0.1])
M01 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.089159],
[0, 0, 0, 1]])
M12 = np.array([[0, 0, 1, 0.28], [0, 1, 0, 0.13585], [-1, 0, 0, 0],
[0, 0, 0, 1]])
M23 = np.array([[1, 0, 0, 0], [0, 1, 0, -0.1197], [0, 0, 1, 0.395],
[0, 0, 0, 1]])
M34 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.14225],
[0, 0, 0, 1]])
G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])
G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])
G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])
Glist = np.array([G1, G2, G3])
Mlist = np.array([M01, M12, M23, M34])
Slist = np.array([[1, 0, 1, 0, 1, 0], [0, 1, 0, -0.089, 0, 0],
[0, 1, 0, -0.089, 0, 0.425]]).T
correct_output = mr.MassMatrix(thetalist, Mlist, Glist, Slist)
output = util.MassMatrix(thetalist, Mlist, Glist, Slist)
self.assertEqual(True, matrix_equal(correct_output, output))
M67 = [[1, 0, 0, 0], [0, 0, 1, 0.0823], [0, -1, 0, 0], [0, 0, 0, 1]]
G1 = np.diag([0.010267495893, 0.010267495893, 0.00666, 3.7, 3.7, 3.7])
G2 = np.diag([0.22689067591, 0.22689067591, 0.0151074, 8.393, 8.393, 8.393])
G3 = np.diag([0.049443313556, 0.049443313556, 0.004095, 2.275, 2.275, 2.275])
G4 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219])
G5 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219])
G6 = np.diag(
[0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879])
Glist = [G1, G2, G3, G4, G5, G6]
Mlist = [M01, M12, M23, M34, M45, M56, M67]
Slist = [[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 1], [1, 0, 0, 0, -1, 0],
[0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491],
[0, 0, 0, 0, 0.81725, 0], [0, 0, 0.425, 0.81725, 0, 0.81725]]
thetaList = np.array(
[0, math.pi / 6, math.pi / 4, math.pi / 3, math.pi / 2, 2 * math.pi / 3])
dthetaList = np.array([0.2, 0.2, 0.2, 0.2, 0.2, 0.2])
ddthetaList = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
g = np.array([0, 0, -9.81])
Ftip = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
massMatrix = mr.MassMatrix(thetaList, Mlist, Glist, Slist)
QuadraticForces = mr.VelQuadraticForces(thetaList, dthetaList, Mlist, Glist,
Slist)
gravityForces = mr.GravityForces(thetaList, g, Mlist, Glist, Slist)
# with np.printoptions(precision=3, suppress=True): print mr.EndEffectorForces(thetaList, Ftip, Mlist, Glist, Slist)
tauList = np.array([0.0128, -41.1477, -3.7809, 0.0323, 0.0370, 0.1034])
with np.printoptions(precision=3, suppress=True):
print mr.ForwardDynamics(thetaList, dthetaList, tauList, g, Ftip, Mlist,
Glist, Slist)
eta = np.zeros([2, N])
if iteration == 1:
tau_first = np.array([])
for t in range(1, N + 1): # Dynamics equations.
t1 = ot[0, t]
t2 = ot[1, t]
t1dot = otdot[0, t]
t2dot = otdot[1, t]
# Theta values begin at [0,0,0] which corresponds to the middle
# of the given angle range.
thetalist = np.array([t1, t2])
# Derivatives are approximations for now
dthetalist = np.array([t1dot, t2dot])
M = mr.MassMatrix(thetalist, Mlist, Glist, Slist)
W = mr.VelQuadraticForces(thetalist, dthetalist, Mlist, Glist,
Slist)
eta[:, t - 1] = np.negative(
sum([M @ otddot[:, t], W @ otdot[:, t]]))
if iteration == 1 and t == 1:
tau_first = np.array(
sum([M @ otddot[:, t], W @ otdot[:, t]]))
elif iteration == 1:
value = sum([M @ otddot[:, t], W @ otdot[:, t]])
if greatest_tau < max(abs(value)):
greatest_tau = max(abs(value))
tau_first = np.c_[tau_first, (
sum([M @ otddot[:, t], W @ otdot[:, t]]))]
def p1(self):
print('P1')
M = mr.MassMatrix(self.thetalist, self.Mlist, self.Glist, self.Slist)
print('M =\n%s' % M)