def get_dp_bc_params(state):
dp_bc_xy = Matrix(lam_bc_com(state[1], state[2]))
dp_bc_xy_inertial = Matrix(lam_bc_com_inertial(state[0], state[1], state[2]))
dp_bc_length = (dp_bc_xy[0]**2 + dp_bc_xy[1]**2)**0.5
dp_bc_angle = -atan(dp_bc_xy[0]/dp_bc_xy[1]) #angle with respect to angle one
dp_bc_vel_xy = Matrix(lam_bc_com_dot(state[1], state[2], state[4], state[5]))
dp_bc_ang_vel = (dp_bc_xy.cross(dp_bc_vel_xy))/norm(dp_bc_xy)**2
dp_bc_params_dict = dict(zip(['bc_length', 'bc_theta', 'bc_ang_vel', 'bc_loc', 'bc_vel_xy'], [dp_bc_length, dp_bc_angle, dp_bc_ang_vel[2], dp_bc_xy_inertial, dp_bc_vel_xy]))
return dp_bc_params_dict
def get_sp_des_xy_acc(params, state):
com_vel_xy = Matrix(lam_com_dot_eq(state[0], state[1], state[2], state[3]))
com_ang_vel = (params.get('sp_com_xy').cross(com_vel_xy))/(norm(params.get('sp_com_xy')))**2
com_ang_vel = com_ang_vel[2]
com_u = -k_p*params.get('sp_theta') + -k_v*com_ang_vel
com_ang_acc = Matrix([0,0,lam_sp_eq_torque_only(params.get('sp_length'), com_u)])
com_ang_acc = com_ang_acc[2]
com_ang_acc_xy = Matrix([lam_sp_com_x(params.get('sp_length'), params.get('sp_theta'), com_ang_vel, com_ang_acc), lam_sp_com_y(params.get('sp_length'), params.get('sp_theta'), com_ang_vel, com_ang_acc)])
com_ang_acc_dict = dict(zip(['ang_acc_x', 'ang_acc_y', 'ang_acc'], [com_ang_acc_xy[0], com_ang_acc_xy[1], com_ang_acc]))
return com_ang_acc_dict
def get_sp_des_xy_acc(params, state): #returns desired com x and y acceleration based off of PD control of the center of mass pendulum
global k_p, k_v
com_vel_xy = Matrix(lam_com_dot_eq(state[0], state[1], state[2], state[3], state[4], state[5]))
com_ang_vel = (params.get('sp_com_xy').cross(com_vel_xy))/(norm(params.get('sp_com_xy')))**2
com_ang_vel = com_ang_vel[2]
com_u = -k_p*params.get('sp_theta') + -k_v*com_ang_vel #pd torque control
# com_ang_acc = Matrix([0,0, lam_sp_eq(params.get('sp_length'), params.get('sp_theta'), com_u)]) #angular acceleration resulting from pd torque control
com_ang_acc = Matrix([0,0, lam_sp_eq_torque_only(params.get('sp_length'), com_u)])
com_ang_acc_xy = (com_ang_acc.cross(params.get('sp_com_xy')))
com_ang_acc_dict = dict(zip(['ang_acc_x', 'ang_acc_y'], [com_ang_acc_xy[0], com_ang_acc_xy[1]]))
# com_ang_acc_dict = dict(zip(['ang_acc_x', 'ang_acc_y'], [com_ang_acc_xy[0], 0.0]))
return com_ang_acc_dict
def test_mev():
output("""\
reim:{$[0>type x;1 0*x;2=count x;x;'`]};
mc:{((x[0]*y 0)-x[1]*y 1;(x[0]*y 1)+x[1]*y 0)};
mmc:{((.qml.mm[x 0]y 0)-.qml.mm[x 1]y 1;(.qml.mm[x 0]y 1)+.qml.mm[x 1]y 0)};
mev_:{[b;x]
if[2<>count wv:.qml.mev x;'`length];
if[not all over prec>=abs
mmc[flip vc;flip(flip')(reim'')flip x]-
flip(w:reim'[wv 0])mc'vc:(flip')(reim'')(v:wv 1);'`check];
/ Normalize sign; LAPACK already normalized to real
v*:1-2*0>{x a?max a:abs x}each vc[;0];
(?'[prec>=abs w[;1];w[;0];w];?'[b;v;0n])};""")
for A in eigenvalue_subjects:
if A.rows <= 3:
V = []
for w, n, r in A.eigenvects():
w = sp.simplify(sp.expand_complex(w))
if len(r) == 1:
r = r[0]
r = sp.simplify(sp.expand_complex(r))
r = r.normalized() / sp.sign(max(r, key=abs))
r = sp.simplify(sp.expand_complex(r))
else:
r = None
V.extend([(w, r)]*n)
V.sort(key=lambda (x, _): (-abs(x), -sp.im(x)))
else:
Am = mp.matrix(A)
# extra precision for complex pairs to be equal in sort
with mp.extradps(mp.mp.dps):
W, R = mp.eig(Am)
V = []
for w, r in zip(W, (R.column(i) for i in range(R.cols))):
w = mp.chop(w)
with mp.extradps(mp.mp.dps):
_, S, _ = mp.svd(Am - w*mp.eye(A.rows))
if sum(x == 0 for x in mp.chop(S)) == 1:
# nullity 1, so normalized eigenvector is unique
r /= mp.norm(r) * mp.sign(max(r, key=abs))
r = mp.chop(r)
else:
r = None
V.append((w, r))
V.sort(key=lambda (x, _): (-abs(x), -x.imag))
W, R = zip(*V)
test("mev_[%sb" % "".join("0" if r is None else "1" for r in R), A,
(W, [r if r is None else list(r) for r in R]), complex_pair=True)
def controller(x, t):
global torque_vector, time_vector, x_vector
returnval = [0., 0., 0.]
global_sp_params = get_global_com_params(x) #convert from triple pendulum to single pendulum
desired_global_sp_acc_xy = get_sp_des_xy_acc(global_sp_params, x) #calculates desired single pendulum acceleration
dp_bc_params = get_dp_bc_params(x)
dp_two_params = dict(zip(['length', 'mass', 'angle', 'ang_vel'],
[dp_bc_params.get('bc_length'),
parameter_dict.get(tp.b_mass) + parameter_dict.get(tp.c_mass),
x[1] + x[2],
dp_bc_params.get('bc_ang_vel')])) #params for link 2 of dp1 model
dp_one_params = dict(zip(['length', 'mass', 'angle', 'ang_vel'],
[parameter_dict.get(tp.a_length),
parameter_dict.get(tp.a_mass),
x[0],
x[3]])) #params for link 1 of dp1 model
dp_angular = get_dp_angular_from_com_xy_acc(dp_one_params, dp_two_params, desired_global_sp_acc_xy) #yields desired a1 and a2 for dp1 based on desired single pendulum acceleration
dp_desired_torques = dp_inverse_dynamics(dp_one_params, dp_two_params, dp_angular) #torque_a set to that required to produce desired dp acceleration by inverse dynamics
returnval[0] = dp_desired_torques.get('one_torque')
dp2_com_ang_acc = (dp_bc_params.get('bc_loc').cross(Matrix([0,0,dp_angular.get('theta2_acc')])))/(norm(dp_bc_params.get('bc_loc'))**2)
dp2_com_ang_acc = dict(zip(['ang_acc_x', 'ang_acc_y'], [dp2_com_ang_acc[0], dp2_com_ang_acc[1]]))
dp_one_params = dict(zip(['length', 'mass', 'angle', 'ang_vel'],
[parameter_dict.get(tp.b_length),
parameter_dict.get(tp.b_mass),
x[0] + x[1],
x[3] + x[4]]))
dp_two_params = dict(zip(['length', 'mass', 'angle', 'ang_vel'],
[parameter_dict.get(tp.c_length),
parameter_dict.get(tp.c_mass),
x[2],
x[5]]))
dp_angular = get_dp_angular_from_com_xy_acc(dp_one_params, dp_two_params, dp2_com_ang_acc)
dp_desired_torques = dp_inverse_dynamics(dp_one_params, dp_two_params, dp_angular)
returnval[1] = dp_desired_torques.get('one_torque')
returnval[2] = dp_desired_torques.get('two_torque')
if(returnval[0] > 10.0):
returnval[0] = 10.0
if(returnval[0] < -10.0):
returnval[0] = -10.0
if(returnval[1] > 10.0):
returnval[1] = 10.0
if(returnval[1] < -10.0):
returnval[1] = -10.0
if(returnval[2] > 10.0):
returnval[2] = 10.0
if(returnval[2] < -10.0):
returnval[2] = -10.0
torque_vector.append(returnval)
time_vector.append(t)
x_vector.append(x)
return returnval
def controller(x, t):
global torque_vector, time_vector, x_vector, com_vector
x_vector.append(x)
# print(x)
returnval = [0., 0., 0.]
global_sp_params = get_global_com_params(x) #convert from triple pendulum to single pendulum
com_vector.append(global_sp_params.get('sp_com_xy'))
desired_global_sp_acc_xy = get_sp_des_xy_acc(global_sp_params, x) #calculates desired single pendulum acceleration
dp_bc_params = get_dp_bc_params(x)
dp_two_params = dict(zip(['length', 'com_x', 'com_y', 'com_x_dot', 'com_y_dot', 'mass', 'angle', 'ang_vel'],
[dp_bc_params.get('bc_length'),
dp_bc_params.get('bc_loc')[0],
dp_bc_params.get('bc_loc')[1],
dp_bc_params.get('bc_vel_xy')[0],
dp_bc_params.get('bc_vel_xy')[1],
parameter_dict.get(tp_b_mass) + parameter_dict.get(tp_c_mass),
x[1]+x[2],
dp_bc_params.get('bc_ang_vel')])) #params for link 2 of dp1 model
com_a = Matrix(lam_a_com(x[0]))
com_a_dot = Matrix(lam_a_com_dot(x[0], x[3]))
dp_one_params = dict(zip(['length', 'com_x', 'com_y', 'com_x_dot', 'com_y_dot', 'mass', 'angle', 'ang_vel'],
[parameter_dict.get(tp_a_length),
com_a[0],
com_a[1],
com_a_dot[0],
com_a_dot[1],
parameter_dict.get(tp_a_mass),
x[0],
x[3]])) #params for link 1 of dp1 model
rb_angular = get_pp_angular_from_com_xy_acc(dp_one_params, dp_two_params, desired_global_sp_acc_xy) #yields desired a1 and a2 for dp1 based on desired single pendulum acceleration
des_acc.append([rb_angular.get('theta1_acc'), rb_angular.get('theta2_acc')])
print([rb_angular.get('theta1_acc'), rb_angular.get('theta2_acc')])
rb_desired_torques = rb_inverse_dynamics(dp_one_params, dp_two_params, rb_angular) #torque_a set to that required to produce desired dp acceleration by inverse dynamics
returnval[0] = rb_desired_torques.get('one_torque')
dp2_com_ang_acc = (dp_bc_params.get('bc_loc').cross(Matrix([0,0, rb_angular.get('theta2_acc')])))/(norm(dp_bc_params.get('bc_loc'))**2)
dp2_com_ang_acc = dict(zip(['ang_acc_x', 'ang_acc_y'], [dp2_com_ang_acc[0], dp2_com_ang_acc[1]]))
dp_one_params = dict(zip(['length', 'mass', 'angle', 'ang_vel'],
[parameter_dict.get(tp_b_length),
parameter_dict.get(tp_b_mass),
x[0] + x[1],
x[4]]))
dp_two_params = dict(zip(['length', 'mass', 'angle', 'ang_vel'],
[parameter_dict.get(tp_c_length),
parameter_dict.get(tp_c_mass),
x[2],
x[5]]))
pp_angular = get_pp_angular_from_com_xy_acc(dp_one_params, dp_two_params, dp2_com_ang_acc)
pp_desired_torques = pp_inverse_dynamics(dp_one_params, dp_two_params, pp_angular)
returnval[1] = pp_desired_torques.get('one_torque')
returnval[2] = pp_desired_torques.get('two_torque')
if(returnval[0] > 20.0):
returnval[0] = 20.0
if(returnval[0] < -20.0):
returnval[0] = -20.0
if(returnval[1] > 20.0):
returnval[1] = 20.0
if(returnval[1] < -20.0):
returnval[1] = -20.0
if(returnval[2] > 20.0):
returnval[2] = 20.0
if(returnval[2] < -20.0):
returnval[2] = -20.0
# if t > 0.5 and t < 0.75:
# returnval[2] = returnval[2] + 0.1
# print(returnval)
torque_vector.append(returnval)
time_vector.append(t)
return returnval
