def exercise_basic():
assert approx_equal(sum(fs.Xrotx(0.1)), 5.98001666111)
assert approx_equal(sum(fs.Xroty(0.2)), 5.92026631136)
assert approx_equal(sum(fs.Xrotz(0.3)), 5.8213459565)
assert approx_equal(sum(fs.Xrot((1, 2, 3, 4, 5, 6, 7, 8, 9))), 90)
assert approx_equal(sum(fs.Xtrans((1, 2, 3))), 6)
assert approx_equal(sum(fs.crm((1, 2, 3, 4, 5, 6))), 0)
assert approx_equal(sum(fs.crf((1, 2, 3, 4, 5, 6))), 0)
assert approx_equal(
sum(fs.mcI(m=1.234, c=matrix.col((1, 2, 3)), I=matrix.sym(sym_mat3=(2, 3, 4, 0.1, 0.2, 0.3)))), 21.306
)
def exercise_basic():
assert approx_equal(sum(fs.Xrotx(0.1)), 5.98001666111)
assert approx_equal(sum(fs.Xroty(0.2)), 5.92026631136)
assert approx_equal(sum(fs.Xrotz(0.3)), 5.8213459565)
assert approx_equal(sum(fs.Xrot((1, 2, 3, 4, 5, 6, 7, 8, 9))), 90)
assert approx_equal(sum(fs.Xtrans((1, 2, 3))), 6)
assert approx_equal(sum(fs.crm((1, 2, 3, 4, 5, 6))), 0)
assert approx_equal(sum(fs.crf((1, 2, 3, 4, 5, 6))), 0)
assert approx_equal(
sum(
fs.mcI(m=1.234,
c=matrix.col((1, 2, 3)),
I=matrix.sym(sym_mat3=(2, 3, 4, 0.1, 0.2, 0.3)))), 21.306)
def spatial_inertia_from_sites(sites,
mass=None,
center_of_mass=None,
alignment_T=None):
if (mass is None):
mass = len(sites)
if (center_of_mass is None):
center_of_mass = center_of_mass_from_sites(sites=sites)
inertia = inertia_from_sites(sites=sites, pivot=center_of_mass)
if (alignment_T is not None):
center_of_mass = alignment_T * center_of_mass
inertia = alignment_T.r * inertia * alignment_T.r.transpose()
return featherstone.mcI(m=mass, c=center_of_mass, I=inertia)
def spatial_inertia_from_sites(
sites,
mass=None,
center_of_mass=None,
alignment_T=None):
if (mass is None):
mass = len(sites)
if (center_of_mass is None):
center_of_mass = center_of_mass_from_sites(sites=sites)
inertia = inertia_from_sites(sites=sites, pivot=center_of_mass)
if (alignment_T is not None):
center_of_mass = alignment_T * center_of_mass
inertia = alignment_T.r * inertia * alignment_T.r.transpose()
return featherstone.mcI(m=mass, c=center_of_mass, I=inertia)
def __init__(model, m, I, J):
model.NB = 1
model.pitch = [J]
model.parent = [-1]
model.Xtree = [matrix.identity(n=6)]
model.I = [featherstone.mcI(m, (0, 0, 0), I)]
def __init__(model, m, I, J):
model.NB = 1
model.pitch = [J]
model.parent = [-1]
model.Xtree = [matrix.identity(n=6)]
model.I = [featherstone.mcI(m, (0, 0, 0), I)]