sum_q[1] = sl.sum_position_difference(sum_q[1], qd[1], q[1],
sampling_time)
sum_q[2] = sl.sum_position_difference(sum_q[2], qd[2], q[2],
sampling_time)
sum_q[3] = sl.sum_position_difference(sum_q[3], qd[3], q[3],
sampling_time)
sum_q = [sum_q[0], sum_q[1], sum_q[2], sum_q[3]]
Tau, actf, thetad = sl.PIDcontrol_eforce_base_3dof(
gain1, gain2, theta, dot_theta, Xd, position1, Jt, k, qd, sum_q, N,
force_gain, eps, Fconstant)
# 偏差と非線形弾性特性値の計算
e = sl.difference_part(theta, q, N)
K = sl.non_linear_item(k, e)
K_error = sl.non_linear_item(k_error, e)
# 拘束力とダイナミクス右辺の計算
dot_P, P, dot_Q, Q = sl.restraint_part(ll, q, dot_q, r0)
Fx = sl.out_force(10, time)
f, A, Tauff = sl.input_forces(ll, q, dot_q, H, D, K_error, Jt, P, Q,
dot_P, dot_Q, N, 0, 0, 10)
# 関節角加速度の計算
ddot_q = sl.angular_acceleration_3dof(invPhi, f, A)
# 拘束項
lam = sl.binding_force(invPhi, f, A)
position = [X, Y]
# 偏差積分値の計算
sum_x = sl.sum_position_difference(sum_x, xd, X, sampling_time)
sum_y = sl.sum_position_difference(sum_y, yd, Y, sampling_time)
sum_X = [sum_x, sum_y]
# モータ入力
Tau = sl.PID_potiton_control_3dof(gain, Xd, position, Jt, dot_theta,
sum_X)
# 偏差と非線形弾性特性値の計算
e = sl.difference_part(theta, q, N)
K = sl.non_linear_item(k, e)
# 拘束力とダイナミクス右辺の計算
dot_P, P, dot_Q, Q = sl.restraint_part(ll, q, dot_q)
f, A = sl.input_forces(ll,
q,
dot_q,
H,
D,
K,
Jt,
P,
Q,
dot_P,
dot_Q,
# 重力項の定義
# G1, G2 = sl.gravity_item_2dof(m1, m2, l1, lg1, lg2, q1, q2, g)
G1, G2 = 0.0, 0.0
# 入力
tau1 = sl.PIDcontrol(kp1, kv1, ki1, thetad1, theta1, dot_thetad1,
dot_theta1, sum_theta1)
tau2 = sl.PIDcontrol(kp2, kv2, ki2, thetad2, theta2, dot_thetad2,
dot_theta2, sum_theta2)
# 偏差と非線形弾性特性値の計算
e1 = sl.difference_part(theta1, q1)
e2 = sl.difference_part(theta2, q2)
e = [e1, e2]
K1 = sl.non_linear_item(k1[0], k2[0], e[0])
K2 = sl.non_linear_item(k1[1], k2[1], e[1])
K1 = K1 * e[0]
K2 = K2 * e[1]
K = [K1, K2]
# 各加速度の計算
ddot_q1 = sl.calculate_angular_acceleration(Minv1, Minv2, K[0], K[1],
h1, h2, G1, G2, D, dot_q1)
ddot_q2 = sl.calculate_angular_acceleration(Minv3, Minv4, K[0], K[1],
h1, h2, G1, G2, D, dot_q2)
ddot_theta1 = sl.motor_angular_acceleration(Mm, tau1, B, dot_theta1,
K[0])
ddot_theta2 = sl.motor_angular_acceleration(Mm, tau2, B, dot_theta2,