def _gen_params(self):
self.m = list(range(self.frame_num))
self.l = list(range(self.frame_num))
self.r = list(range(self.frame_num))
self.r_by_ml = list(range(self.frame_num))
self.L_vec = list(range(self.frame_num))
self.I_vec = list(range(self.frame_num))
self.L_mat = list(range(self.frame_num))
self.I_mat = list(range(self.frame_num))
self.I_by_Llm = list(range(self.frame_num))
self.Fc = list(range(self.frame_num))
self.Fv = list(range(self.frame_num))
self.Fo = list(range(self.frame_num))
self.Ia = list(range(self.frame_num))
self.K = list(range(len(self.springs)))
for num in self.link_nums[1:]:
self.m[num] = new_sym('m' + str(num))
self.l[num] = [
new_sym('l' + str(num) + dim) for dim in ['x', 'y', 'z']
]
self.r[num] = [
new_sym('r' + str(num) + dim) for dim in ['x', 'y', 'z']
]
self.I_vec[num] = [
new_sym('I' + str(num) + elem)
for elem in ['xx', 'xy', 'xz', 'yy', 'yz', 'zz']
]
self.L_vec[num] = [
new_sym('L' + str(num) + elem)
for elem in ['xx', 'xy', 'xz', 'yy', 'yz', 'zz']
]
self.I_mat[num] = inertia_vec2tensor(self.I_vec[num])
self.L_mat[num] = inertia_vec2tensor(self.L_vec[num])
self.r_by_ml[num] = ml2r(self.m[num], self.l[num])
self.I_by_Llm[num] = Lmr2I(self.L_mat[num], self.m[num],
self.r_by_ml[num])
if 'Coulomb' in self.friction_type:
self.Fc[num] = new_sym('Fc' + str(num))
if 'viscous' in self.friction_type:
self.Fv[num] = new_sym('Fv' + str(num))
if 'offset' in self.friction_type:
self.Fo[num] = new_sym('Fo' + str(num))
if self.use_Ia[num]:
self.Ia[num] = new_sym('Ia' + str(num))
self.spring_num = len(self.springs)
for i in range(self.spring_num):
self.K[i] = new_sym('K' + str(i))
vprint(self.m)
vprint(self.l)
vprint(self.r)
vprint(self.I_vec, self.L_vec)
vprint(self.K)
def barycentric2standard_params(x, rbt_def, Rs=None):
i = 0
i_link = 1
x_out = []
while i_link < rbt_def.frame_num:
if rbt_def.use_inertia[i_link]:
m = x[i + 9]
rx, ry, rz = x[i + 6] / m, x[i + 7] / m, x[i + 8] / m
r = [rx, ry, rz]
L_mat = inertia_vec2tensor(x[i: i + 6])
I_vec = inertia_tensor2vec(Lmr2I(L_mat, m, r))
if Rs:
R = Rs[i_link - 1]
RI = np.matmul(R, np.matrix(inertia_vec2tensor(I_vec)).astype(np.float64))
I_vec = inertia_tensor2vec(np.matmul(RI, R.transpose()))
I_vec = np.array(I_vec).astype(np.float64).tolist()
r = np.matmul(R, np.array(r).transpose()).tolist()[0]
x_out += I_vec + r + [m]
i += 10
if rbt_def.use_friction[i_link]:
if 'Coulomb' in rbt_def.friction_type:
x_out += [x[i]]
i += 1
if 'viscous' in rbt_def.friction_type:
x_out += [x[i]]
i += 1
if 'offset' in rbt_def.friction_type:
x_out += [x[i]]
i += 1
if rbt_def.use_Ia[i_link]:
x_out += [x[i]]
i += 1
if rbt_def.spring_dl[i_link] != None:
x_out += [x[i]]
i += 1
i_link += 1
return x_out
def barycentric2standard_params(x, rbt_def):
i = 0
i_link = 1
x_out = []
while i_link < rbt_def.frame_num:
if rbt_def.use_inertia[i_link]:
m = x[i + 9]
rx, ry, rz = x[i + 6] / m, x[i + 7] / m, x[i + 8] / m
r = [rx, ry, rz]
L_mat = inertia_vec2tensor(x[i:i + 6])
I_vec = inertia_tensor2vec(Lmr2I(L_mat, m, r))
#print(I_vec, r, m)
x_out += I_vec + r + [m]
i += 10
if rbt_def.use_friction[i_link]:
if 'Coulomb' in rbt_def.friction_type:
x_out += [x[i]]
i += 1
if 'viscous' in rbt_def.friction_type:
x_out += [x[i]]
i += 1
if 'offset' in rbt_def.friction_type:
x_out += [x[i]]
i += 1
if rbt_def.use_Ia[i_link]:
x_out += [x[i]]
i += 1
i_link += 1
for j in range(rbt_def.spring_num):
x_out += [x[i]]
i += 1
return x_out