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: Symoro
Instance that contains all the relations of the computed model
"""
symo = Symoro()
symo.file_open(robo, 'trm')
symo.write_params_table(robo, 'Direct Geometrix model')
for i, j in frames:
symo.write_line('Tramsformation matrix %s T %s' % (i, j))
print dgm(robo, symo, i, j, fast_form=False,
forced=True, trig_subs=trig_subs)
symo.write_line()
symo.file_close()
return symo
def jacobian_determinant(robo, n, i, j, rows, cols):
symo = Symoro(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 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 = Symoro()
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 kinematic_constraints(robo):
symo = Symoro(None)
res = _kinematic_loop_constraints(robo, symo)
if res == FAIL:
return 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
