def __init__(O, sites, masses): O.number_of_sites = len(sites) mp = mass_points(sites=sites, masses=masses) O.sum_of_masses = mp.sum_of_masses() O.alignment = joint_lib.six_dof_alignment( center_of_mass=mp.center_of_mass()) O.i_spatial = mp.spatial_inertia(alignment_cb_0b=O.alignment.cb_0b) # qe = matrix.col((1, 0, 0, 0)) qr = matrix.col((0, 0, 0)) O.joint = joint_lib.six_dof(qe=qe, qr=qr) O.qd = O.joint.qd_zero
def __init__(O, sites, masses): O.number_of_sites = len(sites) mp = mass_points(sites=sites, masses=masses) O.sum_of_masses = mp.sum_of_masses() O.alignment = joint_lib.six_dof_alignment( center_of_mass=mp.center_of_mass()) O.i_spatial = mp.spatial_inertia(alignment_cb_0b=O.alignment.cb_0b) # qe = matrix.col((1,0,0,0)) qr = matrix.col((0,0,0)) O.joint = joint_lib.six_dof(qe=qe, qr=qr) O.qd = O.joint.qd_zero
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