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)]