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