def exercise_basic(): assert approx_equal(sum(spatial_lib.xrot((1, 2, 3, 4, 5, 6, 7, 8, 9))), 90) assert approx_equal(sum(spatial_lib.xtrans((1, 2, 3))), 6) assert approx_equal( sum( spatial_lib.cb_as_spatial_transform(cb=matrix.rt(((1, 2, 3, 4, 5, 6, 7, 8, 9), (1, 2, 3))))), 90) assert approx_equal(sum(spatial_lib.crm((1, 2, 3, 4, 5, 6))), 0) assert approx_equal(sum(spatial_lib.crf((1, 2, 3, 4, 5, 6))), 0) i_spatial = spatial_lib.mci(m=1.234, c=matrix.col((1, 2, 3)), i=matrix.sym(sym_mat3=(2, 3, 4, 0.1, 0.2, 0.3))) assert approx_equal(sum(i_spatial), 21.306) assert approx_equal( spatial_lib.kinetic_energy(i_spatial=i_spatial, v_spatial=matrix.col((1, 2, 3, 4, 5, 6))), 75.109) # mass_points = body_lib.mass_points(sites=matrix.col_list([ (0.949, 2.815, 5.189), (0.405, 3.954, 5.917), (0.779, 5.262, 5.227) ]), masses=[2.34, 3.56, 1.58]) assert approx_equal(mass_points.sum_of_masses(), 7.48) assert approx_equal(mass_points.center_of_mass(), [0.654181818182, 3.87397058824, 5.54350802139]) assert approx_equal(mass_points._sum_of_masses, 7.48) assert approx_equal( mass_points.inertia(pivot=matrix.col((0.9, -1.3, 0.4))), [ 404.7677928, 10.04129606, 10.09577652, 10.04129606, 199.7384559, -199.3511949, 10.09577652, -199.3511949, 206.8314171 ])
def exercise_basic(): assert approx_equal(sum(spatial_lib.xrot((1,2,3,4,5,6,7,8,9))), 90) assert approx_equal(sum(spatial_lib.xtrans((1,2,3))), 6) assert approx_equal( sum(spatial_lib.cb_as_spatial_transform( cb=matrix.rt(((1,2,3,4,5,6,7,8,9), (1,2,3))))), 90) assert approx_equal(sum(spatial_lib.crm((1,2,3,4,5,6))), 0) assert approx_equal(sum(spatial_lib.crf((1,2,3,4,5,6))), 0) i_spatial = spatial_lib.mci( m=1.234, c=matrix.col((1,2,3)), i=matrix.sym(sym_mat3=(2,3,4,0.1,0.2,0.3))) assert approx_equal(sum(i_spatial), 21.306) assert approx_equal(spatial_lib.kinetic_energy( i_spatial=i_spatial, v_spatial=matrix.col((1,2,3,4,5,6))), 75.109) # mass_points = body_lib.mass_points( sites=matrix.col_list([ (0.949, 2.815, 5.189), (0.405, 3.954, 5.917), (0.779, 5.262, 5.227)]), masses=[2.34, 3.56, 1.58]) assert approx_equal(mass_points.sum_of_masses(), 7.48) assert approx_equal(mass_points.center_of_mass(), [0.654181818182, 3.87397058824, 5.54350802139]) assert approx_equal(mass_points._sum_of_masses, 7.48) assert approx_equal( mass_points.inertia(pivot=matrix.col((0.9,-1.3,0.4))), [404.7677928, 10.04129606, 10.09577652, 10.04129606, 199.7384559, -199.3511949, 10.09577652, -199.3511949, 206.8314171])
def __init__(O): sites = matrix.col_list([(0.04, -0.16, 0.19), (0.10, -0.15, 0.18)]) mass_points = body_lib.mass_points(sites=sites, masses=[1.0, 1.0]) O.alignment = joint_lib.spherical_alignment( pivot=mass_points.center_of_mass()) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) qe = matrix.col((-0.50, -0.33, 0.67, -0.42)).normalize() O.joint = joint_lib.spherical(qe=qe) O.qd = matrix.col((0.12, -0.08, 0.11)) assert O.joint.get_linear_velocity(qd=O.qd) is None assert O.joint.new_linear_velocity(qd=None, value=None) is None O.parent = 0
def __init__(O, parent): pivot = matrix.col((0.779, 5.262, 5.227)) normal = matrix.col((0.25, 0.86, -0.45)).normalize() sites = matrix.col_list([(-0.084, 6.09, 4.936)]) mass_points = body_lib.mass_points(sites=sites, masses=[1.0]) O.alignment = joint_lib.revolute_alignment(pivot=pivot, normal=normal) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) O.joint = joint_lib.revolute(qe=matrix.col([0.26])) O.qd = matrix.col([-0.19]) assert O.joint.get_linear_velocity(qd=O.qd) is None assert O.joint.new_linear_velocity(qd=None, value=None) is None O.parent = parent
def __init__(O, parent): pivot = matrix.col((0.779, 5.262, 5.227)) normal = matrix.col((0.25, 0.86, -0.45)).normalize() sites = matrix.col_list([(-0.084, 6.09, 4.936)]) mass_points = body_lib.mass_points(sites=sites, masses=[1.0]) O.alignment = joint_lib.revolute_alignment(pivot=pivot, normal=normal) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) O.joint = joint_lib.revolute(qe=matrix.col([0.26])) O.qd = matrix.col([-0.19]) assert O.joint.get_linear_velocity(qd=O.qd) is None assert O.joint.new_linear_velocity(qd=None, value=None) is None O.parent = parent
def __init__(O): sites = [matrix.col((0.949, 2.815, 5.189))] mass_points = body_lib.mass_points(sites=sites, masses=[1.0]) O.alignment = joint_lib.translational_alignment( center_of_mass=mass_points.center_of_mass()) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) qr = matrix.col((-0.1, 0.3, 0.2)) O.joint = joint_lib.translational(qr=qr) O.qd = matrix.col((0.05, 0.19, -0.29)) assert O.joint.get_linear_velocity(qd=O.qd).elems == O.qd.elems O.qd = O.joint.new_linear_velocity(qd=O.qd, value=-O.qd) assert O.joint.get_linear_velocity(qd=O.qd).elems == O.qd.elems O.parent = -1
def __init__(O): sites = [matrix.col((0.949, 2.815, 5.189))] mass_points = body_lib.mass_points(sites=sites, masses=[1.0]) O.alignment = joint_lib.translational_alignment( center_of_mass=mass_points.center_of_mass()) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) qr = matrix.col((-0.1,0.3,0.2)) O.joint = joint_lib.translational(qr=qr) O.qd = matrix.col((0.05,0.19,-0.29)) assert O.joint.get_linear_velocity(qd=O.qd).elems == O.qd.elems O.qd = O.joint.new_linear_velocity(qd=O.qd, value=-O.qd) assert O.joint.get_linear_velocity(qd=O.qd).elems == O.qd.elems O.parent = -1
def __init__(O): sites = matrix.col_list([ (0.04, -0.16, 0.19), (0.10, -0.15, 0.18)]) mass_points = body_lib.mass_points(sites=sites, masses=[1.0, 1.0]) O.alignment = joint_lib.spherical_alignment( pivot=mass_points.center_of_mass()) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) qe = matrix.col((-0.50, -0.33, 0.67, -0.42)).normalize() O.joint = joint_lib.spherical(qe=qe) O.qd = matrix.col((0.12, -0.08, 0.11)) assert O.joint.get_linear_velocity(qd=O.qd) is None assert O.joint.new_linear_velocity(qd=None, value=None) is None O.parent = 0
def __init__(O): sites = matrix.col_list([ (0.949, 2.815, 5.189), (0.405, 3.954, 5.917), (0.779, 5.262, 5.227)]) mass_points = body_lib.mass_points(sites=sites, masses=[1.0, 1.0, 1.0]) O.alignment = joint_lib.six_dof_alignment( center_of_mass=mass_points.center_of_mass()) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) qe = matrix.col((0.18, 0.36, 0.54, -0.73)).normalize() qr = matrix.col((-0.1,0.3,0.2)) O.joint = joint_lib.six_dof(qe=qe, qr=qr) O.qd = matrix.col((0.18,-0.02,-0.16,0.05,0.19,-0.29)) assert O.joint.get_linear_velocity(qd=O.qd).elems == (0.05,0.19,-0.29) O.qd = O.joint.new_linear_velocity( qd=O.qd, value=matrix.col((-0.05,-0.19,0.29))) assert O.joint.get_linear_velocity(qd=O.qd).elems == (-0.05,-0.19,0.29) O.parent = -1
def __init__(O): sites = matrix.col_list([(0.949, 2.815, 5.189), (0.405, 3.954, 5.917), (0.779, 5.262, 5.227)]) mass_points = body_lib.mass_points(sites=sites, masses=[1.0, 1.0, 1.0]) O.alignment = joint_lib.six_dof_alignment( center_of_mass=mass_points.center_of_mass()) O.i_spatial = mass_points.spatial_inertia( alignment_cb_0b=O.alignment.cb_0b) qe = matrix.col((0.18, 0.36, 0.54, -0.73)).normalize() qr = matrix.col((-0.1, 0.3, 0.2)) O.joint = joint_lib.six_dof(qe=qe, qr=qr) O.qd = matrix.col((0.18, -0.02, -0.16, 0.05, 0.19, -0.29)) assert O.joint.get_linear_velocity(qd=O.qd).elems == (0.05, 0.19, -0.29) O.qd = O.joint.new_linear_velocity(qd=O.qd, value=matrix.col( (-0.05, -0.19, 0.29))) assert O.joint.get_linear_velocity(qd=O.qd).elems == (-0.05, -0.19, 0.29) O.parent = -1