######################################################## commanded RPM wheel1RPM = motor_spd_vec[0] # motor 2 speed [rpm] wheel0RPM = motor_spd_vec[1] # motor 1 speed [rpm] wheel2RPM = motor_spd_vec[2] # motor 3 speed [rpm] c_rpm1 = float(wheel0RPM) c_rpm2 = float(wheel1RPM) c_rpm3 = float(wheel2RPM) # ~ print(str(c_rpm1)+" , "+str(c_rpm2)+" , "+str(c_rpm3)) ######################################################## Sending input RPM to Arduino robot.motor_rpm(int(wheel0RPM),int(wheel1RPM),int(wheel2RPM)) ######################################################## reading actual RPM m1_rpm = robot.rpm(0) m2_rpm = robot.rpm(1) m3_rpm = robot.rpm(2) data_rpm = str(m1_rpm)+' , ' +str(m2_rpm)+ ' , ' +str(m3_rpm) # ~ print(data_rpm) ######################################################## odometry using encoder pose = odometryCalc(xc,yc,thetac) pos = odometry_RealSense() # ~ current_x = pose.item(0) # ~ current_y = pose.item(1) # ~ current_theta = pose.item(2) ######################################################## odometry using RealSense current_x = pos_x
def path_f(xd, yd, thetad, b, a, R, step):
global current_x
global current_y
global current_theta
delta_min = 0.1
vr = 0.1
k = 5
Kx = k
Ky = k
Kz = k
file = open(
save_folder + "Circle" + "_vr_" + str(vr) + "_K_" + str(k) +
"_delta_" + str(delta_min) + '_R_' + str(R) + '_step_' + str(step) +
".txt", "a")
# ~ file = open(save_folder2 + "Circle"+"_vr_"+str(vr)+"_K_"+str(Kx)+"_"+str(Ky)+"_"+str(Kz)+"_delta_"+str(delta_min)+".txt","a")
while True:
xc = current_x
yc = current_y
thetac = current_theta
if (a - xd == 0):
x_dot_d = vr
y_dot_d = 0
theta_dot_d = 0.1
if (yd - b == 0):
x_dot_d = 0
y_dot_d = vr
theta_dot_d = 0.1
else:
dydx = (a - xd) / (yd - b)
x_dot_d = vr / np.sqrt(1 + dydx * dydx)
y_dot_d = dydx * x_dot_d
theta_dot_d = 0.1
q_dot_d = np.array([x_dot_d, y_dot_d, theta_dot_d]).reshape(3, 1)
j = (2 * np.pi * 0.03 / 60) * np.array(
[(2 / 3) * np.sin(thetad + np.pi / 3), (-2 / 3) * np.sin(thetad),
(2 / 3) * np.sin(thetad - np.pi / 3),
(-2 / 3) * np.cos(thetad + np.pi / 3), (2 / 3) * np.cos(thetad),
(-2 / 3) * np.cos(thetad - np.pi / 3), -1 / (3 * 0.19), -1 /
(3 * 0.19), -1 / (3 * 0.19)]).reshape(3, 3)
j_inv = np.linalg.inv(j).reshape(3, 3)
K = np.array([Kx, 0, 0, 0, Ky, 0, 0, 0, Kz]).reshape(3, 3)
e = np.array([(xc - xd), (yc - yd), (thetac - thetad)]).reshape(3, 1)
s = np.dot(-K, e, out=None) + q_dot_d
######################################################## Input W (RPM)
w = np.dot(j_inv, s, out=None)
motor_spd_vec = w
# ~ print(vm)
######################################################## commanded RPM
wheel1RPM = motor_spd_vec[0] # motor 2 speed [rpm]
wheel0RPM = motor_spd_vec[1] # motor 1 speed [rpm]
wheel2RPM = motor_spd_vec[2] # motor 3 speed [rpm]
c_rpm1 = float(wheel0RPM)
c_rpm2 = float(wheel1RPM)
c_rpm3 = float(wheel2RPM)
robot.motor_rpm(int(wheel0RPM), int(wheel1RPM), int(wheel2RPM))
pose = odometryCalc(xc, yc, thetac)
pos = odometry_RealSense()
current_x = pose.item(0)
current_y = pose.item(1)
current_theta = pose.item(2)
#use RealSense as feedback
# ~ current_x = pos_x
# ~ current_y = pos_y
# ~ current_theta = theta_rs
vel = np.sqrt(vel_x * vel_x + vel_y * vel_y + vel_z * vel_z)
# ~ vel = str(vel_x)+" , "+str(vel_y)
# ~ print(vel)
delta = np.sqrt(((xd - current_x)**2) + ((yd - current_y)**2))
# ~ print(delta)
m1_rpm = robot.rpm(0)
m2_rpm = robot.rpm(1)
m3_rpm = robot.rpm(2)
data_rpm = str(m1_rpm) + ' , ' + str(m2_rpm) + ' , ' + str(m3_rpm)
# ~ print(data_rpm)
# ~ start = time.time()
time_running = time.time()
# ~ print(time_running)
data_pose = "x: " + str(pose[0][0]) + " y: " + str(
pose[1][0]) + " theta: " + str(pose[2][0])
print(data_pose)
# ~ file.writelines(str(pose[0][0])+" , "+str(pose[1][0])+" , "+str(pose[2][0])+" , "+str(pos_x)+" , "+str(pos_y)+" , "+str(m1_rpm)+" , "+str(m2_rpm)+" , "+str(m3_rpm)+" , "+str(time_running)+ "\n")
# ~ file.writelines(str(pose[0][0])+" , "+str(pose[1][0])+" , "+str(pose[2][0])+" , "+str(pos_x)+" , "+str(pos_y)+" , "+str(m1_rpm)+" , "+str(m2_rpm)+" , "+str(m3_rpm)+" , "+str(vel_x)+" , "+str(vel_y)+" , "+str(vel)+" , "+str(time_running)+ "\n")
file.writelines(
str(pose[0][0]) + " , " + str(pose[1][0]) + " , " +
str(pose[2][0]) + " , " + str(pos_x) + " , " + str(pos_y) + " , " +
str(m1_rpm) + " , " + str(m2_rpm) + " , " + str(m3_rpm) + " , " +
str(c_rpm1) + " , " + str(c_rpm2) + " , " + str(c_rpm3) + " , " +
str(vel_x) + " , " + str(vel_y) + " , " + str(vel) + " , " +
str(time_running) + "\n")
if delta < delta_min:
file.close()
robot.stop()
break
b1_inv = np.linalg.inv(b1).reshape(3,3) A2 = -a1 + j_inv * K1 * j B2 = np.dot(a1,j_inv,out=None)*K1 + j_inv_dot * K1 - j_inv * K1**2 C2_1 = np.dot(a1,j_inv,out=None) C2_2 = np.dot(C2_1,q_dot,out=None) C2 = -C2_2 - np.dot(j_inv_dot,q_dot,out=None) - np.dot(j_inv,q_ddot,out=None) A31 = np.dot(b1_in,(A2 + K2),out=None) A3 = -a2 + np.dot(A31,b1,out=None) B3 = ######################################################## reading actual RPM ############ actual RPM (Wc) m1_rpm = float(robot.rpm(0)) m2_rpm = float(robot.rpm(1)) m3_rpm = float(robot.rpm(2)) alpha = dt_tau / tau input1 = float(m1_rpm) input2 = float(m2_rpm) input3 = float(m3_rpm) output1 += alpha*(input1 - last_output1) output2 += alpha*(input2 - last_output2) output3 += alpha*(input3 - last_output3) m1_rpm_f = output1 m2_rpm_f = output2
w1 = np.dot(-K, e1, out=None) + q_dot_d
############ Wd vector
wd = np.dot(j_inv, w1, out=None).reshape(3, 1)
wd_dot = (last_wd - wd) / del_t
# ~ print(str(wd) +" , "+str(wd_dot))
# ~ print(wd_dot)
############ commanded (Wd)
c_rpm2 = float(wd[0])
c_rpm1 = float(wd[1])
c_rpm3 = float(wd[2])
# ~ print(str(c_rpm1)+" , "+str(c_rpm2)+" , "+str(c_rpm3))
############ actual RPM (Wc)
m1_rpm = int(robot.rpm(0))
m2_rpm = int(robot.rpm(1))
m3_rpm = int(robot.rpm(2))
data_rpm = str(m1_rpm) + ' , ' + str(m2_rpm) + ' , ' + str(
m3_rpm)
print(data_rpm)
######################################################## Calculate V
e2 = np.array([(m1_rpm - c_rpm1), (m2_rpm - c_rpm2),
(m3_rpm - c_rpm3)]).reshape(3, 1)
z1 = np.dot(j, e2, out=None).reshape(3, 1)
j1 = np.dot(j_inv, j_dot, out=None)
j2 = np.dot(j_inv, e1, out=None)
vd = 1 / b1 * a1 * wd + 1 / b1 * wd_dot - 1 / b1 * np.dot(
j1, e2, out=None) - 1 / b1 * j2