masterArr2 = np.zeros([len(velArr1),4])
masterArr2[:,0] = velArr1
masterArr2[:,1] = velArr2
masterArr2[:,2] = velArr3
masterArr2[:,3] = velArr4
'''
colors = ['red','green','purple','blue','black']
label = ['limb 1','limb 2','limb 3','limb 4']
fig, (ax1, ax2) = plt.subplots(2)
for i in range(4):
test = ax1.plot(masterArr[:,0,i],masterArr[:,1,i],color=colors[i], label=label[i])
ax1.scatter(masterArr[0,0,i],masterArr[0,1,i],color='black')
ax1.legend()
for i in range(4):
ax2.plot(np.linspace(0, 10, num=len(velArr1)), masterArr2[:,i],color=colors[i], label=label[i])
ax1.set(xlabel='x position (m)', ylabel='y position (m)')
ax2.set(xlabel='time (s)', ylabel='x velocity (m/s)')
ax2.legend()
'''
np.save("limbPos.npy",masterArr)
np.save("limbVels.npy",masterArr2)
fig,ax1,ax2 = m.make_graded_limb_plot(masterArr,masterArr2,0,endtime,"Limb Position: Constant Force","Limb X Velocities vs Time: Constant Force")
fig2,ax3 = m.make_plot(posArr5,0,endtime)
if save:
title=argv[1]+'kp_'+argv[2]+'ki_'+argv[3]+'kd.png'
plt.savefig(title)
raise(EOFError)
plt.show()
# raise(EOFError)
import numpy as np
import make_body_plot as m
import matplotlib.pyplot as plt
numPts = 1800
vels = np.load("limbVels.npy")
pos = np.load("limbPos.npy")
m.make_graded_limb_plot(pos[:numPts], vels[:numPts], 0, 20)
plt.show()
def runSim(t,kp,ki,kd):
modArr,bodyId = startSim()
global goal_pos_y
mods = modArr
mod1 = modArr[0]
mod2 = modArr[1]
mod3 = modArr[2]
mod4 = modArr[3]
k_i = ki
k_p = kp
k_d = kd
time_prev = time.time()
start_time = time.time()
store_data_time = time.time()
for i in range(4):
print(p.getBasePositionAndOrientation(modArr[i]))
#raise(ArithmeticError)
posArr1 = []
posArr2 = []
posArr3 = []
posArr4 = []
posArr5 = []
velArr1 = []
velArr2 = []
velArr3 = []
velArr4 = []
bodyPos = []
bodyPos.append(p.getBasePositionAndOrientation(bodyId)[0])
q = False
numIts = 0
#run for 10 seconds
endtime = 8
bodygoal = []
while numIts < 240*endtime:
time.sleep(1./100.)
numIts += 1
print(numIts)
try:
bodyPos.append(p.getBasePositionAndOrientation(bodyId)[0])
# if time.time()-start_time < 5:
# k_p = 0.2
# k_d = 0
# k_i = 0
# else:
# k_p = kp
# k_i = ki
# k_d = kd
if time.time()-start_time > 1:
# Get velocity of each wheel in x direction
vel_x = []
for i in range(4):
temp = p.getBaseVelocity(mods[i])
temp = temp[0][0] # Get index of x velocity
vel_x.append(temp)
# print(vel_x)
# Get position of each wheel
pos_y = []
for i in range(4):
temp,_ = p.getBasePositionAndOrientation(mods[i])
pos_y.append(temp[1]) # Get index of y position
# temp = p.getBaseVelocity(mods[i])
# temp = temp[0][1] # Get index of y velocity
# pos_y.append(temp)
# print(pos_y)
# noise. Use noise of 3 for simulation
for i in range(4):
# p.applyExternalForce(mods[i],-1,[np.random.normal(0, 5),np.random.normal(0, 5),0],[0,0,0],p.LINK_FRAME)
pass
# Apply force: either random noise or control
if True:#time.time() - lastControlTime > 0.005: # Control every .1 second
# Velocity control for x axis
control_x = []
for i in range(4):
error = (vel_x[i] - goal_vel_x[i])
P = k_p * error
I = control_x_prev[i] + k_i * error * (time.time() - time_prev)
D = k_d * (error - error_x_prev[i]) / (time.time() - time_prev)
control_x.append(- P)
if control_x[i] > 100:
control_x[i] = 100
elif control_x[i] < -100:
control_x[i] = -100
control_x_prev[i] = control_x[i]
error_x_prev[i] = error
# print(control_x)
# Position control for y axis
control_y = []
for i in range(4):
error = pos_y[i] - goal_pos_y[i]
P = k_p * error * 10
I = control_y_prev[i] + k_i*3 * error * (time.time() - time_prev)
D = k_d * (error - error_y_prev[i]) / (time.time() - time_prev)
control_y.append(-P-D)
# control_y.append(0)
control_y_prev[i] = control_y[i]
error_y_prev[i] = error
# print(control_y)
time_prev = time.time()
# Apply control as force
for i in range(2):
# p.applyExternalForce(mods[i],-1,[control_x[i],control_y[i],0],[0,0,0],p.LINK_FRAME)
p.applyExternalForce(mods[i],-1,np.array([control_x[i] ,control_y[i]/1,0]),[0,-0.0,0],p.LINK_FRAME)
p.applyExternalForce(mods[i],-1,np.array([control_x[i] ,control_y[i]/1,0]),[0,0.0,0],p.LINK_FRAME)
for i in range(3,4):
# p.applyExternalForce(mods[i],-1,[control_x[i],control_y[i],0],[0,0,0],p.LINK_FRAME)
p.applyExternalForce(mods[i],-1,np.array([control_x[i]*0.1 ,control_y[i]/1,0]),[0,-0.0,0],p.LINK_FRAME)
p.applyExternalForce(mods[i],-1,np.array([control_x[i]*0.1 ,control_y[i]/1,0]),[0,0.0,0],p.LINK_FRAME)
# if i == 1 or i == 2:
# p.applyExternalForce(mods[i],-1,[control_x[i] - control_y[i]/5,0,0],[0,1/4,0],p.LINK_FRAME)
# p.applyExternalForce(mods[i],-1,[control_x[i] + control_y[i]/5,0,0],[0,-1/4,0],p.LINK_FRAME)
# else:
# p.applyExternalForce(mods[i],-1,[control_x[i] + control_y[i]/5,0,0],[0,-1/4,0],p.LINK_FRAME)
# p.applyExternalForce(mods[i],-1,[control_x[i] - control_y[i]/5,0,0],[0,1/4,0],p.LINK_FRAME)
pass
#lastControlTime = time.time()
# Store
pos1,ort = p.getBasePositionAndOrientation(mod1)
pos2,ort = p.getBasePositionAndOrientation(mod2)
pos3,ort = p.getBasePositionAndOrientation(mod3)
pos4,ort = p.getBasePositionAndOrientation(mod4)
pos5,ort = p.getBasePositionAndOrientation(bodyId)
posArr1.append(pos1)
posArr2.append(pos2)
posArr3.append(pos3)
posArr4.append(pos4)
posArr5.append(pos5)
velArr1.append(vel_x[0])
velArr2.append(vel_x[1])
velArr3.append(vel_x[2])
velArr4.append(vel_x[3])
p.stepSimulation()
if numIts/240 > 2 and numIts/240 < 5:
#print(goal_pos_y)
goal_pos_y += np.ones(4)/240
bodygoal.append(bodygoal[-1]+np.ones(4)/240
except(KeyboardInterrupt):
return
posArr1 = np.array(posArr1)
posArr2 = np.array(posArr2)
posArr3 = np.array(posArr3)
posArr4 = np.array(posArr4)
posArr5 = np.array(posArr5)
# Master array for position
masterArr = np.zeros([posArr1.shape[0],posArr1.shape[1],5])
masterArr[:,:,0] = posArr1
masterArr[:,:,1] = posArr2
masterArr[:,:,2] = posArr3
masterArr[:,:,3] = posArr4
masterArr[:,:,4] = posArr5
# Master array for velocity in x direction
masterArr2 = np.zeros([len(velArr1),4])
masterArr2[:,0] = velArr1
masterArr2[:,1] = velArr2
masterArr2[:,2] = velArr3
masterArr2[:,3] = velArr4
colors = ['red','green','purple','blue','black']
fig, (ax1, ax2) = plt.subplots(2)
for i in range(4):
ax1.plot(masterArr[:,0,i],masterArr[:,1,i],color=colors[i])
ax1.scatter(masterArr[0,0,i],masterArr[0,1,i],color='black')
for i in range(4):
ax2.plot(np.linspace(1, len(velArr1), num=len(velArr1)), masterArr2[:,i],color=colors[i])
ax1.set(xlabel='x', ylabel='y')
ax2.set(xlabel='time', ylabel='x velocity')
plt.savefig()
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
return
posArr1 = np.array(posArr1)
posArr2 = np.array(posArr2)
posArr3 = np.array(posArr3)
posArr4 = np.array(posArr4)
posArr5 = np.array(posArr5)
# Master array for position
masterArr = np.zeros([posArr1.shape[0],posArr1.shape[1],5])
masterArr[:,:,0] = posArr1
masterArr[:,:,1] = posArr2
masterArr[:,:,2] = posArr3
masterArr[:,:,3] = posArr4
masterArr[:,:,4] = posArr5
# Master array for velocity in x direction
masterArr2 = np.zeros([len(velArr1),4])
masterArr2[:,0] = velArr1
masterArr2[:,1] = velArr2
masterArr2[:,2] = velArr3
masterArr2[:,3] = velArr4
colors = ['red','green','purple','blue','black']
label = ['limb 1','limb 2','limb 3','limb 4','body']
fig, (ax1, ax2) = plt.subplots(2)
for i in range(5):
test = ax1.plot(masterArr[:,0,i],masterArr[:,1,i],color=colors[i], label=label[i])
ax1.scatter(masterArr[0,0,i],masterArr[0,1,i],color='black')
ax1.legend()
ax1.set_ylim([-0.5, 3.5])
for i in range(4):
ax2.plot(np.linspace(0, t, num=len(velArr1)), masterArr2[:,i],color=colors[i], label=label[i])
ax1.set(xlabel='x position (m)', ylabel='y position (m)')
ax2.set(xlabel='time (s)', ylabel='x velocity (m/s)')
ax2.legend()
title = str(kp)+"kp_"+str(ki)+"ki_"+str(kd)+"kd.png"
#plt.savefig(title)
fig2,ax3 = plt.subplots(1,1)
N=len(bodyPos)
if N % 2 == 0:
Ndiv2 = int(N/2)
blueMagenta = np.zeros([Ndiv2,4])
magentaRed = np.zeros([Ndiv2,4])
blueMagenta[:,0] = np.linspace(0,1,Ndiv2)
blueMagenta[:,2] = 1
magentaRed[:,2] = np.linspace(1,0,Ndiv2)
magentaRed[:,0] = 1
else:
N = N-1
Ndiv2 = int(N/2)
blueMagenta = np.zeros([Ndiv2,4])
magentaRed = np.zeros([Ndiv2+1,4])
blueMagenta[:,0] = np.linspace(0,1,Ndiv2)
blueMagenta[:,2] = 1
magentaRed[:,2] = np.linspace(1,0,Ndiv2+1)
magentaRed[:,0] = 1
N = N+1
blueMagenta[:,3] = 1
magentaRed[:,3] = 1
twocolors = np.vstack((blueMagenta,magentaRed))
print(np.shape(twocolors))
cmap = ListedColormap(twocolors)
colors = np.array([[0,x/(len(bodyPos)-1),1] for x in range(len(bodyPos)-1)])
for i in range(len(bodyPos)-1):
ax3.plot([bodyPos[i][0],bodyPos[i+1][0]],[bodyPos[i][1],bodyPos[i+1][1]],color=twocolors[i,:3],linewidth=2)
ax3.set(xlabel='Body X Position', ylabel='Body Y Position')
fig2.colorbar(cm.ScalarMappable(cmap=cmap))
m.make_graded_limb_plot(masterArr,masterArr2,0,endtime)
plt.show()
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
print(np.sum(abs(masterArr[:,1,0] - 0.0833)))
print(np.sum(abs(masterArr[:,1,1] + 0.0833)))
print(np.sum(abs(masterArr[:,1,2] - 0.0833)))
print(np.sum(abs(masterArr[:,1,3] + 0.0833)))
avedev = np.sum(abs(masterArr[:,1,0] - 0.0833)) + np.sum(abs(masterArr[:,1,1] + 0.0833)) + np.sum(abs(masterArr[:,1,2] - 0.0833)) + np.sum(abs(masterArr[:,1,3] + 0.0833))
avedev = avedev / 4
print(avedev)
print(np.sum(abs(masterArr[:,1,4])))
print((avedev - np.sum(abs(masterArr[:,1,4])))/avedev)
return
# raise(EOFError)
for i in range(1):
# runSim(30,0.2,0.2,0.5)
runSim(40,1,0,0.01)
#runSim(10,10,0,0.01)
def runSim(t,kp,ki,kd):
modArr,bodyId = startSim()
mods = modArr
mod1 = modArr[0]
mod2 = modArr[1]
mod3 = modArr[2]
mod4 = modArr[3]
startTime = time.time()
posArr = np.zeros([1,4,3])
forces = posArr.copy()
velArr = forces.copy()
x=True
add = np.zeros([4,3])
add[:,0] += 1
speed = 30
des = np.zeros([4,3])
tics = 0
numits = 0
endtime=9
xvels = []
while True:
try:
numits += 1
if time.time()-startTime > 1:
poses = get_positions(modArr)
f=pid(kp,ki,kd,modArr,des,poses)
#p.setJointMotorControl2(bodyId,0,controlMode=p.VELOCITY_CONTROL,targetVelocity=20)
#f = apply_forces(modArr,poses,des,poly,1)
des[:,0] += tstep*20 #10 units/sec
des = poses[-1] + add*speed
des[0,1] = 0.0833
des[1,1] = -des[0,1]
des[2,1] = des[0,1]
des[3,1] = des[1,1]
posArr = np.append(posArr,poses,0)
velArr = np.append(velArr,getVels(modArr),0)
xvels.append(getVels(modArr)[0,:,0])
#forces = np.append(forces,f,0)
p.stepSimulation()
time.sleep(1/100.)
tics += 1
#print(tics)
if tics > 1400:
raise(KeyboardInterrupt)
#print(des)
except(KeyboardInterrupt):
masterArr = posArr
#print(posArr.shape)
colors = ['red','green','purple','blue']
labels = ['limb 1','limb 2','limb 3','limb 4']
fig,(ax1,ax2) = plt.subplots(2)
for i in range(4):
ax1.plot(masterArr[1:,i,0],masterArr[1:,i,1],color=colors[i],label=labels[i])
#ax1.scatter(masterArr[0,0,i],masterArr[0,1,i],color='black')
ax2.plot(np.linspace(1,len(velArr),num=len(velArr))/100,velArr[:,i,0],color=colors[i],label=labels[i])
ax1.set(xlabel='x position(m)',ylabel='y position (m)')
ax2.set(xlabel='time (s)',ylabel='x velocity (m/s)')
ax2.legend()
ax1.legend()
title = "morepics/"+str(kp)+"kP_"+str(ki)+"kI_"+str(kd)+"kD.png"
#plt.savefig(title)
posArr = np.transpose(posArr,(0,2,1))
velArr = np.transpose(velArr,(0,2,1))
#print(posArr.shape)
#print(velArr.shape)
#print("A")
xvels = np.array(xvels)
m.make_graded_limb_plot(posArr[1:],xvels[1:],0,endtime-1)
plt.show()
#plt.figure()
#plt.plot(np.arange(forces.shape[0]),forces[:,2,1])
#plt.xlabel("step num")
#plt.ylabel("Applied X force")
x=False
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
return
posArr4 = np.array(posArr4)
posArr5 = np.array(posArr5)
# Master array for position
masterArr = np.zeros([posArr1.shape[0], posArr1.shape[1], 5])
masterArr[:, :, 0] = posArr1
masterArr[:, :, 1] = posArr2
masterArr[:, :, 2] = posArr3
masterArr[:, :, 3] = posArr4
masterArr[:, :, 4] = posArr5
# Master array for velocity in x direction
masterArr2 = np.zeros([len(velArr1), 4])
masterArr2[:, 0] = velArr1
masterArr2[:, 1] = velArr2
masterArr2[:, 2] = velArr3
masterArr2[:, 3] = velArr4
np.save("limbPos.npy", masterArr)
np.save("limbVels.npy", masterArr2)
#fig,ax1,ax2 = m.make_graded_limb_plot(masterArr,masterArr2,0,20)
#fig2,ax3 = m.make_plot(posArr5,0,endTime)
m.make_graded_limb_plot(masterArr, masterArr2, 0, 20)
#plt.plot(masterArr[:,0,0],masterArr[:,0,0],color='blue')
if save:
title = argv[1] + 'kp_' + argv[2] + 'ki_' + argv[3] + 'kd.png'
plt.savefig(title)
raise (EOFError)
plt.show()
# raise(EOFError)