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 _gen_coordinates(self):
self.coordinates = []
self.coordinates_joint_type = []
# self.joint_coordinate = list(range(self.frame_num))
for num in self.link_nums:
for s in self.dh_T[num].free_symbols:
if s not in self.coordinates:
self.coordinates += [s]
self.coordinates_joint_type += [self.joint_type[num]]
self.dof = len(self.coordinates)
vprint(self.coordinates)
self.d_coordinates = [new_sym('d'+co.name) for co in self.coordinates]
self.dd_coordinates = [new_sym('dd' + co.name) for co in self.coordinates]
vprint(self.d_coordinates)
vprint(self.dd_coordinates)
self.coordinates_t = [dynamicsymbols(co.name+'t') for co in self.coordinates]
vprint(self.coordinates_t)
self.d_coordinates_t = [sympy.diff(co_t) for co_t in self.coordinates_t]
vprint(self.d_coordinates_t)
self.dd_coordinates_t = [sympy.diff(d_co_t) for d_co_t in self.d_coordinates_t]
vprint(self.dd_coordinates_t)
self.subs_q2qt = [(q, qt) for q, qt in zip(self.coordinates, self.coordinates_t)]
vprint(self.subs_q2qt)
self.subs_dq2dqt = [(dq, dqt) for dq, dqt in zip(self.d_coordinates, self.d_coordinates_t)]
vprint(self.subs_dq2dqt)
self.subs_ddq2ddqt = [(ddq, ddqt) for ddq, ddqt in zip(self.dd_coordinates, self.dd_coordinates_t)]
vprint(self.subs_ddq2ddqt)
self.subs_qt2q = [(qt, q) for q, qt in zip(self.coordinates, self.coordinates_t)]
vprint(self.subs_qt2q)
self.subs_dqt2dq = [(dqt, dq) for dq, dqt in zip(self.d_coordinates, self.d_coordinates_t)]
vprint(self.subs_dqt2dq)
self.subs_ddqt2ddq = [(ddqt, ddq) for ddq, ddqt in zip(self.dd_coordinates, self.dd_coordinates_t)]
vprint(self.subs_ddq2ddqt)
self.dq_for_frame = list(range(self.frame_num))
self.ddq_for_frame = list(range(self.frame_num))
for i in range(self.frame_num):
q = None
if self.joint_type[i] == "P":
q = self.dh_d[i]
elif self.joint_type[i] == "R":
q = self.dh_theta[i]
else:
continue
qt = q.subs(self.subs_q2qt)
dqt = sympy.diff(qt, sympy.Symbol('t'))
dq = dqt.subs(self.subs_dqt2dq)
self.dq_for_frame[i] = dq
ddqt = sympy.diff(dqt, sympy.Symbol('t'))
ddq = ddqt.subs(self.subs_ddqt2ddq)
self.ddq_for_frame[i] = ddq
import numpy as np
from robot_def import RobotDef
from kinematics import Geometry
from dynamics import Dynamics
from trajectory_optimization import TrajOptimizer
from trajectory_optimization import TrajPlotter
import time
from utils import new_sym
model_folder = 'data/model/'
trajectory_folder = 'data/trajectory/'
two_link_robot_model_file = model_folder + 'two_link_robot_model.pkl'
# Create joint variables and define their relations
q0, q1, q2, q3, q4, q5, q6, q7, q8, q9 = new_sym('q:10')
# q3 = -q2 + q8
# q9 = -q8 + q2
robot_def = RobotDef([(0, -1, [1], 0, 0, 0, 0),
(1, 0, [2], 0, 0, -0.21537, q1),
(2, 1, [3], 0, -sympy.pi / 2, 0, q2 + sympy.pi / 2)],
dh_convention='mdh',
friction_type=['Coulomb', 'viscous', 'offset'])
geom = Geometry(robot_def)
dyn = Dynamics(robot_def, geom)
#if not os.path.exists(two_link_robot_model_file):
with open(two_link_robot_model_file, 'wr') as f:
import sympy
from sympy.physics.vector import dynamicsymbols
import numpy as np
from utils import inertia_vec2tensor, ml2r, Lmr2I, new_sym
def new_sym(name):
return sympy.symbols(name, real=True)
_cos = sympy.cos
_sin = sympy.sin
_dh_alpha, _dh_a, _dh_d, _dh_theta = new_sym('alpha,a,d,theta')
default_dh_symbols = (_dh_alpha, _dh_a, _dh_d, _dh_theta)
_standard_dh_transfmat = sympy.Matrix(
[[
_cos(_dh_theta), -_sin(_dh_theta) * _cos(_dh_alpha),
_sin(_dh_theta) * _sin(_dh_alpha), _dh_a * _cos(_dh_theta)
],
[
_sin(_dh_theta),
_cos(_dh_theta) * _cos(_dh_alpha), -_cos(_dh_theta) * _sin(_dh_alpha),
_dh_a * _sin(_dh_theta)
], [0, _sin(_dh_alpha), _cos(_dh_alpha), _dh_d], [0, 0, 0, 1]])
_modified_dh_transfmat = sympy.Matrix(
[[_cos(_dh_theta), -_sin(_dh_theta), 0, _dh_a],
[
_sin(_dh_theta) * _cos(_dh_alpha),
