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_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_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_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_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 test_equality(self): """Test __eq__() and __ne__()""" self.assertEqual(self.empty, Screw()) self.assertNotEqual(self.indiv, Screw()) self.assertRaises(ValueError, self.empty.__eq__, Matrix([1, 2, 3, 4, 5, 6, 7, 8])) self.assertRaises(ValueError, self.empty.__ne__, Matrix([1, 2, 3, 4, 5, 6, 7, 8]))
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
def setUp(self): # screw with 0s self.empty = Screw() # screw created by separate linear and angular terms self.indiv = Screw(lin=Matrix([1, 2, 3]), ang=Matrix([4, 5, 6]))
def wrench(self): """Get external force (6x1) column vector (linear + angular).""" return Screw(lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]), ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext]))
def __init__( self, name, links=0, joints=0, frames=0, is_floating=True, structure=tools.TREE, is_mobile=False, is_symbolic=True ): """ Constructor period. Usage: """ """Name of the robot.""" self.name = name """Folder to store the files related to the robot.""" self.directory = filemgr.get_folder_path(name) """Total number of links in the robot.""" self.num_links = links """Total number of joints in the robot.""" self.num_joints = joints """Total number of frames in the robot.""" self.num_frames = frames """To indicate if the base is floating or not.""" self.is_floating = is_floating """Type of the robot structure - simple, tree, closed-loop""" self.structure = structure """To indicate if the robot is a wheeled mobile robot""" self.is_mobile = is_mobile """To indicate if computation should be symbolic or numeric""" self.is_symbolic = is_symbolic # properties dependent on number of links """ List to hold the dynamic parameters. The indices of the list start with 0 and it corresponds to parameters of link 0 (virtual link of the base). """ self.dyns = [DynParams(j) for j in self.link_nums] # properties dependent on number of joints """ To indicate if a joint is rigid or flexible. 0 for rigid and 1 for flexible. The indices of the list start with 0 and corresponds to a virtual joint of the base. This joint is usually rigid. """ self.etas = [0 for j in self.joint_nums] """Joint stiffness usually indicated by k.""" self.stiffness = [0 for j in self.joint_nums] """Joint velocities.""" self.qdots = [var('QP{0}'.format(j)) for j in self.joint_nums] """Joint accelerations.""" self.qddots = [var('QDP{0}'.format(j)) for j in self.joint_nums] """Joint torques.""" self.torques = [var('GAM{0}'.format(j)) for j in self.joint_nums] # properties dependent on number of frames """ List to hold the geometric parameters. NOTE: This might be moved to a separate function. The indices of the list start with 0 and the first object corresponds to parameters of frame 0 (base) wrt its antecedent (some arbitary reference frame). """ self.geos = [GeoParams(j) for j in self.frame_nums] # properties independent of number of links, joints and frames """Gravity vector a 3x1 Matrix.""" self.gravity = Matrix([0, 0, var('G3')]) # the values of properties below would be modified during # the computation of dynamic models. """Base velocity 6x1 column vector - a Screw.""" self.base_vel = Screw() """Base acceleration 6x1 column vector - a Screw.""" self.base_accel = Screw() """Transformation matrix of base wrt a reference frame at time 0.""" self.base_tmat = eye(4) # call init methods self._init_maps()