def _compute_alpha_wrench(model, robo, j):
"""
Compute the wrench as a function of tau - joint torque without
friction.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: joint number
Returns:
An instance of DynModel that contains all the new values.
"""
j_alpha_j = Screw()
# local variables
j_a_j = robo.geos[j].axisa
j_k_j = model.no_qddot_inertias[j].val
j_gamma_j = model.gammas[j].val
j_inertia_j_s = model.star_inertias[j].val
j_beta_j_s = model.star_betas[j].val
h_j = Matrix([model.joint_inertias[j]])
tau_j = Matrix([model.taus[j]])
# actual computation
j_alpha_j.val = (j_k_j * j_gamma_j) + \
(j_inertia_j_s * j_a_j * h_j.inv() * \
(tau_j + j_a_j.transpose() * j_beta_j_s)) - j_beta_j_s
# store in model
model.alphas[j] = j_alpha_j
return model
def _compute_base_acceleration(model, robo):
"""
Compute the base acceleration for a robot with floating base without
and with taking gravity into account. In the case of a robot with
fixed base, this function returns just the effect of gravity as the
base acceleration.
Args:
model: An instance of DynModel
robo: An instance of Robot
Returns:
An instance of DynModel that contains all the new values.
"""
o_vdot_o = Screw()
gravity = Screw()
# local variables
gravity.lin = robo.gravity
if robo.is_floating:
if model.model_type is 'inverse':
o_inertia_o_c = model.composite_inertias[0].val
o_beta_o_c = model.composite_betas[0].val
elif model.model_type is 'direct':
o_inertia_o_c = model.star_inertias[0].val
o_beta_o_c = model.star_betas[0].val
# actual computation
# TODO: replace sympy's matrix inversion with custom function
o_vdot_o.val = o_inertia_o_c.inv() * o_beta_o_c
# store computed base acceleration without gravity effect in model
model.base_accel_w_gravity = copy.copy(o_vdot_o)
# compute base acceleration removing gravity effect
o_vdot_o.val = o_vdot_o.val - gravity.val
# store in model
model.accels[0] = o_vdot_o
return model
def _compute_composite_beta(model, robo, j, i):
"""
Compute the composite beta wrench for link i.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
i_beta_i_c = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
i_beta_i = model.composite_betas[i].val
j_beta_j_c = model.composite_betas[j].val
j_inertia_j_c = model.composite_inertias[j].val
j_zeta_j = model.zetas[j].val
# actual computation
i_beta_i_c.val = i_beta_i + (j_s_i.transpose() * j_beta_j_c) - \
(j_s_i.transpose() * j_inertia_j_c * j_zeta_j)
# store computed beta in model
model.composite_betas[i] = i_beta_i_c
return model
def _compute_relative_acceleration(model, robo, j):
"""
Compute the relative acceleration of link j.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
Returns:
An instance of DynModel that contains all the new values.
"""
j_zeta_j = Screw()
# local variables
j_a_j = robo.geos[j].axisa
j_gamma_j = model.gammas[j].val
if model.model_type is 'inverse':
qddot_j = robo.qddots[j]
elif model.model_type is 'direct':
qddot_j = model.qddots[j]
# actual computation
j_zeta_j.val = j_gamma_j + (qddot_j * j_a_j)
# store computed relative acceleration in model
model.zetas[j] = j_zeta_j
return model
def _compute_beta_wrench(model, robo, j):
"""
Compute the wrench for link j which combines the external forces,
Coriolis forces and centrifugal forces.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
Returns:
An instance of DynModel that contains all the new values.
"""
j_beta_j = Screw()
# local variables
j_omega_j = model.vels[j].ang
j_fe_j = robo.dyns[j].wrench.val
j_ms_j = robo.dyns[j].mass_tensor
j_inertia_j = robo.dyns[j].inertia
# actual computation
# lin_term = j_omega_j x (j_omega_j x j_ms_j)
lin_term = skew(j_omega_j) * (skew(j_omega_j) * j_ms_j)
# ang_term = j_omega_j x (j_inertia_j * j_omega_j)
ang_term = skew(j_omega_j) * (j_inertia_j * j_omega_j)
term = Screw(lin=lin_term, ang=ang_term)
j_beta_j.val = - j_fe_j - term.val
# store computed wrench in model
model.betas[j] = j_beta_j
return model
def _compute_gyroscopic_acceleration(model, robo, j, i):
"""
Compute the gyroscopic acceleration of link j.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecendent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_gamma_j = Screw()
# local variables
j_rot_i = robo.geos[j].tmat.inv_rot
i_trans_j = robo.geos[j].tmat.trans
i_omega_i = model.vels[i].ang
sigma_j = robo.geos[j].sigma
sigma_dash_j = 1 - sigma_j
j_z_j = robo.geos[j].zunit
qdot_j = robo.qdots[j]
# actual computation
j_omega_i = j_rot_i * i_omega_i
# term1 = i_omega_i x (i_omega_i x i_trans_j)
term1 = skew(i_omega_i) * (skew(i_omega_i) * i_trans_j)
# term2 = j_omega_i x (qdot_j * j_z_j)
term2 = skew(j_omega_i) * (qdot_j * j_z_j)
j_gamma_j.lin = (j_rot_i * term1) + (2 * sigma_j * term2)
j_gamma_j.ang = sigma_dash_j * term2
# store computed acceleration in model
model.gammas[j] = j_gamma_j
return model
def _compute_link_velocity(model, robo, j, i):
"""
Compute the velocity of link j whose antecedent is i.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecendent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_v_j = Screw()
if i == 0: model.vels[i] = robo.base_vel
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
qdot_j = robo.qdots[j]
j_a_j = robo.geos[j].axisa
i_v_i = model.vels[i].val
# actual computation
j_v_j.val = (j_s_i * i_v_i) + (qdot_j * j_a_j)
# store computed velocity in model
model.vels[j] = j_v_j
return model
def _compute_link_velocity(model, robo, j, i):
"""
Compute the velocity of link j whose antecedent is i.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecendent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_v_j = Screw()
if i == 0: model.vels[i] = robo.base_vel
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
qdot_j = robo.qdots[j]
j_a_j = robo.geos[j].axisa
i_v_i = model.vels[i].val
# actual computation
j_v_j.val = (j_s_i * i_v_i) + (qdot_j * j_a_j)
# store computed velocity in model
model.vels[j] = j_v_j
return model
def test_equality(self):
"""Test __eq__() and __ne__()"""
self.assertEqual(self.empty, Screw())
self.assertNotEqual(self.indiv, Screw())
self.assertRaises(ValueError, self.empty.__eq__,
Matrix([1, 2, 3, 4, 5, 6, 7, 8]))
self.assertRaises(ValueError, self.empty.__ne__,
Matrix([1, 2, 3, 4, 5, 6, 7, 8]))
def _compute_base_acceleration(model, robo):
"""
Compute the base acceleration for a robot with floating base without
and with taking gravity into account. In the case of a robot with
fixed base, this function returns just the effect of gravity as the
base acceleration.
Args:
model: An instance of DynModel
robo: An instance of Robot
Returns:
An instance of DynModel that contains all the new values.
"""
o_vdot_o = Screw()
gravity = Screw()
# local variables
gravity.lin = robo.gravity
if robo.is_floating:
if model.model_type is 'inverse':
o_inertia_o_c = model.composite_inertias[0].val
o_beta_o_c = model.composite_betas[0].val
elif model.model_type is 'direct':
o_inertia_o_c = model.star_inertias[0].val
o_beta_o_c = model.star_betas[0].val
# actual computation
# TODO: replace sympy's matrix inversion with custom function
o_vdot_o.val = o_inertia_o_c.inv() * o_beta_o_c
# store computed base acceleration without gravity effect in model
model.base_accel_w_gravity = copy.copy(o_vdot_o)
# compute base acceleration removing gravity effect
o_vdot_o.val = o_vdot_o.val - gravity.val
# store in model
model.accels[0] = o_vdot_o
return model
def _compute_reaction_wrench(model, robo, j):
"""
Compute the reaction wrench for link j.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
Returns:
An instance of DynModel that contains all the new values.
"""
j_f_j = Screw()
# local variables
j_vdot_j = model.accels[j].val
j_inertia_j_c = model.composite_inertias[j].val
j_beta_j_c = model.composite_betas[j].val
# actual computation
j_f_j.val = (j_inertia_j_c * j_vdot_j) - j_beta_j_c
# store computed reaction wrench in model
model.wrenchs[j] = j_f_j
return model
def _compute_reaction_wrench(model, robo, j):
"""
Compute the reaction wrench for link j.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
Returns:
An instance of DynModel that contains all the new values.
"""
j_f_j = Screw()
# local variables
j_vdot_j = model.accels[j].val
j_inertia_j_c = model.composite_inertias[j].val
j_beta_j_c = model.composite_betas[j].val
# actual computation
j_f_j.val = (j_inertia_j_c * j_vdot_j) - j_beta_j_c
# store computed reaction wrench in model
model.wrenchs[j] = j_f_j
return model
def _compute_link_acceleration(model, robo, j, i):
"""
Compute the acceleration of link j whose antecedent is i.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecendent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_vdot_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
i_vdot_i = model.accels[i].val
j_zeta_j = model.zetas[j].val
# actual computation
j_vdot_j.val = (j_s_i * i_vdot_i) + j_zeta_j
# store computed velocity in model
model.accels[j] = j_vdot_j
return model
def _compute_link_acceleration(model, robo, j, i):
"""
Compute the acceleration of link j whose antecedent is i.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecendent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_vdot_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_i_wrt_j
i_vdot_i = model.accels[i].val
j_zeta_j = model.zetas[j].val
# actual computation
j_vdot_j.val = (j_s_i * i_vdot_i) + j_zeta_j
# store computed velocity in model
model.accels[j] = j_vdot_j
return model
def _compute_reaction_wrench_alpha(model, robo, j, i):
"""
Compute the reaction wrench for link j as a function of alpha wrench.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_f_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
j_k_j = model.no_qddot_inertias[j].val
j_alpha_j = model.alphas[j].val
i_vdot_i = model.accels[i].val
# actual computation
j_f_j.val = (j_k_j * j_s_i * i_vdot_i) + j_alpha_j
# store in model
model.wrenchs[j] = j_f_j
return model
def _compute_star_beta(model, robo, j, i):
"""
Compute the star beta wrench for link i. This is similar to the
composite beta wrench but is a function of `tau` instead of `qddot`.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
i_beta_i_s = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
i_beta_i = model.star_betas[i].val
j_alpha_j = model.alphas[j].val
# actual computation
i_beta_i_s.val = i_beta_i - (j_s_i.transpose() * j_alpha_j)
# store in model
model.star_betas[i] = i_beta_i_s
return model
def _compute_reaction_wrench_alpha(model, robo, j, i):
"""
Compute the reaction wrench for link j as a function of alpha wrench.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
j_f_j = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
j_k_j = model.no_qddot_inertias[j].val
j_alpha_j = model.alphas[j].val
i_vdot_i = model.accels[i].val
# actual computation
j_f_j.val = (j_k_j * j_s_i * i_vdot_i) + j_alpha_j
# store in model
model.wrenchs[j] = j_f_j
return model
def _compute_star_beta(model, robo, j, i):
"""
Compute the star beta wrench for link i. This is similar to the
composite beta wrench but is a function of `tau` instead of `qddot`.
Args:
model: An instance of DynModel
robo: An instance of Robot
j: link number
i: antecedent value
Returns:
An instance of DynModel that contains all the new values.
"""
i_beta_i_s = Screw()
# local variables
j_s_i = robo.geos[j].tmat.s_j_wrt_i
i_beta_i = model.star_betas[i].val
j_alpha_j = model.alphas[j].val
# actual computation
i_beta_i_s.val = i_beta_i - (j_s_i.transpose() * j_alpha_j)
# store in model
model.star_betas[i] = i_beta_i_s
return model
def setUp(self):
# screw with 0s
self.empty = Screw()
# screw created by separate linear and angular terms
self.indiv = Screw(lin=Matrix([1, 2, 3]), ang=Matrix([4, 5, 6]))
def wrench(self):
"""Get external force (6x1) column vector (linear + angular)."""
return Screw(lin=Matrix([self.fx_ext, self.fy_ext, self.fz_ext]),
ang=Matrix([self.mx_ext, self.my_ext, self.mz_ext]))
def __init__(
self, name, links=0, joints=0, frames=0,
is_floating=True, structure=tools.TREE, is_mobile=False,
is_symbolic=True
):
"""
Constructor period.
Usage:
"""
"""Name of the robot."""
self.name = name
"""Folder to store the files related to the robot."""
self.directory = filemgr.get_folder_path(name)
"""Total number of links in the robot."""
self.num_links = links
"""Total number of joints in the robot."""
self.num_joints = joints
"""Total number of frames in the robot."""
self.num_frames = frames
"""To indicate if the base is floating or not."""
self.is_floating = is_floating
"""Type of the robot structure - simple, tree, closed-loop"""
self.structure = structure
"""To indicate if the robot is a wheeled mobile robot"""
self.is_mobile = is_mobile
"""To indicate if computation should be symbolic or numeric"""
self.is_symbolic = is_symbolic
# properties dependent on number of links
"""
List to hold the dynamic parameters. The indices of the list
start with 0 and it corresponds to parameters of link 0 (virtual
link of the base).
"""
self.dyns = [DynParams(j) for j in self.link_nums]
# properties dependent on number of joints
"""
To indicate if a joint is rigid or flexible. 0 for rigid and 1
for flexible. The indices of the list start with 0 and
corresponds to a virtual joint of the base. This joint is
usually rigid.
"""
self.etas = [0 for j in self.joint_nums]
"""Joint stiffness usually indicated by k."""
self.stiffness = [0 for j in self.joint_nums]
"""Joint velocities."""
self.qdots = [var('QP{0}'.format(j)) for j in self.joint_nums]
"""Joint accelerations."""
self.qddots = [var('QDP{0}'.format(j)) for j in self.joint_nums]
"""Joint torques."""
self.torques = [var('GAM{0}'.format(j)) for j in self.joint_nums]
# properties dependent on number of frames
"""
List to hold the geometric parameters. NOTE: This might be moved
to a separate function.
The indices of the list start with 0 and the first object
corresponds to parameters of frame 0 (base) wrt its antecedent
(some arbitary reference frame).
"""
self.geos = [GeoParams(j) for j in self.frame_nums]
# properties independent of number of links, joints and frames
"""Gravity vector a 3x1 Matrix."""
self.gravity = Matrix([0, 0, var('G3')])
# the values of properties below would be modified during
# the computation of dynamic models.
"""Base velocity 6x1 column vector - a Screw."""
self.base_vel = Screw()
"""Base acceleration 6x1 column vector - a Screw."""
self.base_accel = Screw()
"""Transformation matrix of base wrt a reference frame at time 0."""
self.base_tmat = eye(4)
# call init methods
self._init_maps()
