def Newton_Euler(robo, symo): """Internal function. Computes Inverse Dynamic Model using Newton-Euler formulation Parameters ========== robo : Robot Instance of robot description container symo : symbolmgr.SymbolManager Instance of symbolic manager """ # init external forces Fex = copy(robo.Fex) Nex = copy(robo.Nex) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) # init forces vectors F = ParamsInit.init_vec(robo) N = ParamsInit.init_vec(robo) Fjnt = ParamsInit.init_vec(robo) Njnt = ParamsInit.init_vec(robo) for j in xrange(1, robo.NL): compute_wrench(robo, symo, j, w, wdot, U, vdot, F, N) for j in reversed(xrange(1, robo.NL)): compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, Fjnt, Njnt, F, N, Fex, Nex) for j in xrange(1, robo.NL): compute_torque(robo, symo, j, Fjnt, Njnt)
def mobile_inverse_dynmodel(robo, symo): """ Compute the Inverse Dynamic Model using Newton-Euler algorithm for mobile robots. Parameters: robo: Robot - instance of robot description container symo: symbolmgr.SymbolManager - instance of symbol manager """ # init external forces Fex = copy(robo.Fex) Nex = copy(robo.Nex) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) # init forces vectors F = ParamsInit.init_vec(robo) N = ParamsInit.init_vec(robo) Fjnt = ParamsInit.init_vec(robo) Njnt = ParamsInit.init_vec(robo) # init torque list torque = ParamsInit.init_scalar(robo) for j in xrange(0, robo.NL): compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N) for j in reversed(xrange(0, robo.NL)): compute_joint_wrench( robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex ) for j in xrange(1, robo.NL): compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque)
def mobile_inverse_dynmodel(robo, symo): """ Compute the Inverse Dynamic Model using Newton-Euler algorithm for mobile robots. Parameters: robo: Robot - instance of robot description container symo: symbolmgr.SymbolManager - instance of symbol manager """ # init external forces Fex = copy(robo.Fex) Nex = copy(robo.Nex) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc(robo, symo, antRj, antPj) # init forces vectors F = ParamsInit.init_vec(robo) N = ParamsInit.init_vec(robo) Fjnt = ParamsInit.init_vec(robo) Njnt = ParamsInit.init_vec(robo) # init torque list torque = ParamsInit.init_scalar(robo) for j in xrange(0, robo.NL): compute_dynamic_wrench(robo, symo, j, w, wdot, U, vdot, F, N) for j in reversed(xrange(0, robo.NL)): compute_joint_wrench(robo, symo, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex) for j in xrange(1, robo.NL): compute_joint_torque(robo, symo, j, Fjnt, Njnt, torque)
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 dynamic_identification_model(robo, symo): """ Compute the Dynamic Identification model of a robot using Newton-Euler algorithm. """ # init forces vectors Fjnt = ParamsInit.init_vec(robo) Njnt = ParamsInit.init_vec(robo) # init transformation antRj, antPj = compute_rot_trans(robo, symo) # init velocities and accelerations w, wdot, vdot, U = compute_vel_acc( robo, symo, antRj, antPj, floating=True ) # virtual robot with only one non-zero parameter at once robo_tmp = copy.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) # start link number is_fixed = False if robo.is_floating or robo.is_mobile else True start_link = 0 for k in xrange(start_link, 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 name = '{index}{element}' + str(param_vec[i]) # 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_dynamic_wrench( robo_tmp, symo, name, k, w, wdot, U, vdot, F, N ) # init external forces Fex = ParamsInit.init_vec(robo) Nex = ParamsInit.init_vec(robo) for j in reversed(xrange(1, k + 1)): _compute_reaction_wrench( robo_tmp, symo, name, j, antRj, antPj, vdot, F, N, Fjnt, Njnt, Fex, Nex ) # reaction wrench for base _compute_base_reaction_wrench( robo_tmp, symo, name, antRj,antPj, vdot, F, N, Fex, Nex, Fjnt, Njnt ) for j in xrange(1, k + 1): _compute_joint_torque(robo_tmp, symo, name, j, Fjnt, Njnt) # reset all the parameters to zero robo_tmp.put_inert_param(sympy.zeros(10, 1), k) # compute model for the joint parameters # avoid these parameters for link 0 if k == 0: continue _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 ) return symo