def get_joint_load(self, joint_id):
"""
calculate effective mass, center of mass,
and inertia tensor for a joint specified by jointID
"""
m_eff = 0
cm_eff = np.array([0, 0, 0])
iT_eff = np.zeros((3,3))
for body in self.robot.bodies[joint_id:]:
(m_eff, cm_eff, iT_eff) = mass_combine(m_eff, body.m,
cm_eff, body.cm_gl,
iT_eff, body.iT_gl)
return m_eff, cm_eff, iT_eff
def add_mass(self, cm_new, m_new, iT_new_ar, align, isLoad=False):
"""
add a new mass to body obj. Overall cm, iT, m are updated
Local Coordinate Sys
"""
iT_new = tensor(*iT_new_ar)
cm = self.cm
iT = self.iT
m = self.m
cm_new_aligned = align @ cm_new
if isLoad:
cm_new_aligned = cm_new_aligned + self.dimension
iT_new_aligned = align @ iT_new @ align.T
res = mass_combine(m, m_new, cm, cm_new_aligned, iT, iT_new_aligned)
(self.m, self.cm, self.iT) = res
def remove_mass(self, cm_target, m_target, iT_target_ar, align):
"""
add a new mass to body obj. Overall cm, iT, m are updated
Local Coordinate Sys
"""
iT_target = tensor(*iT_target_ar)
cm = self.cm
iT = self.iT
m = self.m
cm_target_aligned = align @ cm_target
cm_target_aligned = cm_target_aligned + self.dimension
iT_target_aligned = align @ iT_target @ align.T
res = mass_combine(-m, m_target, cm, cm_target_aligned, iT,
-iT_target_aligned)
(self.m, self.cm, self.iT) = res
def masspara_combine(filename, sheetname, max_row):
m = 0
cm = np.array([0, 0, 0])
it = tensor(*np.array([0, 0, 0, 0, 0, 0]))
table = load_workbook(filename)
sheet = table[sheetname]
for idx_r in range(15, max_row):
item = []
for idx_c in range(2, 13):
item.append(sheet.cell(row=idx_r + 1, column=idx_c + 1).value)
m_new = item[0]
cm_new = np.array(item[1:4]) * 1e-3
it_new = tensor(*(np.array(item[4:-1]) * 1e-6))
m, cm, it = mass_combine(m, m_new, cm, cm_new, it, it_new)
it = tear_tensor(it)
return m, cm, it