def _compute_base_acceleration(model, robo):
"""
Compute the base acceleration for a robot with floating base without
and with taking gravity into account. In the case of a robot with
fixed base, this function returns just the effect of gravity as the
base acceleration.
Args:
model: An instance of DynModel
robo: An instance of Robot
Returns:
An instance of DynModel that contains all the new values.
"""
o_vdot_o = Screw()
gravity = Screw()
# local variables
gravity.lin = robo.gravity
if robo.is_floating:
if model.model_type is 'inverse':
o_inertia_o_c = model.composite_inertias[0].val
o_beta_o_c = model.composite_betas[0].val
elif model.model_type is 'direct':
o_inertia_o_c = model.star_inertias[0].val
o_beta_o_c = model.star_betas[0].val
# actual computation
# TODO: replace sympy's matrix inversion with custom function
o_vdot_o.val = o_inertia_o_c.inv() * o_beta_o_c
# store computed base acceleration without gravity effect in model
model.base_accel_w_gravity = copy.copy(o_vdot_o)
# compute base acceleration removing gravity effect
o_vdot_o.val = o_vdot_o.val - gravity.val
# store in model
model.accels[0] = o_vdot_o
return model
def _compute_base_acceleration(model, robo):
"""
Compute the base acceleration for a robot with floating base without
and with taking gravity into account. In the case of a robot with
fixed base, this function returns just the effect of gravity as the
base acceleration.
Args:
model: An instance of DynModel
robo: An instance of Robot
Returns:
An instance of DynModel that contains all the new values.
"""
o_vdot_o = Screw()
gravity = Screw()
# local variables
gravity.lin = robo.gravity
if robo.is_floating:
if model.model_type is 'inverse':
o_inertia_o_c = model.composite_inertias[0].val
o_beta_o_c = model.composite_betas[0].val
elif model.model_type is 'direct':
o_inertia_o_c = model.star_inertias[0].val
o_beta_o_c = model.star_betas[0].val
# actual computation
# TODO: replace sympy's matrix inversion with custom function
o_vdot_o.val = o_inertia_o_c.inv() * o_beta_o_c
# store computed base acceleration without gravity effect in model
model.base_accel_w_gravity = copy.copy(o_vdot_o)
# compute base acceleration removing gravity effect
o_vdot_o.val = o_vdot_o.val - gravity.val
# store in model
model.accels[0] = o_vdot_o
return model
def _compute_composite_beta(model, robo, j, i):
"""
Compute the composite beta wrench for link i.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
i_beta_i_c = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
i_beta_i = model.composite_betas[i].val
j_beta_j_c = model.composite_betas[j].val
j_inertia_j_c = model.composite_inertias[j].val
j_zeta_j = model.zetas[j].val
# actual computation
i_beta_i_c.val = i_beta_i + (j_s_i.transpose() * j_beta_j_c) - \
(j_s_i.transpose() * j_inertia_j_c * j_zeta_j)
# store computed beta in model
model.composite_betas[i] = i_beta_i_c
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_relative_acceleration(model, robo, j):
"""
Compute the relative acceleration of link j.
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_zeta_j = Screw()
# local variables
j_a_j = robo.geos[j].axisa
j_gamma_j = model.gammas[j].val
if model.model_type is 'inverse':
qddot_j = robo.qddots[j]
elif model.model_type is 'direct':
qddot_j = model.qddots[j]
# actual computation
j_zeta_j.val = j_gamma_j + (qddot_j * j_a_j)
# store computed relative acceleration in model
model.zetas[j] = j_zeta_j
return model
def _compute_alpha_wrench(model, robo, j):
"""
Compute the wrench as a function of tau - joint torque without
friction.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: joint number
Returns:
An instance of DynModel that contains all the new values.
"""
j_alpha_j = Screw()
# local variables
j_a_j = robo.geos[j].axisa
j_k_j = model.no_qddot_inertias[j].val
j_gamma_j = model.gammas[j].val
j_inertia_j_s = model.star_inertias[j].val
j_beta_j_s = model.star_betas[j].val
h_j = Matrix([model.joint_inertias[j]])
tau_j = Matrix([model.taus[j]])
# actual computation
j_alpha_j.val = (j_k_j * j_gamma_j) + \
(j_inertia_j_s * j_a_j * h_j.inv() * \
(tau_j + j_a_j.transpose() * j_beta_j_s)) - j_beta_j_s
# store in model
model.alphas[j] = j_alpha_j
return model
def _compute_link_velocity(model, robo, j, i):
"""
Compute the velocity of link j whose antecedent is i.
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_v_j = Screw()
if i == 0: model.vels[i] = robo.base_vel
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
qdot_j = robo.qdots[j]
j_a_j = robo.geos[j].axisa
i_v_i = model.vels[i].val
# actual computation
j_v_j.val = (j_s_i * i_v_i) + (qdot_j * j_a_j)
# store computed velocity in model
model.vels[j] = j_v_j
return model
def _compute_alpha_wrench(model, robo, j):
"""
Compute the wrench as a function of tau - joint torque without
friction.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: joint number
Returns:
An instance of DynModel that contains all the new values.
"""
j_alpha_j = Screw()
# local variables
j_a_j = robo.geos[j].axisa
j_k_j = model.no_qddot_inertias[j].val
j_gamma_j = model.gammas[j].val
j_inertia_j_s = model.star_inertias[j].val
j_beta_j_s = model.star_betas[j].val
h_j = Matrix([model.joint_inertias[j]])
tau_j = Matrix([model.taus[j]])
# actual computation
j_alpha_j.val = (j_k_j * j_gamma_j) + \
(j_inertia_j_s * j_a_j * h_j.inv() * \
(tau_j + j_a_j.transpose() * j_beta_j_s)) - j_beta_j_s
# store in model
model.alphas[j] = j_alpha_j
return model
def _compute_link_velocity(model, robo, j, i):
"""
Compute the velocity of link j whose antecedent is i.
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_v_j = Screw()
if i == 0: model.vels[i] = robo.base_vel
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
qdot_j = robo.qdots[j]
j_a_j = robo.geos[j].axisa
i_v_i = model.vels[i].val
# actual computation
j_v_j.val = (j_s_i * i_v_i) + (qdot_j * j_a_j)
# store computed velocity in model
model.vels[j] = j_v_j
return model
def _compute_composite_beta(model, robo, j, i):
"""
Compute the composite beta wrench for link i.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
i_beta_i_c = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
i_beta_i = model.composite_betas[i].val
j_beta_j_c = model.composite_betas[j].val
j_inertia_j_c = model.composite_inertias[j].val
j_zeta_j = model.zetas[j].val
# actual computation
i_beta_i_c.val = i_beta_i + (j_s_i.transpose() * j_beta_j_c) - \
(j_s_i.transpose() * j_inertia_j_c * j_zeta_j)
# store computed beta in model
model.composite_betas[i] = i_beta_i_c
return model
def _compute_relative_acceleration(model, robo, j):
"""
Compute the relative acceleration of link j.
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_zeta_j = Screw()
# local variables
j_a_j = robo.geos[j].axisa
j_gamma_j = model.gammas[j].val
if model.model_type is 'inverse':
qddot_j = robo.qddots[j]
elif model.model_type is 'direct':
qddot_j = model.qddots[j]
# actual computation
j_zeta_j.val = j_gamma_j + (qddot_j * j_a_j)
# store computed relative acceleration in model
model.zetas[j] = j_zeta_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_reaction_wrench(model, robo, j):
"""
Compute the reaction wrench for link j.
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_f_j = Screw()
# local variables
j_vdot_j = model.accels[j].val
j_inertia_j_c = model.composite_inertias[j].val
j_beta_j_c = model.composite_betas[j].val
# actual computation
j_f_j.val = (j_inertia_j_c * j_vdot_j) - j_beta_j_c
# store computed reaction wrench in model
model.wrenchs[j] = j_f_j
return model
def _compute_reaction_wrench(model, robo, j):
"""
Compute the reaction wrench for link j.
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_f_j = Screw()
# local variables
j_vdot_j = model.accels[j].val
j_inertia_j_c = model.composite_inertias[j].val
j_beta_j_c = model.composite_betas[j].val
# actual computation
j_f_j.val = (j_inertia_j_c * j_vdot_j) - j_beta_j_c
# store computed reaction wrench in model
model.wrenchs[j] = j_f_j
return model
def _compute_link_acceleration(model, robo, j, i):
"""
Compute the acceleration of link j whose antecedent is i.
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_vdot_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
i_vdot_i = model.accels[i].val
j_zeta_j = model.zetas[j].val
# actual computation
j_vdot_j.val = (j_s_i * i_vdot_i) + j_zeta_j
# store computed velocity in model
model.accels[j] = j_vdot_j
return model
def _compute_link_acceleration(model, robo, j, i):
"""
Compute the acceleration of link j whose antecedent is i.
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_vdot_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
i_vdot_i = model.accels[i].val
j_zeta_j = model.zetas[j].val
# actual computation
j_vdot_j.val = (j_s_i * i_vdot_i) + j_zeta_j
# store computed velocity in model
model.accels[j] = j_vdot_j
return model
def _compute_reaction_wrench_alpha(model, robo, j, i):
"""
Compute the reaction wrench for link j as a function of alpha wrench.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_f_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
j_k_j = model.no_qddot_inertias[j].val
j_alpha_j = model.alphas[j].val
i_vdot_i = model.accels[i].val
# actual computation
j_f_j.val = (j_k_j * j_s_i * i_vdot_i) + j_alpha_j
# store in model
model.wrenchs[j] = j_f_j
return model
def _compute_star_beta(model, robo, j, i):
"""
Compute the star beta wrench for link i. This is similar to the
composite beta wrench but is a function of `tau` instead of `qddot`.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
i_beta_i_s = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
i_beta_i = model.star_betas[i].val
j_alpha_j = model.alphas[j].val
# actual computation
i_beta_i_s.val = i_beta_i - (j_s_i.transpose() * j_alpha_j)
# store in model
model.star_betas[i] = i_beta_i_s
return model
def _compute_reaction_wrench_alpha(model, robo, j, i):
"""
Compute the reaction wrench for link j as a function of alpha wrench.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_f_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
j_k_j = model.no_qddot_inertias[j].val
j_alpha_j = model.alphas[j].val
i_vdot_i = model.accels[i].val
# actual computation
j_f_j.val = (j_k_j * j_s_i * i_vdot_i) + j_alpha_j
# store in model
model.wrenchs[j] = j_f_j
return model
def _compute_star_beta(model, robo, j, i):
"""
Compute the star beta wrench for link i. This is similar to the
composite beta wrench but is a function of `tau` instead of `qddot`.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
i_beta_i_s = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
i_beta_i = model.star_betas[i].val
j_alpha_j = model.alphas[j].val
# actual computation
i_beta_i_s.val = i_beta_i - (j_s_i.transpose() * j_alpha_j)
# store in model
model.star_betas[i] = i_beta_i_s
return model
