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