def printFrame(frame, info):
# total_force = 0.0
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
# print("Total Force: " + str(total_force))
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
for n in range(frame.n_contacts):
c = frame.contacts[n]
print("Contact ID: ", c.id)
print("X_pos",c.x_pos)
X.append(c.x_pos)
Y.append(c.y_pos)
print("Y_pos",c.y_pos)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
print("Total Force", total_force)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
# Gives force at contact begin
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
print('xxxxxxxxxxxx')
for n in range(frame.n_contacts):
print('------------')
c = frame.contacts[n]
plt.xlim(0, 230)
plt.ylim(0, 130)
plt.gca().invert_yaxis()
print("Contact ID: ", c.id)
print("X_pos",c.x_pos)
print("Y_pos",c.y_pos)
print("ID",c.id)
print("State",c.state)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
hl.set_xdata(numpy.append(hl.get_xdata(), c.x_pos))
hl.set_ydata(numpy.append(hl.get_ydata(), c.y_pos))
plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
plt.annotate(c.id+1,(c.x_pos,c.y_pos),size=8)
# popt, pcov = curve_fit(func, hl.get_xdata(), hl.get_ydata(), bounds=(0, [3., 1., 0.5]))
# plt.plot(hl.get_xdata(), func(xdata, *popt), 'g--',
# label='fit: a=%5.3f, b=%5.3f, c=%5.3f' % tuple(popt))
if c.state==3:
# count=count+1
# if count==frame.n_contacts:
print("XY", hl.get_xdata(), hl.get_ydata())
print("STD", statistics.stdev(hl.get_ydata()))
print("STD", statistics.stdev(hl.get_xdata()))
hl.set_xdata(0)
hl.set_ydata(0)
plt.cla()
plt.pause(0.0001)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
# hl.set_xdata(0)
# hl.set_ydata(0)
print("XY",hl.get_xdata(), hl.get_ydata())
def printFrame(frame, info):
# total_force = 0.0
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
# print("Total Force: " + str(total_force))
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
for n in range(frame.n_contacts):
plt.xlim(0, 230)
plt.ylim(0, 130)
# plt.scatter(X, Y)
plt.gca().invert_yaxis()
# plt.plot([], [], linestyle='--', linewidth=0.5)
c = frame.contacts[n]
print("Contact ID: ", c.id)
print("X_pos", c.x_pos)
print("Y_pos", c.y_pos)
print("Force at contact point", c.total_force)
print("STATE", c.state)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
print("Total Force", total_force)
# hl.set_xdata(numpy.append(hl.get_xdata(), c.x_pos))
# hl.set_ydata(numpy.append(hl.get_ydata(), c.y_pos))
# if(c.total_force<100):
# plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
# elif(c.total_force<400):
# plt.scatter(c.x_pos, c.y_pos, marker='o', color='y')
# elif (c.total_force < 600):
# plt.scatter(c.x_pos, c.y_pos, marker='o',color='g')
# elif (c.total_force >600):
# plt.scatter(c.x_pos, c.y_pos, marker='o', color='r')
if c.state == 3:
plt.cla()
plt.pause(0.0001)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
for n in range(frame.n_contacts):
c = frame.contacts[n]
print("Contact ID: ", c.id)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
# total_force = 0.0
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
# print("Total Force: " + str(total_force))
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
for n in range(frame.n_contacts):
c = frame.contacts[n]
print("Contact ID: ", c.id)
print("X_pos",c.x_pos)
print("Y_pos",c.y_pos)
print("Force at contact point",c.total_force)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
print("Total Force", total_force)
scats.append(c.x_pos)
scats1.append(c.y_pos)
print(scats)
print(scats1)
# hl.set_xdata(numpy.append(hl.get_xdata(), c.x_pos))
# hl.set_ydata(numpy.append(hl.get_ydata(), c.y_pos))
if(c.total_force<100):
ax.scatter(c.x_pos, c.y_pos, marker='o', color='b')
elif(c.total_force<400):
ax.scatter(c.x_pos, c.y_pos, marker='o', color='y')
elif (c.total_force < 600):
ax.scatter(c.x_pos, c.y_pos, marker='o',color='g')
elif (c.total_force >600):
ax.scatter(c.x_pos, c.y_pos, marker='o', color='r')
plt.pause(0.0001)
if c.state==3:
scats.clear()
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
# total_force = 0.0
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
# print("Total Force: " + str(total_force))
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
for n in range(frame.n_contacts):
c = frame.contacts[n]
print("Contact ID: ", c.id)
print("X_pos", c.x_pos)
print("Y_pos", c.y_pos)
X.append(c.x_pos)
Y.append(c.y_pos)
plt.ion()
animated_plot = plt.plot(X, Y, 'ro')[0]
for i in range(len(X)):
animated_plot.set_xdata(X[0:i])
animated_plot.set_ydata(Y[0:i])
plt.draw()
plt.pause(0.0001)
# f = open('sampleText', 'a')
# f.write(str(c.x_pos)+','+str(c.y_pos)+'\n')
# animate(c.x_pos,c.y_pos)
# plt.scatter(c.x_pos, c.y_pos)
# ani = animation.FuncAnimation(plt.figure(), plt.scatter(c.x_pos,c.y_pos), interval=1000)
# plt.show(block=False)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
print("Total Force", total_force)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
# Gives force at contact begin
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
# total_force = 0.0
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
# print("Total Force: " + str(total_force))
plt.gca().set_xticks(numpy.arange(0, 185, 1))
plt.gca().set_yticks(numpy.arange(0, 105, 1))
plt.grid()
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
for n in range(frame.n_contacts):
c = frame.contacts[n]
print("Contact ID: ", c.id)
print("X_pos", c.x_pos)
print("Y_pos", c.y_pos)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
print("Total Force", total_force)
hl.set_xdata(numpy.append(hl.get_xdata(), c.x_pos))
hl.set_ydata(numpy.append(hl.get_ydata(), c.y_pos))
if (total_force < 100):
plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
elif (total_force < 500):
plt.scatter(c.x_pos, c.y_pos, marker='o', color='y')
elif (total_force < 1500):
plt.scatter(c.x_pos, c.y_pos, marker='o', color='g')
elif (total_force > 2000):
plt.scatter(c.x_pos, c.y_pos, marker='o', color='r')
plt.pause(0.0001)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
# total_force = 0.0
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
# print("Total Force: " + str(total_force))
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
for n in range(frame.n_contacts):
c = frame.contacts[n]
print("Contact ID: ", c.id)
print("X_pos", c.x_pos)
print("Y_pos", c.y_pos)
print("Orientation", c.orientation)
print("Force at contact point", c.total_force)
print("Major Axis", c.major_axis, c.minor_axis)
# ellipse = Ellipse(xy=(c.x_pos,c.y_pos), width=c.major_axis, height=c.minor_axis,
# edgecolor='r', fc='None', lw=2)
# ax.add_patch(ellipse)
fig = plt.figure(0)
ax = fig.add_subplot(111, aspect='equal')
ax.set_xlim(0, 230)
ax.set_ylim(0, 130)
ax.invert_yaxis()
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
print("Total Force", total_force)
print("Peak Force of contact Area", +c.peak_force)
print("MINX " + str(c.min_x) + "MAXX " + str(c.max_x) + "MINY " +
str(c.min_y) + "MAXY " + str(c.max_y))
# hl.set_xdata(numpy.append(hl.get_xdata(), c.x_pos))
# hl.set_ydata(numpy.append(hl.get_ydata(), c.y_pos))
if (c.total_force < 100):
ells = Ellipse(xy=(c.x_pos, c.y_pos),
width=c.major_axis,
height=c.minor_axis,
color='b',
angle=c.orientation)
plt.scatter(c.peak_x, c.peak_y, marker='o', color='r')
elif (c.total_force < 400):
ells = Ellipse(xy=(c.x_pos, c.y_pos),
width=c.major_axis,
height=c.minor_axis,
color='y',
angle=c.orientation)
plt.scatter(c.peak_x, c.peak_y, marker='o', color='b')
elif (c.total_force < 600):
ells = Ellipse(xy=(c.x_pos, c.y_pos),
width=c.major_axis,
height=c.minor_axis,
color='g',
angle=c.orientation)
plt.scatter(c.peak_x, c.peak_y, marker='o', color='y')
elif (c.total_force > 600):
ells = Ellipse(xy=(c.x_pos, c.y_pos),
width=c.major_axis,
height=c.minor_axis,
color='r',
angle=c.orientation)
plt.scatter(c.peak_x, c.peak_y, marker='o', color='g')
# if (c.total_force < 100):
#
# elif (c.total_force < 400):
# plt.scatter(c.peak_x, c.peak_y, marker='o', color='y')
# elif (c.total_force < 600):
# plt.scatter(c.peak_x, c.peak_y, marker='o', color='g')
# elif (c.total_force > 600):
# plt.scatter(c.peak_x, c.peak_y, marker='o', color='r')
ax.add_artist(ells)
if c.state == 3:
plt.cla()
plt.pause(0.0001)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
print('xxxxxxxxxxxx')
count = 0
for n in range(frame.n_contacts):
print('------------')
c = frame.contacts[n]
plt.xlim(0, 230)
plt.ylim(0, 130)
plt.gca().invert_yaxis()
print("Contact ID: ", c.id)
print("X_pos", c.x_pos)
print("Y_pos", c.y_pos)
print("ID", c.id)
print("State", c.state)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
hl.set_xdata(numpy.append(hl.get_xdata(), c.x_pos))
hl.set_ydata(numpy.append(hl.get_ydata(), c.y_pos))
plt.subplot(2, 1, 1)
plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
# if(c.total_force<100):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
# elif(c.total_force<400):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o', color='y')
# elif (c.total_force < 600):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o',color='g')
# elif (c.total_force >600):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o', color='r')
plt.annotate(c.id + 1, (c.x_pos, c.y_pos), size=8)
# popt, pcov = curve_fit(func, hl.get_xdata(), hl.get_ydata(), bounds=(0, [3., 1., 0.5]))
# plt.plot(hl.get_xdata(), func(xdata, *popt), 'g--',
# label='fit: a=%5.3f, b=%5.3f, c=%5.3f' % tuple(popt))
if c.state == 3:
count = count + 1
if count == frame.n_contacts:
# print("XY", hl.get_xdata(), hl.get_ydata())
# print("STDY", statistics.stdev(hl.get_ydata()))
# print("STDX", statistics.stdev(hl.get_xdata()))
# if len(hl.get_data()) > 5:
# plt.scatter(hl.get_xdata(), hl.get_ydata())
# popt, pcov = curve_fit(func, hl.get_xdata(), hl.get_ydata())
# plt.figure(300)
# plt.xlim(0, 230)
# plt.ylim(0, 130)
# plt.gca().invert_yaxis()
# plt.plot(hl.get_xdata(), func(hl.get_xdata(), *popt), 'r-')
# plt.pause(0.0001)
xdata = np.delete(np.array(hl.get_xdata()), 0)
ydata = np.delete(np.array(hl.get_ydata()), 0)
print("START POINT", xdata[0], ydata[0])
print("END POINT", xdata[len(xdata) - 1],
ydata[len(ydata) - 1])
print("PONITS SEEN BEFORE")
for point in points_seen_till_now:
print(point)
if len(points_seen_till_now) > 1:
tree = spatial.KDTree(points_seen_till_now)
print(hl.get_xdata())
print(hl.get_ydata())
# print("QUERY",tree.query((xdata[len(xdata)-1],ydata[len(ydata)-1])))
querystart = tree.query((xdata[0], ydata[0]))
print("POINT TO BE COMPARED WITH START", xdata[0],
ydata[0])
print("QUERY START POINT", querystart[0],
points_seen_till_now[querystart[1]])
if querystart[0] < 40:
print(xdata[0])
xdata[0] = points_seen_till_now[querystart[1]][0]
print(points_seen_till_now[querystart[1]][0])
print(ydata[0])
ydata[0] = points_seen_till_now[querystart[1]][1]
print(points_seen_till_now[querystart[1]][1])
queryend = tree.query(
(xdata[len(xdata) - 1], ydata[len(ydata) - 1]))
print("POINT TO BE COMPARED WITH END",
xdata[len(xdata) - 1], xdata[len(xdata) - 1])
print("QUERY END POINT", queryend[0],
points_seen_till_now[queryend[1]])
if queryend[0] < 40:
xdata[len(xdata) -
1] = points_seen_till_now[queryend[1]][0]
ydata[len(xdata) -
1] = points_seen_till_now[queryend[1]][1]
points_seen_till_now.append((xdata[0], ydata[0]))
points_seen_till_now.append(
(xdata[len(xdata) - 1], ydata[len(ydata) - 1]))
# print("PONITS SEEN AFTER", points_seen_till_now)
plt.subplot(2, 1, 2)
plt.xlim(0, 230)
plt.ylim(0, 130)
plt.gca().invert_yaxis()
plt.plot([xdata[0], xdata[len(xdata) - 1]],
[ydata[0], ydata[len(ydata) - 1]], 'k-')
# optimizedParameters, pcov = opt.curve_fit(func, xdata, ydata)
# plt.figure(2)
# plt.xlim(0, 230)
# plt.ylim(0, 130)
# plt.gca().invert_yaxis()
# plt.plot(xdata, func(xdata, *optimizedParameters), label="fit")
hl.set_xdata(0)
hl.set_ydata(0)
# plt.subplot(2,1,1)
# plt.cla()
plt.pause(0.0001)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
# total_force = 0.0
# for n in range(info.num_rows * info.num_cols):
# total_force += frame.force_array[n]
# print("Total Force: " + str(total_force))
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
print('xxxxxxxxxxxx')
for n in range(frame.n_contacts):
c = frame.contacts[n]
plt.xlim(0, 230)
plt.ylim(0, 130)
# plt.scatter(X, Y)
plt.gca().invert_yaxis()
print('------------')
print("Contact ID: ", c.id)
print("X_pos", c.x_pos)
print("Y_pos", c.y_pos)
# print("Force at contact point",c.total_force)
# print("STATE",c.state)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
# print("Total Force", total_force)
# print("XYXYX",c.min_x,c.min_y,c.max_x,c.max_y)
if c.state == 1 or c.state == 2:
if (c.total_force < 100):
x = plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
elif (c.total_force < 400):
x = plt.scatter(c.x_pos, c.y_pos, marker='o', color='y')
elif (c.total_force < 600):
x = plt.scatter(c.x_pos, c.y_pos, marker='o', color='g')
elif (c.total_force > 600):
x = plt.scatter(c.x_pos, c.y_pos, marker='o', color='r')
plt.annotate(c.id + 1, (c.x_pos, c.y_pos), size=8)
print("ID", c.id)
print("State", c.state)
if c.state == 3:
plt.cla()
plt.pause(0.0001)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
def printFrame(frame, info):
if frame.n_contacts > 0:
print("\nNum Contacts: ", frame.n_contacts)
print('xxxxxxxxxxxx')
count = 0
for n in range(frame.n_contacts):
print('------------')
c = frame.contacts[n]
plt.xlim(0, 230)
plt.ylim(0, 130)
plt.gca().invert_yaxis()
print("Contact ID: ", c.id)
print("X_pos", c.x_pos)
print("Y_pos", c.y_pos)
print("ID", c.id)
print("State", c.state)
total_force = 0.0
for n in range(info.num_rows * info.num_cols):
total_force += frame.force_array[n]
hl.set_xdata(numpy.append(hl.get_xdata(), c.x_pos))
hl.set_ydata(numpy.append(hl.get_ydata(), c.y_pos))
plt.subplot(2, 1, 1)
plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
# if(c.total_force<100):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o', color='b')
# elif(c.total_force<400):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o', color='y')
# elif (c.total_force < 600):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o',color='g')
# elif (c.total_force >600):
# x=plt.scatter(c.x_pos, c.y_pos, marker='o', color='r')
plt.annotate(c.id + 1, (c.x_pos, c.y_pos), size=8)
# popt, pcov = curve_fit(func, hl.get_xdata(), hl.get_ydata(), bounds=(0, [3., 1., 0.5]))
# plt.plot(hl.get_xdata(), func(xdata, *popt), 'g--',
# label='fit: a=%5.3f, b=%5.3f, c=%5.3f' % tuple(popt))
if c.state == 3:
count = count + 1
if count == frame.n_contacts:
print("XY", hl.get_xdata(), hl.get_ydata())
print("STD", statistics.stdev(hl.get_ydata()))
print("STD", statistics.stdev(hl.get_xdata()))
# if len(hl.get_data()) > 5:
# plt.scatter(hl.get_xdata(), hl.get_ydata())
# popt, pcov = curve_fit(func, hl.get_xdata(), hl.get_ydata())
# plt.figure(300)
# plt.xlim(0, 230)
# plt.ylim(0, 130)
# plt.gca().invert_yaxis()
# plt.plot(hl.get_xdata(), func(hl.get_xdata(), *popt), 'r-')
# plt.pause(0.0001)
xdata = np.array(hl.get_xdata())
ydata = np.array(hl.get_ydata())
xdata = np.delete(xdata, 0)
ydata = np.delete(ydata, 0)
print("START POINT", xdata[0], xdata[len(xdata) - 1])
print("END POINT", ydata[0], ydata[len(ydata) - 1])
plt.subplot(2, 1, 2)
plt.xlim(0, 230)
plt.ylim(0, 130)
plt.gca().invert_yaxis()
plt.plot([xdata[0], xdata[len(xdata) - 1]],
[ydata[0], ydata[len(ydata) - 1]], 'k-')
# optimizedParameters, pcov = opt.curve_fit(func, xdata, ydata)
# plt.figure(2)
# plt.xlim(0, 230)
# plt.ylim(0, 130)
# plt.gca().invert_yaxis()
# plt.plot(xdata, func(xdata, *optimizedParameters), label="fit")
hl.set_xdata(0)
hl.set_ydata(0)
# plt.subplot(2,1,1)
# plt.cla()
plt.pause(0.0001)
if c.state == sensel.CONTACT_START:
sensel.setLEDBrightness(handle, c.id, 100)
elif c.state == sensel.CONTACT_END:
sensel.setLEDBrightness(handle, c.id, 0)
