def compute_vel_acc(robo, symo, antRj, antPj, forced=False, gravity=True):
"""Internal function. Computes speeds and accelerations usitn
Parameters
==========
robo : Robot
Instance of robot description container
symo : Symoro
Instance of symbolic manager
"""
#init velocities and accelerations
w = Init.init_w(robo)
wdot, vdot = Init.init_wv_dot(robo, gravity)
#init auxilary matrix
U = Init.init_U(robo)
for j in xrange(1, robo.NL):
jRant = antRj[j].T
qdj = Z_AXIS * robo.qdot[j]
qddj = Z_AXIS * robo.qddot[j]
wi = _omega_i(robo, symo, j, jRant, w)
symo.mat_replace(wi, 'WI', j)
w[j] = _omega_j(robo, j, jRant, w, wi, qdj)
symo.mat_replace(w[j], 'W', j)
_omega_dot_j(robo, j, jRant, w, wi, wdot, qdj, qddj)
symo.mat_replace(wdot[j], 'WP', j, forced)
_v_dot_j(robo, symo, j, jRant, antPj, w, wi, wdot, U, vdot, qdj, qddj)
symo.mat_replace(vdot[j], 'VP', j, forced)
return w, wdot, vdot, U
def jdot_qdot(robo):
symo = Symoro(None)
symo.file_open(robo, 'jpqp')
symo.write_params_table(robo, 'JdotQdot')
antRj, antPj = compute_rot_trans(robo, symo)
w = Init.init_w(robo)
wdot, vdot = Init.init_wv_dot(robo, gravity=False)
U = Init.init_U(robo)
for j in xrange(1, robo.NL):
jRant = antRj[j].T
qdj = Z_AXIS * robo.qdot[j]
qddj = Z_AXIS * ZERO
wi = _omega_i(robo, symo, j, jRant, w)
symo.mat_replace(wi, 'WI', j)
w[j] = _omega_j(robo, j, jRant, w, wi, qdj)
symo.mat_replace(w[j], 'W', 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
