def mobile_inverse_dynmodel(robo, symo):
"""
Compute the Inverse Dynamic Model using Newton-Euler algorithm for
mobile robots.
Parameters:
robo: Robot - instance of robot description container
symo: symbolmgr.SymbolManager - instance of symbol 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 = ParamsInit.init_vec(robo)
N = ParamsInit.init_vec(robo)
Fjnt = ParamsInit.init_vec(robo)
Njnt = ParamsInit.init_vec(robo)
# init torque list
torque = ParamsInit.init_scalar(robo)
for j in xrange(0, robo.NL):
compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N)
for j in reversed(xrange(0, robo.NL)):
compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, F, N, Fjnt,
Njnt, Fex, Nex)
for j in xrange(1, robo.NL):
compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque)
def Newton_Euler(robo, symo):
"""Internal function. Computes Inverse Dynamic Model using
Newton-Euler formulation
Parameters
==========
robo : Robot
Instance of robot description container
symo : symbolmgr.SymbolManager
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 = ParamsInit.init_vec(robo)
N = ParamsInit.init_vec(robo)
Fjnt = ParamsInit.init_vec(robo)
Njnt = ParamsInit.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 Newton_Euler(robo, symo):
"""Internal function. Computes Inverse Dynamic Model using
Newton-Euler formulation
Parameters
==========
robo : Robot
Instance of robot description container
symo : symbolmgr.SymbolManager
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 = ParamsInit.init_vec(robo)
N = ParamsInit.init_vec(robo)
Fjnt = ParamsInit.init_vec(robo)
Njnt = ParamsInit.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 mobile_inverse_dynmodel(robo, symo):
"""
Compute the Inverse Dynamic Model using Newton-Euler algorithm for
mobile robots.
Parameters:
robo: Robot - instance of robot description container
symo: symbolmgr.SymbolManager - instance of symbol 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 = ParamsInit.init_vec(robo)
N = ParamsInit.init_vec(robo)
Fjnt = ParamsInit.init_vec(robo)
Njnt = ParamsInit.init_vec(robo)
# init torque list
torque = ParamsInit.init_scalar(robo)
for j in xrange(0, robo.NL):
compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N)
for j in reversed(xrange(0, robo.NL)):
compute_joint_wrench(
robo, symo, j, antRj, antPj, vdot,
F, N, Fjnt, Njnt, Fex, Nex
)
for j in xrange(1, robo.NL):
compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque)
def floating_inertia_matrix(robo, symo):
"""
Compute Inertia Matrix for robots with floating or mobile base. This
function computes the A11, A12 and A22 matrices when the inertia
matrix A = [A11, A12; A12.transpose(), A22]
"""
# init terms
comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo)
aje1 = ParamsInit.init_vec(robo)
forces = ParamsInit.init_vec(robo, ext=1)
moments = ParamsInit.init_vec(robo, ext=1)
inertia_a12 = ParamsInit.init_vec(robo, num=6)
inertia_a22 = sympy.zeros(robo.nl, robo.nl)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
for j in reversed(xrange(0, robo.NL)):
replace_composite_terms(
symo, j, comp_inertia3, comp_ms, comp_mass
)
if j != 0:
compute_composite_inertia(
robo, symo, j, antRj, antPj,
aje1, comp_inertia3, comp_ms, comp_mass
)
for j in xrange(1, robo.NL):
compute_diagonal_elements(
robo, symo, j, comp_inertia3, comp_ms,
comp_mass, forces, moments, inertia_a22
)
ka = j
while ka != 0:
k = ka
ka = robo.ant[ka]
compute_triangle_elements(
robo, symo, j, k, ka, antRj, antPj, aje1,
forces, moments, inertia_a12, inertia_a22
)
symo.mat_replace(inertia_a22, 'A', forced=True, symmet=True)
inertia_a11 = inertia_spatial(
comp_inertia3[0], comp_ms[0], comp_mass[0]
)
inertia_a11 = symo.mat_replace(
inertia_a11, 'Jcomp', 0, forced=True, symmet=True
)
# setup inertia_a12 in Matrix form
a12mat = sympy.zeros(6, robo.NL)
for j in xrange(1, robo.NL):
a12mat[:, j] = inertia_a12[j]
a12mat = a12mat[:, 1:]
# setup the complete inertia matrix
inertia = Matrix([
inertia_a11.row_join(a12mat),
a12mat.transpose().row_join(inertia_a22)
])
return inertia
def compute_rot_trans(robo, symo):
#init transformation
antRj = ParamsInit.init_mat(robo)
antPj = ParamsInit.init_vec(robo)
for j in xrange(robo.NL):
compute_transform(robo, symo, j, antRj, antPj)
return antRj, antPj
def compute_rot_trans(robo, symo):
#init transformation
antRj = ParamsInit.init_mat(robo)
antPj = ParamsInit.init_vec(robo)
for j in xrange(robo.NL):
compute_transform(robo, symo, j, antRj, antPj)
return antRj, antPj
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 = ParamsInit.init_jplus(robo)
AJE1 = ParamsInit.init_vec(robo)
f = ParamsInit.init_vec(robo, ext=1)
n = ParamsInit.init_vec(robo, ext=1)
A = sympy.zeros(robo.NL, robo.NL)
symo = symbolmgr.SymbolManager()
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 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 = ParamsInit.init_jplus(robo)
AJE1 = ParamsInit.init_vec(robo)
f = ParamsInit.init_vec(robo, ext=1)
n = ParamsInit.init_vec(robo, ext=1)
A = sympy.zeros(robo.NL, robo.NL)
symo = symbolmgr.SymbolManager()
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 fixed_inertia_matrix(robo, symo):
"""
Compute Inertia Matrix for robots with fixed base. This function
computes just the A22 matrix when the inertia matrix
A = [A11, A12; A12.transpose(), A22].
"""
# init terms
comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo)
aje1 = ParamsInit.init_vec(robo)
forces = ParamsInit.init_vec(robo, ext=1)
moments = ParamsInit.init_vec(robo, ext=1)
inertia_a12 = ParamsInit.init_vec(robo, num=6)
inertia_a22 = sympy.zeros(robo.nl, robo.nl)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
for j in reversed(xrange(1, robo.NL)):
replace_composite_terms(
symo, j, comp_inertia3, comp_ms, comp_mass
)
if j != 1:
compute_composite_inertia(
robo, symo, j, antRj, antPj,
aje1, comp_inertia3, comp_ms, comp_mass
)
for j in xrange(1, robo.NL):
compute_diagonal_elements(
robo, symo, j, comp_inertia3, comp_ms,
comp_mass, forces, moments, inertia_a22
)
ka = j
while ka != 1:
k = ka
ka = robo.ant[ka]
compute_triangle_elements(
robo, symo, j, k, ka, antRj, antPj, aje1,
forces, moments, inertia_a12, inertia_a22
)
symo.mat_replace(inertia_a22, 'A', forced=True, symmet=True)
return inertia_a22
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 = ParamsInit.init_vec(robo)
# antecedent angular velocity, projected into jth frame
w = ParamsInit.init_w(robo)
jaj = ParamsInit.init_vec(robo, 6)
jTant = ParamsInit.init_mat(robo,
6) # Twist transform list of Matrices 6x6
beta_star = ParamsInit.init_vec(robo, 6)
grandJ = ParamsInit.init_mat(robo, 6)
link_acc = ParamsInit.init_vec(robo, 6)
H_inv = ParamsInit.init_scalar(robo)
juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj
Tau = ParamsInit.init_scalar(robo)
grandVp = ParamsInit.init_vec(robo, 6)
grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0]))
symo = symbolmgr.SymbolManager()
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 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 = ParamsInit.init_vec(robo)
# antecedent angular velocity, projected into jth frame
w = ParamsInit.init_w(robo)
jaj = ParamsInit.init_vec(robo, 6)
jTant = ParamsInit.init_mat(robo, 6) # Twist transform list of Matrices 6x6
beta_star = ParamsInit.init_vec(robo, 6)
grandJ = ParamsInit.init_mat(robo, 6)
link_acc = ParamsInit.init_vec(robo, 6)
H_inv = ParamsInit.init_scalar(robo)
juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj
Tau = ParamsInit.init_scalar(robo)
grandVp = ParamsInit.init_vec(robo, 6)
grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0]))
symo = symbolmgr.SymbolManager()
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 composite_inverse_dynmodel(robo, symo):
"""
Compute the Inverse Dynamic Model using Composite link Newton-Euler
algorithm for tree structure robots with fixed and floating base.
Parameters:
robo: Robot - instance of robot description container
symo: symbolmgr.SymbolManager - instance of symbol manager
"""
# antecedent angular velocity, projected into jth frame
# j^omega_i
wi = ParamsInit.init_vec(robo)
# j^omega_j
w = ParamsInit.init_w(robo)
# j^a_j -- joint axis in screw form
jaj = ParamsInit.init_vec(robo, 6)
# Twist transform list of Matrices 6x6
grandJ = ParamsInit.init_mat(robo, 6)
jTant = ParamsInit.init_mat(robo, 6)
gamma = ParamsInit.init_vec(robo, 6)
beta = ParamsInit.init_vec(robo, 6)
zeta = ParamsInit.init_vec(robo, 6)
composite_inertia = ParamsInit.init_mat(robo, 6)
composite_beta = ParamsInit.init_vec(robo, 6)
comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo)
grandVp = ParamsInit.init_vec(robo, 6)
react_wrench = ParamsInit.init_vec(robo, 6)
torque = ParamsInit.init_scalar(robo)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
# first forward recursion
for j in xrange(1, robo.NL):
# compute spatial inertia matrix for use in backward recursion
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# set jaj vector
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])
# compute j^omega_j and j^omega_i
compute_omega(robo, symo, j, antRj, w, wi)
# compute j^S_i : screw transformation matrix
compute_screw_transform(robo, symo, j, antRj, antPj, jTant)
# first forward recursion (still)
for j in xrange(1, robo.NL):
# compute j^gamma_j : gyroscopic acceleration (6x1)
compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma)
# compute j^beta_j : external+coriolis+centrifugal wrench (6x1)
compute_beta(robo, symo, j, w, beta)
# compute j^zeta_j : relative acceleration (6x1)
compute_zeta(robo, symo, j, gamma, jaj, zeta)
# first backward recursion - initialisation step
for j in reversed(xrange(0, robo.NL)):
if j == 0:
# compute spatial inertia matrix for base
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# compute 0^beta_0
compute_beta(robo, symo, j, w, beta)
replace_composite_terms(
symo, grandJ, beta, j, composite_inertia, composite_beta
)
# second backward recursion - compute composite term
for j in reversed(xrange(0, robo.NL)):
replace_composite_terms(
symo, composite_inertia, composite_beta, j,
composite_inertia, composite_beta, replace=True
)
if j == 0:
continue
compute_composite_inertia(
robo, symo, j, antRj, antPj,
comp_inertia3, comp_ms, comp_mass, composite_inertia
)
compute_composite_beta(
robo, symo, j, jTant, zeta, composite_inertia, composite_beta
)
# compute base acceleration : this returns the correct value for
# fixed base and floating base robots
compute_base_accel_composite(
robo, symo, composite_inertia, composite_beta, grandVp
)
# second forward recursion
for j in xrange(1, robo.NL):
# compute j^Vdot_j : link acceleration
compute_link_accel(robo, symo, j, jTant, zeta, grandVp)
# compute j^F_j : reaction wrench
compute_reaction_wrench(
robo, symo, j, grandVp,
composite_inertia, composite_beta, react_wrench
)
# second forward recursion still - to make the output pretty
for j in xrange(1, robo.NL):
# compute torque
compute_torque(robo, symo, j, jaj, react_wrench, torque)
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 = ParamsInit.init_vec(robo)
Njnt = ParamsInit.init_vec(robo)
# init file output, writing the robot description
symo = symbolmgr.SymbolManager()
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 = ParamsInit.init_vec(robo)
N = ParamsInit.init_vec(robo)
for i in xrange(10):
if param_vec[i] == tools.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 composite_inverse_dynmodel(robo, symo):
"""
Compute the Inverse Dynamic Model using Composite link Newton-Euler
algorithm for tree structure robots with fixed and floating base.
Parameters:
robo: Robot - instance of robot description container
symo: symbolmgr.SymbolManager - instance of symbol manager
"""
# antecedent angular velocity, projected into jth frame
# j^omega_i
wi = ParamsInit.init_vec(robo)
# j^omega_j
w = ParamsInit.init_w(robo)
# j^a_j -- joint axis in screw form
jaj = ParamsInit.init_vec(robo, 6)
# Twist transform list of Matrices 6x6
grandJ = ParamsInit.init_mat(robo, 6)
jTant = ParamsInit.init_mat(robo, 6)
gamma = ParamsInit.init_vec(robo, 6)
beta = ParamsInit.init_vec(robo, 6)
zeta = ParamsInit.init_vec(robo, 6)
composite_inertia = ParamsInit.init_mat(robo, 6)
composite_beta = ParamsInit.init_vec(robo, 6)
comp_inertia3, comp_ms, comp_mass = ParamsInit.init_jplus(robo)
grandVp = ParamsInit.init_vec(robo, 6)
react_wrench = ParamsInit.init_vec(robo, 6)
torque = ParamsInit.init_scalar(robo)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
# first forward recursion
for j in xrange(1, robo.NL):
# compute spatial inertia matrix for use in backward recursion
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# set jaj vector
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])
# compute j^omega_j and j^omega_i
compute_omega(robo, symo, j, antRj, w, wi)
# compute j^S_i : screw transformation matrix
compute_screw_transform(robo, symo, j, antRj, antPj, jTant)
# first forward recursion (still)
for j in xrange(1, robo.NL):
# compute j^gamma_j : gyroscopic acceleration (6x1)
compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma)
# compute j^beta_j : external+coriolis+centrifugal wrench (6x1)
compute_beta(robo, symo, j, w, beta)
# compute j^zeta_j : relative acceleration (6x1)
compute_zeta(robo, symo, j, gamma, jaj, zeta)
# first backward recursion - initialisation step
for j in reversed(xrange(0, robo.NL)):
if j == 0:
# compute spatial inertia matrix for base
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# compute 0^beta_0
compute_beta(robo, symo, j, w, beta)
replace_composite_terms(symo, grandJ, beta, j, composite_inertia,
composite_beta)
# second backward recursion - compute composite term
for j in reversed(xrange(0, robo.NL)):
replace_composite_terms(symo,
composite_inertia,
composite_beta,
j,
composite_inertia,
composite_beta,
replace=True)
if j == 0:
continue
compute_composite_inertia(robo, symo, j, antRj, antPj, comp_inertia3,
comp_ms, comp_mass, composite_inertia)
compute_composite_beta(robo, symo, j, jTant, zeta, composite_inertia,
composite_beta)
# compute base acceleration : this returns the correct value for
# fixed base and floating base robots
compute_base_accel_composite(robo, symo, composite_inertia, composite_beta,
grandVp)
# second forward recursion
for j in xrange(1, robo.NL):
# compute j^Vdot_j : link acceleration
compute_link_accel(robo, symo, j, jTant, zeta, grandVp)
# compute j^F_j : reaction wrench
compute_reaction_wrench(robo, symo, j, grandVp, composite_inertia,
composite_beta, react_wrench)
# second forward recursion still - to make the output pretty
for j in xrange(1, robo.NL):
# compute torque
compute_torque(robo, symo, j, jaj, react_wrench, torque)
def direct_dynmodel(robo, symo):
"""
Compute the Direct Dynamic Model using Newton-Euler algorithm for
robots with floating and fixed base.
Parameters:
robo: Robot - instance of robot description container
symo: symbolmgr.SymbolManager - instance of symbol manager
"""
# antecedent angular velocity, projected into jth frame
# j^omega_i
wi = ParamsInit.init_vec(robo)
# j^omega_j
w = ParamsInit.init_w(robo)
# j^a_j -- joint axis in screw form
jaj = ParamsInit.init_vec(robo, 6)
# Twist transform list of Matrices 6x6
grandJ = ParamsInit.init_mat(robo, 6)
jTant = ParamsInit.init_mat(robo, 6)
gamma = ParamsInit.init_vec(robo, 6)
beta = ParamsInit.init_vec(robo, 6)
zeta = ParamsInit.init_vec(robo, 6)
h_inv = ParamsInit.init_scalar(robo)
jah = ParamsInit.init_vec(robo, 6) # Jj*aj*Hinv_j
tau = ParamsInit.init_scalar(robo)
star_inertia = ParamsInit.init_mat(robo, 6)
star_beta = ParamsInit.init_vec(robo, 6)
qddot = ParamsInit.init_scalar(robo)
grandVp = ParamsInit.init_vec(robo, 6)
react_wrench = ParamsInit.init_vec(robo, 6)
torque = ParamsInit.init_scalar(robo)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
# first forward recursion
for j in xrange(1, robo.NL):
# compute spatial inertia matrix for use in backward recursion
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# set jaj vector
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])
# compute j^omega_j and j^omega_i
compute_omega(robo, symo, j, antRj, w, wi)
# compute j^S_i : screw transformation matrix
compute_screw_transform(robo, symo, j, antRj, antPj, jTant)
# compute j^gamma_j : gyroscopic acceleration (6x1)
compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma)
# compute j^beta_j : external+coriolis+centrifugal wrench (6x1)
compute_beta(robo, symo, j, w, beta)
# decide first link
first_link = 0 if robo.is_floating else 1
# first backward recursion - initialisation step
for j in reversed(xrange(first_link, robo.NL)):
if j == first_link and robo.is_floating:
# compute spatial inertia matrix for base
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# compute 0^beta_0
compute_beta(robo, symo, j, w, beta)
replace_star_terms(symo, grandJ, beta, j, star_inertia, star_beta)
# second backward recursion - compute star terms
for j in reversed(xrange(first_link, robo.NL)):
replace_star_terms(symo,
star_inertia,
star_beta,
j,
star_inertia,
star_beta,
replace=True)
if j == 0:
continue
compute_tau(robo, symo, j, jaj, star_beta, tau)
compute_star_terms(robo, symo, j, jaj, jTant, gamma, tau, h_inv, jah,
star_inertia, star_beta)
if j == first_link:
continue
# compute base acceleration : this returns the correct value for
# fixed base and floating base robots
compute_base_accel(robo, symo, star_inertia, star_beta, grandVp)
# second forward recursion
for j in xrange(1, robo.NL):
# compute qddot_j : joint acceleration
compute_joint_accel(robo, symo, j, jaj, jTant, h_inv, jah, gamma, tau,
grandVp, star_beta, star_inertia, qddot)
# compute j^zeta_j : relative acceleration (6x1)
compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot)
# compute j^Vdot_j : link acceleration
compute_link_accel(robo, symo, j, jTant, zeta, grandVp)
# compute j^F_j : reaction wrench
compute_reaction_wrench(robo, symo, j, grandVp, star_inertia,
star_beta, react_wrench)
def direct_dynmodel(robo, symo):
"""
Compute the Direct Dynamic Model using Newton-Euler algorithm for
robots with floating and fixed base.
Parameters:
robo: Robot - instance of robot description container
symo: symbolmgr.SymbolManager - instance of symbol manager
"""
# antecedent angular velocity, projected into jth frame
# j^omega_i
wi = ParamsInit.init_vec(robo)
# j^omega_j
w = ParamsInit.init_w(robo)
# j^a_j -- joint axis in screw form
jaj = ParamsInit.init_vec(robo, 6)
# Twist transform list of Matrices 6x6
grandJ = ParamsInit.init_mat(robo, 6)
jTant = ParamsInit.init_mat(robo, 6)
gamma = ParamsInit.init_vec(robo, 6)
beta = ParamsInit.init_vec(robo, 6)
zeta = ParamsInit.init_vec(robo, 6)
h_inv = ParamsInit.init_scalar(robo)
jah = ParamsInit.init_vec(robo, 6) # Jj*aj*Hinv_j
tau = ParamsInit.init_scalar(robo)
star_inertia = ParamsInit.init_mat(robo, 6)
star_beta = ParamsInit.init_vec(robo, 6)
qddot = ParamsInit.init_scalar(robo)
grandVp = ParamsInit.init_vec(robo, 6)
react_wrench = ParamsInit.init_vec(robo, 6)
torque = ParamsInit.init_scalar(robo)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
# first forward recursion
for j in xrange(1, robo.NL):
# compute spatial inertia matrix for use in backward recursion
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# set jaj vector
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])
# compute j^omega_j and j^omega_i
compute_omega(robo, symo, j, antRj, w, wi)
# compute j^S_i : screw transformation matrix
compute_screw_transform(robo, symo, j, antRj, antPj, jTant)
# compute j^gamma_j : gyroscopic acceleration (6x1)
compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma)
# compute j^beta_j : external+coriolis+centrifugal wrench (6x1)
compute_beta(robo, symo, j, w, beta)
# decide first link
first_link = 0 if robo.is_floating else 1
# first backward recursion - initialisation step
for j in reversed(xrange(first_link, robo.NL)):
if j == first_link and robo.is_floating:
# compute spatial inertia matrix for base
grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j])
# compute 0^beta_0
compute_beta(robo, symo, j, w, beta)
replace_star_terms(
symo, grandJ, beta, j, star_inertia, star_beta
)
# second backward recursion - compute star terms
for j in reversed(xrange(first_link, robo.NL)):
replace_star_terms(
symo, star_inertia, star_beta, j,
star_inertia, star_beta, replace=True
)
if j == 0:
continue
compute_tau(robo, symo, j, jaj, star_beta, tau)
compute_star_terms(
robo, symo, j, jaj, jTant, gamma, tau,
h_inv, jah, star_inertia, star_beta
)
if j == first_link:
continue
# compute base acceleration : this returns the correct value for
# fixed base and floating base robots
compute_base_accel(
robo, symo, star_inertia, star_beta, grandVp
)
# second forward recursion
for j in xrange(1, robo.NL):
# compute qddot_j : joint acceleration
compute_joint_accel(
robo, symo, j, jaj, jTant, h_inv, jah, gamma,
tau, grandVp, star_beta, star_inertia, qddot
)
# compute j^zeta_j : relative acceleration (6x1)
compute_zeta(robo, symo, j, gamma, jaj, zeta, qddot)
# compute j^Vdot_j : link acceleration
compute_link_accel(robo, symo, j, jTant, zeta, grandVp)
# compute j^F_j : reaction wrench
compute_reaction_wrench(
robo, symo, j, grandVp,
star_inertia, star_beta, react_wrench
)
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 = ParamsInit.init_vec(robo)
Njnt = ParamsInit.init_vec(robo)
# init file output, writing the robot description
symo = symbolmgr.SymbolManager()
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 = ParamsInit.init_vec(robo)
N = ParamsInit.init_vec(robo)
for i in xrange(10):
if param_vec[i] == tools.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 dynamic_identification_model(robo, symo):
"""
Compute the Dynamic Identification model of a robot using
Newton-Euler algorithm.
"""
# init forces vectors
Fjnt = ParamsInit.init_vec(robo)
Njnt = ParamsInit.init_vec(robo)
# init transformation
antRj, antPj = compute_rot_trans(robo, symo)
# init velocities and accelerations
w, wdot, vdot, U = compute_vel_acc(
robo, symo, antRj, antPj, floating=True
)
# virtual robot with only one non-zero parameter at once
robo_tmp = copy.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)
# start link number
is_fixed = False if robo.is_floating or robo.is_mobile else True
start_link = 0
for k in xrange(start_link, robo.NL):
param_vec = robo.get_inert_param(k)
F = ParamsInit.init_vec(robo)
N = ParamsInit.init_vec(robo)
for i in xrange(10):
if param_vec[i] == tools.ZERO:
continue
# change link names according to current non-zero parameter
name = '{index}{element}' + str(param_vec[i])
# 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_dynamic_wrench(
robo_tmp, symo, name, k, w, wdot, U, vdot, F, N
)
# init external forces
Fex = ParamsInit.init_vec(robo)
Nex = ParamsInit.init_vec(robo)
for j in reversed(xrange(1, k + 1)):
_compute_reaction_wrench(
robo_tmp, symo, name, j, antRj, antPj,
vdot, F, N, Fjnt, Njnt, Fex, Nex
)
# reaction wrench for base
_compute_base_reaction_wrench(
robo_tmp, symo, name, antRj,antPj,
vdot, F, N, Fex, Nex, Fjnt, Njnt
)
for j in xrange(1, k + 1):
_compute_joint_torque(robo_tmp, symo, name, j, Fjnt, Njnt)
# reset all the parameters to zero
robo_tmp.put_inert_param(sympy.zeros(10, 1), k)
# compute model for the joint parameters
# avoid these parameters for link 0
if k == 0: continue
_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
)
return symo