def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True):
"""Internal function. Computes speeds and accelerations usitn
Parameters
==========
robo : Robot
Instance of robot description container
symo : Symoro
Instance of symbolic manager
"""
#init velocities and accelerations
w = Init.init_w(robo)
wdot, vdot = Init.init_wv_dot(robo, gravity)
#init auxilary matrix
U = Init.init_U(robo)
for j in xrange(1, robo.NL):
jRant = antRj[j].T
qdj = Z_AXIS * robo.qdot[j]
qddj = Z_AXIS * robo.qddot[j]
wi = _omega_i(robo, symo, j, jRant, w)
symo.mat_replace(wi, 'WI', j)
w[j] = _omega_j(robo, j, jRant, w, wi, qdj)
symo.mat_replace(w[j], 'W', j)
_omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj)
symo.mat_replace(wdot[j], 'WP', j, forced)
_v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj)
symo.mat_replace(vdot[j], 'VP', j, forced)
return w, wdot, vdot, U
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
def Newton_Euler(robo, symo):
"""Internal function. Computes Inverse Dynamic Model using
Newton-Euler formulation
Parameters
==========
robo : Robot
Instance of robot description container
symo : Symoro
Instance of symbolic manager
"""
# init external forces
Fex = copy(robo.Fex)
Nex = copy(robo.Nex)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
# init velocities and accelerations
w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
# init forces vectors
F = Init.init_vec(robo)
N = Init.init_vec(robo)
Fjnt = Init.init_vec(robo)
Njnt = Init.init_vec(robo)
for j in xrange(1, robo.NL):
compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N)
for j in reversed(xrange(1, robo.NL)):
compute_joint_wrench(robo, symo, j, antRj, antPj, vdot,
Fjnt, Njnt, F, N, Fex, Nex)
for j in xrange(1, robo.NL):
compute_torque(robo, symo, j, Fjnt, Njnt)
def velocities(robo):
symo = Symoro(None)
symo.file_open(robo, 'vel')
symo.write_params_table(robo, 'Link velocities')
antRj, antPj = compute_rot_trans(robo, symo)
w = Init.init_w(robo)
v = Init.init_v(robo)
for j in xrange(1, robo.NL):
jRant = antRj[j].T
qdj = Z_AXIS * robo.qdot[j]
wi = _omega_i(robo, symo, j, jRant, w)
w[j] = _omega_j(robo, j, jRant, w, wi, qdj)
symo.mat_replace(w[j], 'W', j, forced=True)
_v_j(robo, j, antPj, jRant, v, w, qdj)
symo.mat_replace(v[j], 'V', j, forced=True)
symo.file_close()
return symo
def inertia_matrix(robo):
"""Computes Inertia Matrix using composed link
Parameters
==========
robo : Robot
Instance of robot description container
Returns
=======
symo.sydi : dictionary
Dictionary with the information of all the sybstitution
"""
Jplus, MSplus, Mplus = Init.init_Jplus(robo)
AJE1 = Init.init_vec(robo)
f = Init.init_vec(robo, ext=1)
n = Init.init_vec(robo, ext=1)
A = sympy.zeros(robo.NL, robo.NL)
symo = Symoro()
symo.file_open(robo, 'inm')
title = 'Inertia Matrix using composite links'
symo.write_params_table(robo, title, inert=True, dynam=True)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
for j in reversed(xrange(-1, robo.NL)):
replace_Jplus(robo, symo, j, Jplus, MSplus, Mplus)
if j != - 1:
compute_Jplus(robo, symo, j, antRj, antPj,
Jplus, MSplus, Mplus, AJE1)
for j in xrange(1, robo.NL):
compute_A_diagonal(robo, symo, j, Jplus, MSplus, Mplus, f, n, A)
ka = j
while ka != - 1:
k = ka
ka = robo.ant[ka]
compute_A_triangle(robo, symo, j, k, ka,
antRj, antPj, f, n, A, AJE1)
symo.mat_replace(A, 'A', forced=True, symmet=True)
J_base = inertia_spatial(Jplus[-1], MSplus[-1], Mplus[-1])
symo.mat_replace(J_base, 'JP', 0, forced=True, symmet=True)
symo.file_close()
return symo
def jdot_qdot(robo):
symo = Symoro(None)
symo.file_open(robo, 'jpqp')
symo.write_params_table(robo, 'JdotQdot')
antRj, antPj = compute_rot_trans(robo, symo)
w = Init.init_w(robo)
wdot, vdot = Init.init_wv_dot(robo, gravity=False)
U = Init.init_U(robo)
for j in xrange(1, robo.NL):
jRant = antRj[j].T
qdj = Z_AXIS * robo.qdot[j]
qddj = Z_AXIS * ZERO
wi = _omega_i(robo, symo, j, jRant, w)
symo.mat_replace(wi, 'WI', j)
w[j] = _omega_j(robo, j, jRant, w, wi, qdj)
symo.mat_replace(w[j], 'W', j)
_omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj)
symo.mat_replace(wdot[j], 'WPJ', j, forced=True)
_v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj)
symo.mat_replace(vdot[j], 'VPJ', j, forced=True)
symo.file_close()
return symo
def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj):
DV = Init.product_combinations(w[j])
symo.mat_replace(DV, 'DV', j)
hatw_hatw = Matrix([[-DV[3]-DV[5], DV[1], DV[2]],
[DV[1], -DV[5]-DV[0], DV[4]],
[DV[2], DV[4], -DV[3]-DV[0]]])
U[j] = hatw_hatw + hat(wdot[j])
symo.mat_replace(U[j], 'U', j)
vsp = vdot[robo.ant[j]] + U[robo.ant[j]]*antPj[j]
symo.mat_replace(vsp, 'VSP', j)
vdot[j] = jRant*vsp
if robo.sigma[j] == 1: # prismatic joint
vdot[j] += qddj + 2*hat(wi)*qdj
return vdot[j]
def dynamic_identification_NE(robo):
"""Computes Dynamic Identification 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
"""
# init forces vectors
Fjnt = Init.init_vec(robo)
Njnt = Init.init_vec(robo)
# init file output, writing the robot description
symo = Symoro()
symo.file_open(robo, 'dim')
title = "Dynamic identification model using Newton - Euler Algorith"
symo.write_params_table(robo, title, inert=True, dynam=True)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
# init velocities and accelerations
w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj)
# virtual robot with only one non-zero parameter at once
robo_tmp = deepcopy(robo)
robo_tmp.IA = sympy.zeros(robo.NL, 1)
robo_tmp.FV = sympy.zeros(robo.NL, 1)
robo_tmp.FS = sympy.zeros(robo.NL, 1)
for k in xrange(1, robo.NL):
param_vec = robo.get_inert_param(k)
F = Init.init_vec(robo)
N = Init.init_vec(robo)
for i in xrange(10):
if param_vec[i] == ZERO:
continue
# change link names according to current non-zero parameter
robo_tmp.num = [str(l) + str(param_vec[i])
for l in xrange(k + 1)]
# set the parameter to 1
mask = sympy.zeros(10, 1)
mask[i] = 1
robo_tmp.put_inert_param(mask, k)
# compute the total forcec of the link k
compute_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N)
# init external forces
Fex = copy(robo.Fex)
Nex = copy(robo.Nex)
for j in reversed(xrange(k + 1)):
compute_joint_wrench(robo_tmp, symo, j, antRj, antPj,
vdot, Fjnt, Njnt, F, N, Fex, Nex)
for j in xrange(k + 1):
compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG')
# reset all the parameters to zero
robo_tmp.put_inert_param(sympy.zeros(10, 1), k)
# compute model for the joint parameters
compute_joint_torque_deriv(symo, robo.IA[k],
robo.qddot[k], k)
compute_joint_torque_deriv(symo, robo.FS[k],
sympy.sign(robo.qdot[k]), k)
compute_joint_torque_deriv(symo, robo.FV[k],
robo.qdot[k], k)
# closing the output file
symo.file_close()
return symo
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
