def _compute_gyroscopic_acceleration(model, robo, j, i):
"""
Compute the gyroscopic acceleration of link j.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecendent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_gamma_j = Screw()
# local variables
j_rot_i = robo.geos[j].tmat.inv_rot
i_trans_j = robo.geos[j].tmat.trans
i_omega_i = model.vels[i].ang
sigma_j = robo.geos[j].sigma
sigma_dash_j = 1 - sigma_j
j_z_j = robo.geos[j].zunit
qdot_j = robo.qdots[j]
# actual computation
j_omega_i = j_rot_i * i_omega_i
# term1 = i_omega_i x (i_omega_i x i_trans_j)
term1 = skew(i_omega_i) * (skew(i_omega_i) * i_trans_j)
# term2 = j_omega_i x (qdot_j * j_z_j)
term2 = skew(j_omega_i) * (qdot_j * j_z_j)
j_gamma_j.lin = (j_rot_i * term1) + (2 * sigma_j * term2)
j_gamma_j.ang = sigma_dash_j * term2
# store computed acceleration in model
model.gammas[j] = j_gamma_j
return model
def _compute_beta_wrench(model, robo, j):
"""
Compute the wrench for link j which combines the external forces,
Coriolis forces and centrifugal forces.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
Returns:
An instance of DynModel that contains all the new values.
"""
j_beta_j = Screw()
# local variables
j_omega_j = model.vels[j].ang
j_fe_j = robo.dyns[j].wrench.val
j_ms_j = robo.dyns[j].mass_tensor
j_inertia_j = robo.dyns[j].inertia
# actual computation
# lin_term = j_omega_j x (j_omega_j x j_ms_j)
lin_term = skew(j_omega_j) * (skew(j_omega_j) * j_ms_j)
# ang_term = j_omega_j x (j_inertia_j * j_omega_j)
ang_term = skew(j_omega_j) * (j_inertia_j * j_omega_j)
term = Screw(lin=lin_term, ang=ang_term)
j_beta_j.val = - j_fe_j - term.val
# store computed wrench in model
model.betas[j] = j_beta_j
return model
def compute_triangle_elements(
robo, symo, j, k, ka, antRj, antPj, aje1,
forces, moments, inertia_a12, inertia_a22
):
"""
Compute elements below and above diagonal of the inertia matrix
(internal function).
Note:
forces, moments, inertia_a12, inertia_a22 are the output
parameters
"""
forces[ka] = antRj[k] * forces[k]
if k == j and robo.sigma[j] == 0:
moments[ka] = aje1[k] + \
(tools.skew(antPj[k]) * antRj[k] * forces[k])
else:
moments[ka] = (antRj[k] * moments[k]) + \
(tools.skew(antPj[k]) * antRj[k] * forces[k])
if ka == 0:
inertia_a12[j][:3, 0] = symo.mat_replace(
forces[ka], 'AV0', j, forced=True
)
inertia_a12[j][3:, 0] = symo.mat_replace(
moments[ka], 'AW0', j, forced=True
)
else:
symo.mat_replace(forces[ka], 'E' + CHARSYMS[j], ka)
symo.mat_replace(moments[ka], 'N' + CHARSYMS[j], ka)
if robo.sigma[ka] == 0:
inertia_a22[j-1, ka-1] = moments[ka][2]
elif robo.sigma[ka] == 1:
inertia_a22[j-1, ka-1] = forces[ka][2]
inertia_a22[ka-1, j-1] = inertia_a22[j-1, ka-1]
def _compute_beta_wrench(model, robo, j):
"""
Compute the wrench for link j which combines the external forces,
Coriolis forces and centrifugal forces.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
Returns:
An instance of DynModel that contains all the new values.
"""
j_beta_j = Screw()
# local variables
j_omega_j = model.vels[j].ang
j_fe_j = robo.dyns[j].wrench.val
j_ms_j = robo.dyns[j].mass_tensor
j_inertia_j = robo.dyns[j].inertia
# actual computation
# lin_term = j_omega_j x (j_omega_j x j_ms_j)
lin_term = skew(j_omega_j) * (skew(j_omega_j) * j_ms_j)
# ang_term = j_omega_j x (j_inertia_j * j_omega_j)
ang_term = skew(j_omega_j) * (j_inertia_j * j_omega_j)
term = Screw(lin=lin_term, ang=ang_term)
j_beta_j.val = -j_fe_j - term.val
# store computed wrench in model
model.betas[j] = j_beta_j
return model
def _compute_gyroscopic_acceleration(model, robo, j, i):
"""
Compute the gyroscopic acceleration of link j.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecendent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_gamma_j = Screw()
# local variables
j_rot_i = robo.geos[j].tmat.inv_rot
i_trans_j = robo.geos[j].tmat.trans
i_omega_i = model.vels[i].ang
sigma_j = robo.geos[j].sigma
sigma_dash_j = 1 - sigma_j
j_z_j = robo.geos[j].zunit
qdot_j = robo.qdots[j]
# actual computation
j_omega_i = j_rot_i * i_omega_i
# term1 = i_omega_i x (i_omega_i x i_trans_j)
term1 = skew(i_omega_i) * (skew(i_omega_i) * i_trans_j)
# term2 = j_omega_i x (qdot_j * j_z_j)
term2 = skew(j_omega_i) * (qdot_j * j_z_j)
j_gamma_j.lin = (j_rot_i * term1) + (2 * sigma_j * term2)
j_gamma_j.ang = sigma_dash_j * term2
# store computed acceleration in model
model.gammas[j] = j_gamma_j
return model
def compute_composite_inertia(
robo, symo, j, antRj, antPj,
aje1, comp_inertia3, comp_ms, comp_mass
):
"""
Compute composite inertia (internal function).
Note:
aje1, comp_inertia3, comp_ms, comp_mass are the output
parameters.
"""
i = robo.ant[j]
i_ms_j_c = antRj[j] * comp_ms[j]
i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j)
expr1 = antRj[j] * comp_inertia3[j]
expr1 = symo.mat_replace(expr1, 'AJ', j)
aje1[j] = expr1[:, 2]
expr2 = expr1 * antRj[j].transpose()
expr2 = symo.mat_replace(expr2, 'AJA', j)
expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c)
expr3 = symo.mat_replace(expr3, 'PAS', j)
comp_inertia3[i] += expr2 - (expr3 + expr3.transpose()) + \
(comp_mass[j] * tools.skew(antPj[j]) * \
tools.skew(antPj[j]).transpose())
comp_ms[i] = comp_ms[i] + i_ms_j_c + (antPj[j] * comp_mass[j])
comp_mass[i] = comp_mass[i] + comp_mass[j]
def inertia_spatial(inertia, ms_tensor, mass):
"""
Setup spatial inertia matrix (internal function).
"""
return Matrix([
(mass * sympy.eye(3)).row_join(tools.skew(ms_tensor).transpose()),
tools.skew(ms_tensor).row_join(inertia)
])
def init_u(cls, robo):
"""Generates a list of auxiliary U matrices"""
u_aux_matrix = ParamsInit.init_mat(robo)
# the value for the -1th base frame
u_aux_matrix.append(
tools.skew(robo.w0)**2 + tools.skew(robo.wdot0)
)
return u_aux_matrix
def init_u(cls, robo):
"""Generates a list of auxiliary U matrices"""
u_aux_matrix = ParamsInit.init_mat(robo)
# the value for the -1th base frame
u_aux_matrix.append(
tools.skew(robo.w0)**2 + tools.skew(robo.wdot0)
)
return u_aux_matrix
def vec_mut_M(P):
"""Internal function. Needed for inertia parameters transformation
Parameters
==========
P : Matrix 3x1
position vector
Returns : Matrix 6x1
"""
U = -tools.skew(P) * tools.skew(P)
return Matrix([U[0, 0], U[0, 1], U[0, 2], U[1, 1], U[1, 2], U[2, 2]])
def vec_mut_M(P):
"""Internal function. Needed for inertia parameters transformation
Parameters
==========
P : Matrix 3x1
position vector
Returns : Matrix 6x1
"""
U = -tools.skew(P)*tools.skew(P)
return Matrix([U[0, 0], U[0, 1], U[0, 2], U[1, 1], U[1, 2], U[2, 2]])
def _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj):
DV = ParamsInit.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 + tools.skew(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 * tools.skew(wi) * qdj
return vdot[j]
def compute_beta(robo, symo, j, w, beta_star):
"""Internal function. Computes link's wrench when
the joint accelerations are zero
Notes
=====
beta_star is the output parameter
"""
E1 = symo.mat_replace(robo.J[j]*w[j], 'JW', j)
E2 = symo.mat_replace(tools.skew(w[j])*E1, 'KW', j)
E3 = tools.skew(w[j])*robo.MS[j]
E4 = symo.mat_replace(tools.skew(w[j])*E3, 'SW', j)
E5 = -robo.Nex[j] - E2
E6 = -robo.Fex[j] - E4
beta_star[j] = Matrix([E6, E5])
def vec_mut_MS(v, P):
"""Internal function. Needed for inertia parameters transformation
Parameters
==========
v : Matrix 3x1
axis vector
P : Matrix 3x1
position vector
Returns : Matrix 6x1
"""
U = - tools.skew(v)*tools.skew(P)
return Matrix([2*U[0, 0], U[0, 1] + U[1, 0], U[0, 2] + U[2, 0],
2*U[1, 1], U[1, 2] + U[2, 1], 2*U[2, 2]])
def compute_beta(robo, symo, j, w, beta_star):
"""Internal function. Computes link's wrench when
the joint accelerations are zero
Notes
=====
beta_star is the output parameter
"""
E1 = symo.mat_replace(robo.J[j] * w[j], 'JW', j)
E2 = symo.mat_replace(tools.skew(w[j]) * E1, 'KW', j)
E3 = tools.skew(w[j]) * robo.MS[j]
E4 = symo.mat_replace(tools.skew(w[j]) * E3, 'SW', j)
E5 = -robo.Nex[j] - E2
E6 = -robo.Fex[j] - E4
beta_star[j] = Matrix([E6, E5])
def _v_dot_j(
robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj
):
DV = ParamsInit.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 + tools.skew(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*tools.skew(wi)*qdj
return vdot[j]
def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N,
Fex, Nex):
"""Internal function. Computes wrench (torques and forces)
of the joint j
Notes
=====
Fjnt, Njnt, Fex, Nex are the output parameters
"""
Fjnt[j] = symo.mat_replace(F[j] + Fex[j], 'E', j)
Njnt[j] = N[j] + Nex[j] + tools.skew(robo.MS[j]) * vdot[j]
symo.mat_replace(Njnt[j], 'N', j)
f_ant = symo.mat_replace(antRj[j] * Fjnt[j], 'FDI', j)
if robo.ant[j] != -1:
Fex[robo.ant[j]] += f_ant
Nex[robo.ant[j]] += antRj[j] * Njnt[j] + tools.skew(antPj[j]) * f_ant
def compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1):
"""Internal function. Computes inertia parameters of composed link
Notes
=====
Jplus, MSplus, Mplus are the output parameters
"""
hat_antPj = tools.skew(antPj[j])
antMSj = symo.mat_replace(antRj[j] * MSplus[j], 'AS', j)
E1 = symo.mat_replace(antRj[j] * Jplus[j], 'AJ', j)
AJE1[j] = E1[:, 2]
E2 = symo.mat_replace(E1 * antRj[j].T, 'AJA', j)
E3 = symo.mat_replace(hat_antPj * tools.skew(antMSj), 'PAS', j)
Jplus[robo.ant[j]] += E2 - (E3 + E3.T) + hat_antPj * hat_antPj.T * Mplus[j]
MSplus[robo.ant[j]] += antMSj + antPj[j] * Mplus[j]
Mplus[robo.ant[j]] += Mplus[j]
def compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1):
"""Internal function. Computes inertia parameters of composed link
Notes
=====
Jplus, MSplus, Mplus are the output parameters
"""
hat_antPj = tools.skew(antPj[j])
antMSj = symo.mat_replace(antRj[j]*MSplus[j], 'AS', j)
E1 = symo.mat_replace(antRj[j]*Jplus[j], 'AJ', j)
AJE1[j] = E1[:, 2]
E2 = symo.mat_replace(E1*antRj[j].T, 'AJA', j)
E3 = symo.mat_replace(hat_antPj*tools.skew(antMSj), 'PAS', j)
Jplus[robo.ant[j]] += E2 - (E3 + E3.T) + hat_antPj*hat_antPj.T*Mplus[j]
MSplus[robo.ant[j]] += antMSj + antPj[j]*Mplus[j]
Mplus[robo.ant[j]] += Mplus[j]
def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot,
Fjnt, Njnt, F, N, Fex, Nex):
"""Internal function. Computes wrench (torques and forces)
of the joint j
Notes
=====
Fjnt, Njnt, Fex, Nex are the output parameters
"""
Fjnt[j] = symo.mat_replace(F[j] + Fex[j], 'E', j)
Njnt[j] = N[j] + Nex[j] + tools.skew(robo.MS[j])*vdot[j]
symo.mat_replace(Njnt[j], 'N', j)
f_ant = symo.mat_replace(antRj[j]*Fjnt[j], 'FDI', j)
if robo.ant[j] != - 1:
Fex[robo.ant[j]] += f_ant
Nex[robo.ant[j]] += antRj[j]*Njnt[j] + tools.skew(antPj[j])*f_ant
def setUp(self):
link = 33
# setup an instance of DynParams
self.data = DynParams(link)
# setup data to compare against
self.link = link
# inertia matrix terms
self.xx = var('XX33')
self.xy = var('XY33')
self.xz = var('XZ33')
self.yy = var('YY33')
self.yz = var('YZ33')
self.zz = var('ZZ33')
# mass tensor terms
self.msx = var('MX33')
self.msy = var('MY33')
self.msz = var('MZ33')
# link mass
self.mass = var('M33')
# rotor inertia term
self.ia = var('IA33')
# coulomb friction parameter
self.frc = var('FS33')
# viscous friction parameter
self.frv = var('FV33')
# external forces and moments
self.fx_ext = var('FX33')
self.fy_ext = var('FY33')
self.fz_ext = var('FZ33')
self.mx_ext = var('CX33')
self.my_ext = var('CY33')
self.mz_ext = var('CZ33')
# setup matrix terms to compare against
self.inertia = Matrix([
[self.xx, self.xy, self.xz],
[self.xy, self.yy, self.yz],
[self.xz, self.yz, self.zz]
])
self.mass_tensor = Matrix([self.msx, self.msy, self.msz])
m_eye = self.mass * eye(3)
ms_skew = tools.skew(self.mass_tensor)
self.spatial_inertia = screw6.Screw6(
tl=m_eye, tr=ms_skew.transpose(),
bl=ms_skew, br=self.inertia
)
self.wrench = screw.Screw(
lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]),
ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext])
)
# setup params to compare against
self.param_xx = var('XX20')
self.param_yy = var('YY20')
self.param_zz = var('ZZ20')
self.params = {
'xx': self.param_xx,
'yy': self.param_yy,
'zz': self.param_zz
}
self.wrong_param = {'rand': 'some-value'}
def spatial_inertia(self):
"""Get spatial inertia (6x6) matrix."""
m_eye = self.mass * eye(3)
ms_skew = tools.skew(self.mass_tensor)
return Screw6(
tl=m_eye, tr=ms_skew.transpose(),
bl=ms_skew, br=self.inertia
)
def vec_mut_MS(v, P):
"""Internal function. Needed for inertia parameters transformation
Parameters
==========
v : Matrix 3x1
axis vector
P : Matrix 3x1
position vector
Returns : Matrix 6x1
"""
U = -tools.skew(v) * tools.skew(P)
return Matrix([
2 * U[0, 0], U[0, 1] + U[1, 0], U[0, 2] + U[2, 0], 2 * U[1, 1],
U[1, 2] + U[2, 1], 2 * U[2, 2]
])
def _compute_smat(self):
"""
Compute the transformation matrix in Screw (6x6) matrix form.
"""
self._smat = Screw6(
tl=self.rot, tr=tools.skew(self.trans),
bl=sympy.zeros(3, 3), br=self.rot
)
def spatial_inertia(self):
"""Get spatial inertia (6x6) matrix."""
m_eye = self.mass * eye(3)
ms_skew = tools.skew(self.mass_tensor)
return Screw6(tl=m_eye,
tr=ms_skew.transpose(),
bl=ms_skew,
br=self.inertia)
def _compute_smat(self):
"""
Compute the transformation matrix in Screw (6x6) matrix form.
"""
self._smat = Screw6(tl=self.rot,
tr=tools.skew(self.trans),
bl=sympy.zeros(3, 3),
br=self.rot)
def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi):
"""Internal function. Computes link's accelerations when
the joint accelerations are zero
Notes
=====
link_acc is the output parameter
"""
E1 = symo.mat_replace(tools.skew(wi[j])*Matrix([0, 0, robo.qdot[j]]),
'WQ', j)
E2 = (1 - robo.sigma[j])*E1
E3 = 2*robo.sigma[j]*E1
E4 = tools.skew(w[robo.ant[j]])*antPj[j]
E5 = tools.skew(w[robo.ant[j]])*E4
E6 = antRj[j].T*E5
E7 = symo.mat_replace(E6 + E3, 'LW', j)
link_acc[j] = Matrix([E7, E2])
def compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi):
"""Internal function. Computes link's accelerations when
the joint accelerations are zero
Notes
=====
link_acc is the output parameter
"""
E1 = symo.mat_replace(
tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]]), 'WQ', j)
E2 = (1 - robo.sigma[j]) * E1
E3 = 2 * robo.sigma[j] * E1
E4 = tools.skew(w[robo.ant[j]]) * antPj[j]
E5 = tools.skew(w[robo.ant[j]]) * E4
E6 = antRj[j].T * E5
E7 = symo.mat_replace(E6 + E3, 'LW', j)
link_acc[j] = Matrix([E7, E2])
def _compute_sinv(self):
"""
Compute the inverse transformation matrix in Screw
(6x6) matrix form.
"""
self._sinv = Screw6(tl=self.inv_rot,
tr=-(self.inv_rot * tools.skew(self.trans)),
bl=sympy.zeros(3, 3),
br=self.inv_rot)
def _compute_sinv(self):
"""
Compute the inverse transformation matrix in Screw
(6x6) matrix form.
"""
self._sinv = Screw6(
tl=self.inv_rot, tr=-(self.inv_rot * tools.skew(self.trans)),
bl=sympy.zeros(3, 3), br=self.inv_rot
)
def setUp(self):
link = 33
# setup an instance of DynParams
self.data = DynParams(link)
# setup data to compare against
self.link = link
# inertia matrix terms
self.xx = var('XX33')
self.xy = var('XY33')
self.xz = var('XZ33')
self.yy = var('YY33')
self.yz = var('YZ33')
self.zz = var('ZZ33')
# mass tensor terms
self.msx = var('MX33')
self.msy = var('MY33')
self.msz = var('MZ33')
# link mass
self.mass = var('M33')
# rotor inertia term
self.ia = var('IA33')
# coulomb friction parameter
self.frc = var('FS33')
# viscous friction parameter
self.frv = var('FV33')
# external forces and moments
self.fx_ext = var('FX33')
self.fy_ext = var('FY33')
self.fz_ext = var('FZ33')
self.mx_ext = var('CX33')
self.my_ext = var('CY33')
self.mz_ext = var('CZ33')
# setup matrix terms to compare against
self.inertia = Matrix([[self.xx, self.xy, self.xz],
[self.xy, self.yy, self.yz],
[self.xz, self.yz, self.zz]])
self.mass_tensor = Matrix([self.msx, self.msy, self.msz])
m_eye = self.mass * eye(3)
ms_skew = tools.skew(self.mass_tensor)
self.spatial_inertia = screw6.Screw6(tl=m_eye,
tr=ms_skew.transpose(),
bl=ms_skew,
br=self.inertia)
self.wrench = screw.Screw(
lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]),
ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext]))
# setup params to compare against
self.param_xx = var('XX20')
self.param_yy = var('YY20')
self.param_zz = var('ZZ20')
self.params = {
'xx': self.param_xx,
'yy': self.param_yy,
'zz': self.param_zz
}
self.wrong_param = {'rand': 'some-value'}
def compute_beta(robo, symo, j, w, beta):
"""
Compute beta wrench which is a combination of coriolis forces,
centrifugal forces and external forces (internal function).
Note:
beta is the output parameter
"""
expr1 = robo.J[j] * w[j]
expr1 = symo.mat_replace(expr1, 'JW', j)
expr2 = tools.skew(w[j]) * expr1
expr2 = symo.mat_replace(expr2, 'KW', j)
expr3 = tools.skew(w[j]) * robo.MS[j]
expr4 = tools.skew(w[j]) * expr3
expr4 = symo.mat_replace(expr4, 'SW', j)
expr5 = -robo.Nex[j] - expr2
expr6 = -robo.Fex[j] - expr4
beta[j] = Matrix([expr6, expr5])
beta[j] = symo.mat_replace(beta[j], 'BETA', j)
def compute_beta(robo, symo, j, w, beta):
"""
Compute beta wrench which is a combination of coriolis forces,
centrifugal forces and external forces (internal function).
Note:
beta is the output parameter
"""
expr1 = robo.J[j] * w[j]
expr1 = symo.mat_replace(expr1, 'JW', j)
expr2 = tools.skew(w[j]) * expr1
expr2 = symo.mat_replace(expr2, 'KW', j)
expr3 = tools.skew(w[j]) * robo.MS[j]
expr4 = tools.skew(w[j]) * expr3
expr4 = symo.mat_replace(expr4, 'SW', j)
expr5 = -robo.Nex[j] - expr2
expr6 = -robo.Fex[j] - expr4
beta[j] = Matrix([expr6, expr5])
beta[j] = symo.mat_replace(beta[j], 'BETA', j)
def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma):
"""
Compute gyroscopic acceleration (internal function).
Note:
gamma is the output parameter
"""
i = robo.ant[j]
expr1 = tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]])
expr1 = symo.mat_replace(expr1, 'WQ', j)
expr2 = (1 - robo.sigma[j]) * expr1
expr3 = 2 * robo.sigma[j] * expr1
expr4 = tools.skew(w[i]) * antPj[j]
expr5 = tools.skew(w[i]) * expr4
expr6 = antRj[j].transpose() * expr5
expr7 = expr6 + expr3
expr7 = symo.mat_replace(expr7, 'LW', j)
gamma[j] = Matrix([expr7, expr2])
gamma[j] = symo.mat_replace(gamma[j], 'GYACC', j)
def compute_gamma(robo, symo, j, antRj, antPj, w, wi, gamma):
"""
Compute gyroscopic acceleration (internal function).
Note:
gamma is the output parameter
"""
i = robo.ant[j]
expr1 = tools.skew(wi[j]) * Matrix([0, 0, robo.qdot[j]])
expr1 = symo.mat_replace(expr1, 'WQ', j)
expr2 = (1 - robo.sigma[j]) * expr1
expr3 = 2 * robo.sigma[j] * expr1
expr4 = tools.skew(w[i]) * antPj[j]
expr5 = tools.skew(w[i]) * expr4
expr6 = antRj[j].transpose() * expr5
expr7 = expr6 + expr3
expr7 = symo.mat_replace(expr7, 'LW', j)
gamma[j] = Matrix([expr7, expr2])
gamma[j] = symo.mat_replace(gamma[j], 'GYACC', j)
def compute_composite_inertia(
robo, symo, j, antRj, antPj,
comp_inertia3, comp_ms, comp_mass, composite_inertia
):
"""
Compute composite inertia (internal function).
Note:
comp_inertia3, comp_ms, comp_mass, composite_inertia are the
output parameters.
"""
i = robo.ant[j]
# update inertia3, ms, mass from inertia in order to have the
# intermediate variables
comp_inertia3[i] = composite_inertia[i][3:, 3:]
comp_ms[i] = tools.skew2vec(composite_inertia[i][3:, 0:3])
comp_mass[i] = composite_inertia[i][0, 0]
comp_inertia3[j] = composite_inertia[j][3:, 3:]
comp_ms[j] = tools.skew2vec(composite_inertia[j][3:, 0:3])
comp_mass[j] = composite_inertia[j][0, 0]
# actual computation
i_ms_j_c = antRj[j] * comp_ms[j]
i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j)
expr1 = antRj[j] * comp_inertia3[j]
expr1 = symo.mat_replace(expr1, 'AJ', j)
expr2 = expr1 * antRj[j].transpose()
expr2 = symo.mat_replace(expr2, 'AJA', j)
expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c)
expr3 = symo.mat_replace(expr3, 'PAS', j)
i_comp_inertia3_j = expr2 - (expr3 + expr3.transpose()) + \
(comp_mass[j] * tools.skew(antPj[j]) * \
tools.skew(antPj[j]).transpose())
i_comp_inertia3_j = symo.mat_replace(i_comp_inertia3_j, 'JJI', j)
comp_inertia3[i] = comp_inertia3[i] + i_comp_inertia3_j
i_comp_ms_j = i_ms_j_c + (antPj[j] * comp_mass[j])
i_comp_ms_j = symo.mat_replace(i_comp_ms_j, 'MSJI', j)
comp_ms[i] = comp_ms[i] + i_comp_ms_j
i_comp_mass_j = symo.replace(comp_mass[j], 'MJI', j)
comp_mass[i] = comp_mass[i] + i_comp_mass_j
composite_inertia[i] = inertia_spatial(
comp_inertia3[i], comp_ms[i], comp_mass[i]
)
def compute_screw_transform(robo, symo, j, antRj, antPj, jTant):
"""Internal function. Computes the screw transformation matrix
between ant[j] and j frames.
Notes
=====
jTant is an output parameter
"""
jRant = antRj[j].T
ET = symo.mat_replace(-jRant * tools.skew(antPj[j]), 'JPR', j)
jTant[j] = (Matrix([jRant.row_join(ET), zeros(3, 3).row_join(jRant)]))
def compute_screw_transform(robo, symo, j, antRj, antPj, jTant):
"""Internal function. Computes the screw transformation matrix
between ant[j] and j frames.
Notes
=====
jTant is an output parameter
"""
jRant = antRj[j].T
ET = symo.mat_replace(-jRant*tools.skew(antPj[j]), 'JPR', j)
jTant[j] = (Matrix([jRant.row_join(ET),
zeros(3, 3).row_join(jRant)]))
def compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt,
Fex, Nex):
"""
Compute reaction wrench (for default Newton-Euler) of joint j
(internal function).
Note:
Fjnt, Njnt, Fex, Nex are the output parameters
"""
forced = True if j == 0 else False
i = robo.ant[j]
Fjnt[j] = F[j] + Fex[j]
Fjnt[j] = symo.mat_replace(Fjnt[j], 'E', j, forced=forced)
Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
Njnt[j] = symo.mat_replace(Njnt[j], 'N', j, forced=forced)
f_ant = antRj[j] * Fjnt[j]
f_ant = symo.mat_replace(f_ant, 'FDI', j)
if i != -1:
Fex[i] = Fex[i] + f_ant
Nex[i] = Nex[i] + \
(antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant)
def _compute_reaction_wrench(
robo, symo, name, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex
):
"""
Compute reaction wrench (for default Newton-Euler) of joint j
(internal function).
Note:
Fjnt, Njnt, Fex, Nex are the output parameters
"""
i = robo.ant[j]
Fjnt[j] = F[j] + Fex[j]
Fjnt[j] = vec_replace_wrapper(symo, Fjnt[j], 'E', name, j)
Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
Njnt[j] = vec_replace_wrapper(symo, Njnt[j], 'N', name, j)
f_ant = antRj[j] * Fjnt[j]
f_ant = vec_replace_wrapper(symo, f_ant, 'FDI', name, j)
if i != -1:
Fex[i] = Fex[i] + f_ant
Nex[i] = Nex[i] + \
(antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant)
def compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
"""
Compute total wrench of link j (internal function).
Note:
F, N are the output parameters
"""
F[j] = (robo.M[j] * vdot[j]) + (U[j] * robo.MS[j])
F[j] = symo.mat_replace(F[j], 'F', j)
Psi = robo.J[j] * w[j]
Psi = symo.mat_replace(Psi, 'PSI', j)
N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi)
N[j] = symo.mat_replace(N[j], 'No', j)
def compute_joint_wrench(
robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex
):
"""
Compute reaction wrench (for default Newton-Euler) of joint j
(internal function).
Note:
Fjnt, Njnt, Fex, Nex are the output parameters
"""
forced = True if j == 0 else False
i = robo.ant[j]
Fjnt[j] = F[j] + Fex[j]
Fjnt[j] = symo.mat_replace(Fjnt[j], 'E', j, forced=forced)
Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
Njnt[j] = symo.mat_replace(Njnt[j], 'N', j, forced=forced)
f_ant = antRj[j] * Fjnt[j]
f_ant = symo.mat_replace(f_ant, 'FDI', j)
if i != -1:
Fex[i] = Fex[i] + f_ant
Nex[i] = Nex[i] + \
(antRj[j] * Njnt[j]) + (tools.skew(antPj[j]) * f_ant)
def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
"""Internal function. Computes total wrench (torques and forces)
of the link j
Notes
=====
F, N are the output parameters
"""
F[j] = robo.M[j] * vdot[j] + U[j] * robo.MS[j]
symo.mat_replace(F[j], 'F', j)
Psi = symo.mat_replace(robo.J[j] * w[j], 'PSI', j)
N[j] = robo.J[j] * wdot[j] + tools.skew(w[j]) * Psi
symo.mat_replace(N[j], 'No', j)
def compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
"""
Compute total wrench of link j (internal function).
Note:
F, N are the output parameters
"""
F[j] = (robo.M[j] * vdot[j]) + (U[j] * robo.MS[j])
F[j] = symo.mat_replace(F[j], 'F', j)
Psi = robo.J[j] * w[j]
Psi = symo.mat_replace(Psi, 'PSI', j)
N[j] = (robo.J[j] * wdot[j]) + (tools.skew(w[j]) * Psi)
N[j] = symo.mat_replace(N[j], 'No', j)
def compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N):
"""Internal function. Computes total wrench (torques and forces)
of the link j
Notes
=====
F, N are the output parameters
"""
F[j] = robo.M[j]*vdot[j] + U[j]*robo.MS[j]
symo.mat_replace(F[j], 'F', j)
Psi = symo.mat_replace(robo.J[j]*w[j], 'PSI', j)
N[j] = robo.J[j]*wdot[j] + tools.skew(w[j])*Psi
symo.mat_replace(N[j], 'No', j)
def compute_composite_inertia(robo, symo, j, antRj, antPj, comp_inertia3,
comp_ms, comp_mass, composite_inertia):
"""
Compute composite inertia (internal function).
Note:
comp_inertia3, comp_ms, comp_mass, composite_inertia are the
output parameters.
"""
i = robo.ant[j]
# update inertia3, ms, mass from inertia in order to have the
# intermediate variables
comp_inertia3[i] = composite_inertia[i][3:, 3:]
comp_ms[i] = tools.skew2vec(composite_inertia[i][3:, 0:3])
comp_mass[i] = composite_inertia[i][0, 0]
comp_inertia3[j] = composite_inertia[j][3:, 3:]
comp_ms[j] = tools.skew2vec(composite_inertia[j][3:, 0:3])
comp_mass[j] = composite_inertia[j][0, 0]
# actual computation
i_ms_j_c = antRj[j] * comp_ms[j]
i_ms_j_c = symo.mat_replace(i_ms_j_c, 'AS', j)
expr1 = antRj[j] * comp_inertia3[j]
expr1 = symo.mat_replace(expr1, 'AJ', j)
expr2 = expr1 * antRj[j].transpose()
expr2 = symo.mat_replace(expr2, 'AJA', j)
expr3 = tools.skew(antPj[j]) * tools.skew(i_ms_j_c)
expr3 = symo.mat_replace(expr3, 'PAS', j)
i_comp_inertia3_j = expr2 - (expr3 + expr3.transpose()) + \
(comp_mass[j] * tools.skew(antPj[j]) * \
tools.skew(antPj[j]).transpose())
i_comp_inertia3_j = symo.mat_replace(i_comp_inertia3_j, 'JJI', j)
comp_inertia3[i] = comp_inertia3[i] + i_comp_inertia3_j
i_comp_ms_j = i_ms_j_c + (antPj[j] * comp_mass[j])
i_comp_ms_j = symo.mat_replace(i_comp_ms_j, 'MSJI', j)
comp_ms[i] = comp_ms[i] + i_comp_ms_j
i_comp_mass_j = symo.replace(comp_mass[j], 'MJI', j)
comp_mass[i] = comp_mass[i] + i_comp_mass_j
composite_inertia[i] = inertia_spatial(comp_inertia3[i], comp_ms[i],
comp_mass[i])
def compute_vel_acc(robo,
symo,
antRj,
antPj,
forced=False,
gravity=True,
floating=False):
"""Internal function. Computes speeds and accelerations usitn
Parameters
==========
robo : Robot
Instance of robot description container
symo : symbolmgr.SymbolManager
Instance of symbolic manager
"""
#init velocities and accelerations
w = ParamsInit.init_w(robo)
wdot, vdot = ParamsInit.init_wv_dot(robo, gravity)
# decide first link
first_link = 1
if floating or robo.is_floating or robo.is_mobile:
first_link = 0
#init auxilary matrix
U = ParamsInit.init_u(robo)
for j in xrange(first_link, robo.NL):
if j == 0:
w[j] = symo.mat_replace(w[j], 'W', j)
wdot[j] = symo.mat_replace(wdot[j], 'WP', j)
vdot[j] = symo.mat_replace(vdot[j], 'VP', j)
dv0 = ParamsInit.product_combinations(w[j])
symo.mat_replace(dv0, 'DV', j)
hatw_hatw = Matrix([[-dv0[3] - dv0[5], dv0[1], dv0[2]],
[dv0[1], -dv0[5] - dv0[0], dv0[4]],
[dv0[2], dv0[4], -dv0[3] - dv0[0]]])
U[j] = hatw_hatw + tools.skew(wdot[j])
symo.mat_replace(U[j], 'U', j)
else:
jRant = antRj[j].T
qdj = Z_AXIS * robo.qdot[j]
qddj = Z_AXIS * robo.qddot[j]
wi, w[j] = _omega_ij(robo, j, jRant, w, qdj)
symo.mat_replace(w[j], 'W', j)
symo.mat_replace(wi, 'WI', 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_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1):
"""Internal function. Computes elements below and above diagonal
of the inertia matrix
Notes
=====
f, n, A are the output parameters
"""
f[ka] = antRj[k]*f[k]
if k == j and robo.sigma[j] == 0:
n[ka] = AJE1[k] + tools.skew(antPj[k])*f[k]
else:
n[ka] = antRj[k]*n[k] + tools.skew(antPj[k])*f[k]
if ka == - 1:
symo.mat_replace(f[ka], 'AV0')
symo.mat_replace(n[ka], 'AW0')
else:
symo.mat_replace(f[ka], 'E' + chars[j], ka)
symo.mat_replace(n[ka], 'N' + chars[j], ka)
if robo.sigma[ka] == 0:
A[j, ka] = n[ka][2]
elif robo.sigma[ka] == 1:
A[j, ka] = f[ka][2]
A[ka, j] = A[j, ka]
def compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1):
"""Internal function. Computes elements below and above diagonal
of the inertia matrix
Notes
=====
f, n, A are the output parameters
"""
f[ka] = antRj[k] * f[k]
if k == j and robo.sigma[j] == 0:
n[ka] = AJE1[k] + tools.skew(antPj[k]) * f[k]
else:
n[ka] = antRj[k] * n[k] + tools.skew(antPj[k]) * f[k]
if ka == -1:
symo.mat_replace(f[ka], 'AV0')
symo.mat_replace(n[ka], 'AW0')
else:
symo.mat_replace(f[ka], 'E' + chars[j], ka)
symo.mat_replace(n[ka], 'N' + chars[j], ka)
if robo.sigma[ka] == 0:
A[j, ka] = n[ka][2]
elif robo.sigma[ka] == 1:
A[j, ka] = f[ka][2]
A[ka, j] = A[j, ka]
def compute_vel_acc(
robo, symo, antRj, antPj, forced=False, gravity=True, floating=False
):
"""Internal function. Computes speeds and accelerations usitn
Parameters
==========
robo : Robot
Instance of robot description container
symo : symbolmgr.SymbolManager
Instance of symbolic manager
"""
#init velocities and accelerations
w = ParamsInit.init_w(robo)
wdot, vdot = ParamsInit.init_wv_dot(robo, gravity)
# decide first link
first_link = 1
if floating or robo.is_floating or robo.is_mobile:
first_link = 0
#init auxilary matrix
U = ParamsInit.init_u(robo)
for j in xrange(first_link, robo.NL):
if j == 0:
w[j] = symo.mat_replace(w[j], 'W', j)
wdot[j] = symo.mat_replace(wdot[j], 'WP', j)
vdot[j] = symo.mat_replace(vdot[j], 'VP', j)
dv0 = ParamsInit.product_combinations(w[j])
symo.mat_replace(dv0, 'DV', j)
hatw_hatw = Matrix([
[-dv0[3]-dv0[5], dv0[1], dv0[2]],
[dv0[1], -dv0[5]-dv0[0], dv0[4]],
[dv0[2], dv0[4], -dv0[3]-dv0[0]]
])
U[j] = hatw_hatw + tools.skew(wdot[j])
symo.mat_replace(U[j], 'U', j)
else:
jRant = antRj[j].T
qdj = Z_AXIS * robo.qdot[j]
qddj = Z_AXIS * robo.qddot[j]
wi, w[j] = _omega_ij(robo, j, jRant, w, qdj)
symo.mat_replace(w[j], 'W', j)
symo.mat_replace(wi, 'WI', 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 _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False):
"""
Computes jacobian of frame n (with origin On in Oj) projected to frame i
"""
# symo.write_geom_param(robo, 'Jacobian')
# TODO: Check projection frames, rewrite DGM call for higher efficiency
J_col_list = []
if chain is None:
chain = robo.chain(n)
chain.reverse()
# chain_ext = chain + [robo.ant[min(chain)]]
# if not i in chain_ext:
# i = min(chain_ext)
# if not j in chain_ext:
# j = max(chain_ext)
kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs)
kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs)
kTj_dict.update(kTj_tmp)
iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs)
iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs)
iTk_dict.update(iTk_tmp)
for k in chain:
kTj = kTj_dict[k, j]
iTk = iTk_dict[i, k]
isk, ink, iak = Transform.sna(iTk)
sigm = robo.sigma[k]
if sigm == 1:
dvdq = iak
J_col = dvdq.col_join(Matrix([0, 0, 0]))
elif sigm == 0:
dvdq = kTj[0, 3] * ink - kTj[1, 3] * isk
J_col = dvdq.col_join(iak)
else:
J_col = Matrix([0, 0, 0, 0, 0, 0])
J_col_list.append(J_col.T)
Jac = Matrix(J_col_list).T
Jac = Jac.applyfunc(symo.simp)
iRj = Transform.R(iTk_dict[i, j])
jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs)
jPn = Transform.P(jTn)
L = -tools.skew(iRj * jPn)
L = L.applyfunc(trigsimp)
if forced:
symo.mat_replace(Jac, 'J', '', forced)
L = symo.mat_replace(L, 'L', '', forced)
return Jac, L
def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False):
"""
Computes jacobian of frame n (with origin On in Oj) projected to frame i
"""
# symo.write_geom_param(robo, 'Jacobian')
# TODO: Check projection frames, rewrite DGM call for higher efficiency
J_col_list = []
if chain is None:
chain = robo.chain(n)
chain.reverse()
# chain_ext = chain + [robo.ant[min(chain)]]
# if not i in chain_ext:
# i = min(chain_ext)
# if not j in chain_ext:
# j = max(chain_ext)
kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs)
kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs)
kTj_dict.update(kTj_tmp)
iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs)
iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs)
iTk_dict.update(iTk_tmp)
for k in chain:
kTj = kTj_dict[k, j]
iTk = iTk_dict[i, k]
isk, ink, iak = Transform.sna(iTk)
sigm = robo.sigma[k]
if sigm == 1:
dvdq = iak
J_col = dvdq.col_join(Matrix([0, 0, 0]))
elif sigm == 0:
dvdq = kTj[0, 3]*ink-kTj[1, 3]*isk
J_col = dvdq.col_join(iak)
else:
J_col = Matrix([0, 0, 0, 0, 0, 0])
J_col_list.append(J_col.T)
Jac = Matrix(J_col_list).T
Jac = Jac.applyfunc(symo.simp)
iRj = Transform.R(iTk_dict[i, j])
jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs)
jPn = Transform.P(jTn)
L = -tools.skew(iRj*jPn)
L = L.applyfunc(trigsimp)
if forced:
symo.mat_replace(Jac, 'J', '', forced)
L = symo.mat_replace(L, 'L', '', forced)
return Jac, L
def _compute_base_reaction_wrench(
robo, symo, name, antRj, antPj, vdot, F, N, Fex, Nex, Fjnt, Njnt
):
"""
Compute reaction wrench (for default Newton-Euler) on the base
(internal function).
Note:
Fjnt, Njnt are the output parameters
"""
j = 0
Fjnt[j] = F[j] + Fex[j]
Fjnt[j] = vec_replace_wrapper(
symo, Fjnt[j], 'DE', name, j, forced=True
)
Njnt[j] = N[j] + Nex[j] + (tools.skew(robo.MS[j]) * vdot[j])
Njnt[j] = vec_replace_wrapper(
symo, Njnt[j], 'DN', name, j, forced=True
)
def inertia_spatial(J, MS, M):
return Matrix([(M * sympy.eye(3)).row_join(tools.skew(MS).T),
tools.skew(MS).row_join(J)])
def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False):
ant = robo.ant[j]
v[j] = jRant * (tools.skew(w[ant]) * antPj[j] + v[ant])
if robo.sigma[j] == 1: # prismatic joint
v[j] += qdj
return v[j]
def _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj):
wdot[j] = jRant * wdot[robo.ant[j]]
if robo.sigma[j] == 0: # revolute joint
wdot[j] += (qddj + tools.skew(wi) * qdj)
return wdot[j]
def _v_j(robo, j, antPj, jRant, v, w, qdj, forced=False):
ant = robo.ant[j]
v[j] = jRant*(tools.skew(w[ant])*antPj[j] + v[ant])
if robo.sigma[j] == 1: # prismatic joint
v[j] += qdj
return v[j]
