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