def direct_dynamic_NE(robo):
"""Computes Direct Dynamic Model using
Newton-Euler formulation
Parameters
==========
robo : Robot
Instance of robot description container
Returns
=======
symo.sydi : dictionary
Dictionary with the information of all the sybstitution
"""
wi = Init.init_vec(robo)
# antecedent angular velocity, projected into jth frame
w = Init.init_w(robo)
jaj = Init.init_vec(robo, 6)
jTant = Init.init_mat(robo, 6) # Twist transform list of Matrices 6x6
beta_star = Init.init_vec(robo, 6)
grandJ = Init.init_mat(robo, 6)
link_acc = Init.init_vec(robo, 6)
H_inv = Init.init_scalar(robo)
juj = Init.init_vec(robo, 6) # Jj*aj / Hj
Tau = Init.init_scalar(robo)
grandVp = Init.init_vec(robo, 6)
grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0]))
symo = Symoro()
symo.file_open(robo, 'ddm')
title = 'Direct dynamic model using Newton - Euler Algorith'
symo.write_params_table(robo, title, inert=True, dynam=True)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
for j in xrange(1, robo.NL):
compute_omega(robo, symo, j, antRj, w, wi)
compute_screw_transform(robo, symo, j, antRj, antPj, jTant)
if robo.sigma[j] == 0:
jaj[j] = Matrix([0, 0, 0, 0, 0, 1])
elif robo.sigma[j] == 1:
jaj[j] = Matrix([0, 0, 1, 0, 0, 0])
for j in xrange(1, robo.NL):
compute_beta(robo, symo, j, w, beta_star)
compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi)
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
for j in reversed(xrange(1, robo.NL)):
replace_beta_J_star(robo, symo, j, grandJ, beta_star)
compute_Tau(robo, symo, j, grandJ, beta_star, jaj, juj, H_inv, Tau)
if robo.ant[j] != - 1:
compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau,
beta_star, jTant, link_acc)
for j in xrange(1, robo.NL):
compute_acceleration(robo, symo, j, jTant, grandVp,
juj, H_inv, jaj, Tau, link_acc)
for j in xrange(1, robo.NL):
compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star)
symo.file_close()
return symo
def compute_rot_trans(robo, symo):
#init transformation
antRj = Init.init_mat(robo)
antPj = Init.init_vec(robo)
for j in xrange(robo.NL):
compute_transform(robo, symo, j, antRj, antPj)
return antRj, antPj
