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_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_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
