def direct_geometric(robo, frames, trig_subs): """Computes trensformation matrix iTj. Parameters ========== robo: Robot Instance of robot description container frames: list of tuples of type (i,j) Defines list of required transformation matrices iTj trig_subs: bool, optional If True, all the sin(x) and cos(x) will be replaced by symbols SX and CX with adding them to the dictionary Returns ======= symo: symbolmgr.SymbolManager Instance that contains all the relations of the computed model """ symo = symbolmgr.SymbolManager() symo.file_open(robo, 'trm') symo.write_params_table(robo, 'Direct Geometric model') for i, j in frames: symo.write_line('Tramsformation matrix %s T %s' % (i, j)) T = dgm(robo, symo, i, j, fast_form=False, trig_subs=trig_subs) symo.mat_replace(T, 'T%sT%s' % (i, j), forced=True, skip=1) symo.write_line() symo.file_close() return symo
def kinematic_constraints(robo): symo = symbolmgr.SymbolManager(None) res = _kinematic_loop_constraints(robo, symo) if res == tools.FAIL: return tools.FAIL W_a, W_p, W_ac, W_pc, W_c = res symo.file_open(robo, 'ckel') symo.write_params_table(robo, 'Constraint kinematic equations of loop', equations=False) symo.write_line('Active joint variables') symo.write_line([robo.get_q(i) for i in robo.indx_active]) symo.write_line() symo.write_line('Passive joints variables') symo.write_line([robo.get_q(i) for i in robo.indx_passive]) symo.write_line() symo.write_line('Cut joints variables') symo.write_line([robo.get_q(i) for i in robo.indx_cut]) symo.write_line() symo.mat_replace(W_a, 'WA', forced=True) symo.mat_replace(W_p, 'WP', forced=True) symo.mat_replace(W_ac, 'WPA', forced=True) symo.mat_replace(W_pc, 'WPC', forced=True) symo.mat_replace(W_c, 'WC', forced=True) symo.file_close() return symo
def compute_pseudotorques(self): """ Compute Coriolis, Centrifugal, Gravity, Friction and external torques using Newton-Euler algortihm. """ pseudo_robo = copy.deepcopy(self) pseudo_robo.qddot = zeros(pseudo_robo.NL, 1) symo = symbolmgr.SymbolManager() symo.file_open(self, 'ccg') title = "Pseudo forces using Newton-Euler Algorithm\n" if 1 in pseudo_robo.eta: # with flexible joints title = title + "Robot with flexible joints\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.flexible_inverse_dynmodel(pseudo_robo, symo) elif pseudo_robo.is_floating: # with rigid joints and floating base title = title + "Robot with rigid joints and floating base\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.composite_inverse_dynmodel(pseudo_robo, symo) elif pseudo_robo.is_mobile: # mobile robot with rigid joints - known base acceleration title = title + "Robot with mobile base (Vdot0 is known)\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.mobile_inverse_dynmodel(pseudo_robo, symo) else: # with rigid joints and fixed base title = title + "Robot with rigid joints and fixed base\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.fixed_inverse_dynmodel(pseudo_robo, symo) symo.file_close() return symo
def compute_idym(self): """ Compute the Inverse Dynamic Model of the robot using the recursive Newton-Euler algorithm. Also choose the Newton-Euler algorithm based on the robot type. """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'idm') title = "Inverse Dynamic Model using Newton-Euler Algorithm\n" if 1 in self.eta: # with flexible joints title = title + "Robot with flexible joints\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.flexible_inverse_dynmodel(self, symo) elif self.is_floating: # with rigid joints and floating base title = title + "Robot with rigid joints and floating base\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.composite_inverse_dynmodel(self, symo) elif self.is_mobile: # mobile robot with rigid joints - known base acceleration title = title + "Robot with mobile base (Vdot0 is known)\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.mobile_inverse_dynmodel(self, symo) else: # with rigid joints and fixed base title = title + "Robot with rigid joints and fixed base\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.fixed_inverse_dynmodel(self, symo) symo.file_close() return symo
def igm_pieper(robo, T_ref, n): symo = symbolmgr.SymbolManager() symo.file_open(robo, 'pieper') symo.write_params_table(robo, 'Inverse Geometrix Model for frame %s' % n) _pieper_solve(robo, symo) symo.file_close() return symo
def generate_loop_fcn(self): symo = symbolmgr.SymbolManager(sydi=self.pars_num) loop_solve(self.robo, symo) self.l_solver = symo.gen_func('IGM_gen', self.q_pas_sym, self.q_act_sym, multival=True)
def direct_dynamic_NE(robo): """Computes Direct Dynamic Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ wi = ParamsInit.init_vec(robo) # antecedent angular velocity, projected into jth frame w = ParamsInit.init_w(robo) jaj = ParamsInit.init_vec(robo, 6) jTant = ParamsInit.init_mat(robo, 6) # Twist transform list of Matrices 6x6 beta_star = ParamsInit.init_vec(robo, 6) grandJ = ParamsInit.init_mat(robo, 6) link_acc = ParamsInit.init_vec(robo, 6) H_inv = ParamsInit.init_scalar(robo) juj = ParamsInit.init_vec(robo, 6) # Jj*aj / Hj Tau = ParamsInit.init_scalar(robo) grandVp = ParamsInit.init_vec(robo, 6) grandVp.append(Matrix([robo.vdot0 - robo.G, robo.w0])) symo = symbolmgr.SymbolManager() symo.file_open(robo, 'ddm') title = 'Direct dynamic model using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in xrange(1, robo.NL): compute_omega(robo, symo, j, antRj, w, wi) compute_screw_transform(robo, symo, j, antRj, antPj, jTant) if robo.sigma[j] == 0: jaj[j] = Matrix([0, 0, 0, 0, 0, 1]) elif robo.sigma[j] == 1: jaj[j] = Matrix([0, 0, 1, 0, 0, 0]) for j in xrange(1, robo.NL): compute_beta(robo, symo, j, w, beta_star) compute_link_acc(robo, symo, j, antRj, antPj, link_acc, w, wi) grandJ[j] = inertia_spatial(robo.J[j], robo.MS[j], robo.M[j]) for j in reversed(xrange(1, robo.NL)): replace_beta_J_star(robo, symo, j, grandJ, beta_star) compute_Tau(robo, symo, j, grandJ, beta_star, jaj, juj, H_inv, Tau) if robo.ant[j] != -1: compute_beta_J_star(robo, symo, j, grandJ, jaj, juj, Tau, beta_star, jTant, link_acc) for j in xrange(1, robo.NL): compute_acceleration(robo, symo, j, jTant, grandVp, juj, H_inv, jaj, Tau, link_acc) for j in xrange(1, robo.NL): compute_coupled_forces(robo, symo, j, grandVp, grandJ, beta_star) symo.file_close() return symo
def accelerations(robo): symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'acc') symo.write_params_table(robo, 'Link accelerations') antRj, antPj = compute_rot_trans(robo, symo) compute_vel_acc(robo, symo, antRj, antPj, forced=True, gravity=False) symo.file_close() return symo
def igm_Paul(robo, T_ref, n): if isinstance(T_ref, list): T_ref = Matrix(4, 4, T_ref) symo = symbolmgr.SymbolManager() symo.file_open(robo, 'igm') symo.write_params_table(robo, 'Inverse Geometric Model for frame %s' % n) _paul_solve(robo, symo, T_ref, 0, n) symo.file_close() return symo
def jacobian(robo, n, i, j): symo = symbolmgr.SymbolManager() symo.file_open(robo, 'jac') title = "Jacobian matrix for frame {}\n" title += "Projection frame {}, intermediate frame {}" symo.write_params_table(robo, title.format(n, i, j)) _jac(robo, symo, n, i, j, forced=True) symo.file_close() return symo
def dgm_for_frame(self, i): if i not in self.dgms: jnt = self.jnt_objs[i] if i > 0 and jnt.r == 0 and jnt.d == 0 and jnt.b == 0: self.dgms[i] = self.dgm_for_frame(self.robo.ant[i]) else: symo = symbolmgr.SymbolManager(sydi=self.pars_num) T = dgm(self.robo, symo, 0, i, fast_form=True, trig_subs=True) self.dgms[i] = symo.gen_func('dgm_generated', T, self.q_sym) return self.dgms[i]
def compute_dynidenmodel(self): """ Compute the Dynamic Identification model of the robot. """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'dim') title = "Dynamic Identification Model (Newton-Euler method)" symo.write_params_table(self, title, inert=True, dynam=True) dyniden.dynamic_identification_model(self, symo) symo.file_close() return symo
def jacobian_determinant(robo, n, i, j, rows, cols): symo = symbolmgr.SymbolManager(None) J, L = _jac(robo, symo, n, i, j, trig_subs=False) J_reduced = zeros(len(rows), len(cols)) for i, i_old in enumerate(rows): for j, j_old in enumerate(cols): J_reduced[i, j] = J[i_old, j_old] symo.file_open(robo, 'det') symo.write_params_table(robo, 'Jacobian determinant for frame %s' % n) symo.write_line(_jac_det(robo, symo, J=J_reduced)) symo.file_close() return symo
def velocities(robo): symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'vel') symo.write_params_table(robo, 'Link velocities') antRj, antPj = compute_rot_trans(robo, symo) w = ParamsInit.init_w(robo) v = ParamsInit.init_v(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j, forced=True) _v_j(robo, j, antPj, jRant, v, w, qdj) symo.mat_replace(v[j], 'V', j, forced=True) symo.file_close() return symo
def base_paremeters(robo_orig): """Computes grouped inertia parameters. New parametrization contains less parameters but generates the same dynamic model Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ robo = copy(robo_orig) lam = [0 for i in xrange(robo.NL)] symo = symbolmgr.SymbolManager() symo.file_open(robo, 'regp') title = 'Base parameters computation' symo.write_params_table(robo, title, inert=True, dynam=True) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in reversed(xrange(1, robo.NL)): if robo.sigma[j] == 0: # general grouping compute_lambda(robo, symo, j, antRj, antPj, lam) group_param_rot(robo, symo, j, lam) # special grouping group_param_rot_spec(robo, symo, j, lam, antRj) pass elif robo.sigma[j] == 1: # general grouping group_param_prism(robo, symo, j, antRj) # special grouping group_param_prism_spec(robo, symo, j, antRj, antPj) pass elif robo.sigma[j] == 2: # fixed joint, group everuthing compute_lambda(robo, symo, j, antRj, antPj) group_param_fix(robo, symo, j, lam) pass symo.write_line('*=*') symo.write_line() title = robo.name + ' grouped inertia parameters' symo.write_params_table(robo, title, inert=True, equations=False) symo.file_close() return robo, symo.sydi
def compute_ddym(self): """ Compute the Direct Dynamic Model of the robot using the recursive Newton-Euler algorithm. """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'ddm') title = "Direct Dynamic Model using Newton-Euler Algorithm\n" if self.is_floating: # with floating base title = title + "Robot with floating base\n" else: # with fixed base title = title + "Robot with fixed base\n" symo.write_params_table(self, title, inert=True, dynam=True) nealgos.direct_dynmodel(self, symo) symo.file_close() return symo
def inertia_matrix(robo): """Computes Inertia Matrix using composed link Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ Jplus, MSplus, Mplus = ParamsInit.init_jplus(robo) AJE1 = ParamsInit.init_vec(robo) f = ParamsInit.init_vec(robo, ext=1) n = ParamsInit.init_vec(robo, ext=1) A = sympy.zeros(robo.NL, robo.NL) symo = symbolmgr.SymbolManager() symo.file_open(robo, 'inm') title = 'Inertia Matrix using composite links' symo.write_params_table(robo, title, inert=True, dynam=True) # init transformation antRj, antPj = compute_rot_trans(robo, symo) for j in reversed(xrange(-1, robo.NL)): replace_Jplus(robo, symo, j, Jplus, MSplus, Mplus) if j != -1: compute_Jplus(robo, symo, j, antRj, antPj, Jplus, MSplus, Mplus, AJE1) for j in xrange(1, robo.NL): compute_A_diagonal(robo, symo, j, Jplus, MSplus, Mplus, f, n, A) ka = j while ka != -1: k = ka ka = robo.ant[ka] compute_A_triangle(robo, symo, j, k, ka, antRj, antPj, f, n, A, AJE1) symo.mat_replace(A, 'A', forced=True, symmet=True) J_base = inertia_spatial(Jplus[-1], MSplus[-1], Mplus[-1]) symo.mat_replace(J_base, 'JP', 0, forced=True, symmet=True) symo.file_close() return symo
def compute_baseparams(self): """ Compute the Base Inertial Parameters of the robot. """ base_robo = copy.deepcopy(self) symo = symbolmgr.SymbolManager() symo.file_open(base_robo, 'regp') title = "Base Inertial Parameters equations" symo.write_params_table(base_robo, title, inert=True, dynam=True) # compute base inertial params baseparams.base_inertial_parameters(base_robo, symo) symo.write_line() title = "Grouped inertial parameters" symo.write_params_table(base_robo, title, inert=True, equations=False) symo.file_close() # set new name for robot with base params base_robo.name = base_robo.name + "_base" file_path = filemgr.get_file_path(base_robo) base_robo.set_par_file_path(file_path) return symo, base_robo
def compute_inertiamatrix(self): """ Compute the Inertia Matrix of the robot using the Composite link algorithm. """ symo = symbolmgr.SymbolManager() symo.file_open(self, 'inm') title = "Inertia matrix using Composite links algorithm\n" if self.is_floating or self.is_mobile: # with floating or mobile base title = title + "Robot with floating/mobile base\n" symo.write_params_table(self, title, inert=True, dynam=True) inertia.floating_inertia_matrix(self, symo) else: # with fixed base title = title + "Robot with fixed base\n" symo.write_params_table(self, title, inert=True, dynam=True) inertia.fixed_inertia_matrix(self, symo) symo.file_close() return symo
def jdot_qdot(robo): symo = symbolmgr.SymbolManager(None) symo.file_open(robo, 'jpqp') symo.write_params_table(robo, 'JdotQdot') antRj, antPj = compute_rot_trans(robo, symo) w = ParamsInit.init_w(robo) wdot, vdot = ParamsInit.init_wv_dot(robo, gravity=False) U = ParamsInit.init_u(robo) for j in xrange(1, robo.NL): jRant = antRj[j].T qdj = Z_AXIS * robo.qdot[j] qddj = Z_AXIS * tools.ZERO wi, w[j] = _omega_ij(robo, j, jRant, w, qdj) symo.mat_replace(w[j], 'W', j) symo.mat_replace(wi, 'WI', j) _omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj) symo.mat_replace(wdot[j], 'WPJ', j, forced=True) _v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj) symo.mat_replace(vdot[j], 'VPJ', j, forced=True) symo.file_close() return symo
def inverse_dynamic_NE(robo): """Computes Inverse Dynamic Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ symo = symbolmgr.SymbolManager() symo.file_open(robo, 'idm') title = 'Inverse dynamic model using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) Newton_Euler(robo, symo) symo.file_close() return symo
def pseudo_force_NE(robo): """Computes Coriolis, Centrifugal, Gravity, Friction and external torques using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ robo_pseudo = deepcopy(robo) robo_pseudo.qddot = sympy.zeros(robo_pseudo.NL, 1) symo = symbolmgr.SymbolManager() symo.file_open(robo, 'ccg') title = 'Pseudo forces using Newton - Euler Algorith' symo.write_params_table(robo, title, inert=True, dynam=True) Newton_Euler(robo_pseudo, symo) symo.file_close() return symo
def direct_geometric_fast(robo, i, j): """Computes trensformation matrix iTj. Parameters ========== robo: Robot Instance of robot description container i: int the to-frame j: int the from-frame Returns ======= symo: symbolmgr.SymbolManager Instance that contains all the relations of the computed model """ symo = symbolmgr.SymbolManager() symo.file_open(robo, 'fgm') symo.write_params_table(robo, 'Direct Geometric model') dgm(robo, symo, i, j, fast_form=True, forced=True) symo.file_close() return symo
def dynamic_identification_NE(robo): """Computes Dynamic Identification Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container Returns ======= symo.sydi : dictionary Dictionary with the information of all the sybstitution """ # init forces vectors Fjnt = ParamsInit.init_vec(robo) Njnt = ParamsInit.init_vec(robo) # init file output, writing the robot description symo = symbolmgr.SymbolManager() symo.file_open(robo, 'dim') title = "Dynamic identification model using Newton - Euler Algorith" symo.write_params_table(robo, title, inert=True, dynam=True) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) # virtual robot with only one non-zero parameter at once robo_tmp = deepcopy(robo) robo_tmp.IA = sympy.zeros(robo.NL, 1) robo_tmp.FV = sympy.zeros(robo.NL, 1) robo_tmp.FS = sympy.zeros(robo.NL, 1) for k in xrange(1, robo.NL): param_vec = robo.get_inert_param(k) F = ParamsInit.init_vec(robo) N = ParamsInit.init_vec(robo) for i in xrange(10): if param_vec[i] == tools.ZERO: continue # change link names according to current non-zero parameter robo_tmp.num = [str(l) + str(param_vec[i]) for l in xrange(k + 1)] # set the parameter to 1 mask = sympy.zeros(10, 1) mask[i] = 1 robo_tmp.put_inert_param(mask, k) # compute the total forcec of the link k compute_wrench(robo_tmp, symo, k, w, wdot, U, vdot, F, N) # init external forces Fex = copy(robo.Fex) Nex = copy(robo.Nex) for j in reversed(xrange(k + 1)): compute_joint_wrench(robo_tmp, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N, Fex, Nex) for j in xrange(k + 1): compute_torque(robo_tmp, symo, j, Fjnt, Njnt, 'DG') # reset all the parameters to zero robo_tmp.put_inert_param(sympy.zeros(10, 1), k) # compute model for the joint parameters compute_joint_torque_deriv(symo, robo.IA[k], robo.qddot[k], k) compute_joint_torque_deriv(symo, robo.FS[k], sympy.sign(robo.qdot[k]), k) compute_joint_torque_deriv(symo, robo.FV[k], robo.qdot[k], k) # closing the output file symo.file_close() return symo
def setUp(self): self.symo = symbolmgr.SymbolManager()
def setUp(self): self.symo = symbolmgr.SymbolManager() self.robo = samplerobots.rx90()