def identify_motor_acc(dt, dq, ddq, current, tau, Kt_p, Kv_p, ZERO_VELOCITY_THRESHOLD_SMALL, ZERO_JERK_THRESHOLD,
SHOW_THRESHOLD_EFFECT):
# Filter current*****************************************************
win = signal.hann(10)
filtered_current = signal.convolve(current, win, mode='same') / sum(win)
current = filtered_current
# Mask valid data***************************************************
# # remove high jerk
dddq = np.gradient(ddq, 1) / dt
maskConstAcc = (abs(dddq) < ZERO_JERK_THRESHOLD)
# # erode to get only steady phases where acceleration is constant
maskConstAcc = ndimage.morphology.binary_erosion(maskConstAcc, None, 100)
maskPosVel = (dq > ZERO_VELOCITY_THRESHOLD_SMALL)
maskNegVel = (dq < -ZERO_VELOCITY_THRESHOLD_SMALL)
maskConstPosAcc = np.logical_and(maskConstAcc, maskPosVel)
maskConstNegAcc = np.logical_and(maskConstAcc, maskNegVel)
if SHOW_THRESHOLD_EFFECT:
plt.figure()
plt.plot(ddq)
plt.ylabel('ddq')
ddq_const = ddq.copy()
ddq_const[np.logical_not(maskConstAcc)] = np.nan
plt.plot(ddq_const)
plt.ylabel('ddq_const')
plt.show()
# y = a. x + b
# i-Kt.tau-Kv.dq = Ka.ddq + Kf
#
# Identification ***************************************************
y = current - Kt_p * tau - Kv_p * dq
y[maskConstPosAcc] = current[maskConstPosAcc] - Kt_p * tau[maskConstPosAcc] - Kv_p * dq[maskConstPosAcc]
y[maskConstNegAcc] = current[maskConstNegAcc] - Kt_p * tau[maskConstNegAcc] - Kv_p * dq[maskConstNegAcc]
y_label = r'$i(t)-{K_t}{\tau(t)}-{K_v}{\dot{q}(t)}$'
x = ddq
x_label = r'$\ddot{q}(t)$'
(Kap, Kfp) = solve1stOrderLeastSquare(x[maskConstPosAcc], y[maskConstPosAcc])
(Kan, b) = solve1stOrderLeastSquare(x[maskConstNegAcc], y[maskConstNegAcc])
Kfn = -b
# Plot *************************************************************
plt.figure()
plt.axhline(0, color='black', lw=1)
plt.axvline(0, color='black', lw=1)
plt.plot(x, y, '.', lw=3, markersize=1, c='0.5')
plt.plot(x[maskConstPosAcc], y[maskConstPosAcc], 'rx', lw=3, markersize=1)
plt.plot(x[maskConstNegAcc], y[maskConstNegAcc], 'bx', lw=3, markersize=1)
# plot identified lin model
plt.plot([min(x), max(x)], [Kap * min(x) + Kfp, Kap * max(x) + Kfp], 'g:', lw=3)
plt.plot([min(x), max(x)], [Kan * min(x) - Kfn, Kan * max(x) - Kfn], 'g:', lw=3)
plt.ylabel(y_label)
plt.xlabel(x_label)
plt.show()
return (Kap, Kan, Kfp, Kfn)
def identify_motor_vel(dt, dq, ddq, ctrl, current, tau, Ktp, Ktn, Ks,
ZERO_VEL_THRESHOLD, ZERO_ACC_THRESHOLD, Nvel,
SHOW_THRESHOLD_EFFECT):
# Mask valid data***************************************************
# remove high acceleration
maskConstVel = np.logical_and((abs(ddq) < ZERO_ACC_THRESHOLD),
(abs(dq) > ZERO_VEL_THRESHOLD))
# erode to get only steady phases where velocity is constant
maskConstVel = ndimage.morphology.binary_erosion(maskConstVel, None, 100)
maskPosVel = (dq > ZERO_VEL_THRESHOLD)
maskNegVel = (dq < -ZERO_VEL_THRESHOLD)
maskConstPosVel = np.logical_and(maskConstVel, maskPosVel)
maskConstNegVel = np.logical_and(maskConstVel, maskNegVel)
if SHOW_THRESHOLD_EFFECT:
plt.figure()
time = np.arange(0, dt * ddq.shape[0], dt)
plt.plot(time, ddq, label='ddq')
plt.ylabel('ddq')
plt.plot(time[maskConstVel],
ddq[maskConstVel],
'rx ',
label='ddq const vel')
plt.legend()
plt.figure()
plt.plot(dq)
plt.ylabel('dq')
dq_const = dq.copy()
dq_const[np.logical_not(maskConstVel)] = np.nan
plt.plot(dq_const)
plt.ylabel('dq_const')
plt.show()
# Identification of BEMF effect ************************************
times = np.arange(len(dq)) * dt
plt.subplot(221)
plt.plot(times, dq, lw=1)
vels = kmeans(dq[maskConstVel], Nvel)
# print('Velocity founds are:', vels)
couleurs = ['g', 'r', 'c', 'm', 'y', 'k'] * 10 # why not?
masksVels = []
av_dq = [] # List of point kept for identification of BEMF effect
av_delta_i = []
it = 0
for vel in vels[0]:
it += 1
currentMask = np.logical_and(dq > vel - 0.1, dq < vel + 0.1)
currentMask = np.logical_and(currentMask, maskConstVel)
masksVels.append(currentMask)
plt.subplot(221)
plt.plot(times[currentMask], dq[currentMask], 'o' + couleurs[it])
plt.subplot(222)
plt.xlabel('control')
plt.ylabel('current')
plt.plot(ctrl[currentMask] / IN_OUT_GAIN, Ks * current[currentMask],
'x' + couleurs[it])
plt.subplot(223)
plt.xlabel('control - current')
plt.ylabel('velocity')
plt.plot(ctrl[currentMask] / IN_OUT_GAIN - Ks * current[currentMask],
dq[currentMask], 'x' + couleurs[it])
av_dq.append(np.mean(dq[currentMask]))
av_delta_i.append(
np.mean(ctrl[currentMask] / IN_OUT_GAIN -
Ks * current[currentMask]))
plt.plot(av_delta_i, av_dq, 'o')
av_dq = np.array(av_dq)
av_delta_i = np.array(av_delta_i)
av_dq_pos = av_dq[av_dq > 0]
av_dq_neg = av_dq[av_dq < 0]
av_delta_i_pos = av_delta_i[av_dq > 0]
av_delta_i_neg = av_delta_i[av_dq < 0]
(ap, bp) = solve1stOrderLeastSquare(av_delta_i_pos, av_dq_pos)
(an, bn) = solve1stOrderLeastSquare(av_delta_i_neg, av_dq_neg)
a = (an + ap) / 2
b = (-bp + bn) / 2
DeadZone = b / a
# the half of the total dead zone
K_bemf = 1.0 / a
x = av_delta_i
plt.plot([-b / a, b / a], [0., 0.], 'g:', lw=3)
plt.plot([min(x), -b / a], [a * min(x) + b, 0.], 'g:', lw=3)
plt.plot([b / a, max(x)], [0., a * max(x) - b], 'g:', lw=3)
plt.show()
# y = a. x + b
# i-Kt.tau = Kv.dq + Kf
#
# Identification with fixed Kt ***************************************************
y = Ks * current - Ktp * tau
y[maskConstPosVel] = Ks * current[maskConstPosVel] - Ktp * tau[
maskConstPosVel]
y[maskConstNegVel] = Ks * current[maskConstNegVel] - Ktn * tau[
maskConstNegVel]
x = dq
(a, b) = solve1stOrderLeastSquare(x[maskConstPosVel], y[maskConstPosVel])
Kvp = a
Kfp = b
(a, b) = solve1stOrderLeastSquare(x[maskConstNegVel], y[maskConstNegVel])
Kvn = a
Kfn = -b
# Plot *************************************************************
plt.figure()
plt.axhline(0, color='black', lw=1)
plt.axvline(0, color='black', lw=1)
plt.plot(x, y, '.', lw=3, markersize=1, c='0.5')
plt.plot(x[maskConstPosVel], y[maskConstPosVel], 'rx', lw=3, markersize=1)
plt.plot(x[maskConstNegVel], y[maskConstNegVel], 'bx', lw=3, markersize=1)
# plot identified lin model
plt.plot([0.0, max(dq)], [Kfp, Kvp * max(dq) + Kfp], 'g-')
plt.plot([0.0, min(dq)], [-Kfn, Kvn * min(dq) - Kfn], 'g-')
plt.ylabel(r'$i(t)-{K_t}{\tau(t)}$')
plt.xlabel(r'$\dot{q}(t)$')
plt.title('Fixed Kt identification')
# Identification with variable Kt ***************************************************
# y = Ks*current
# A=np.vstack([np.ones(len(y[maskConstPosVel])),dq[maskConstPosVel], tau[maskConstPosVel]])
# coef = solveLeastSquare(A.T,y[maskConstPosVel])
# (Ktp2,Kvp2,Kfp2)=coef[2,0],coef[1,0],coef[0,0]
# A=np.vstack([np.ones(len(y[maskConstNegVel])),dq[maskConstNegVel], tau[maskConstNegVel]])
# coef = solveLeastSquare(A.T,y[maskConstNegVel])
# (Ktn2,Kvn2,Kfn2)=coef[2,0],coef[1,0],-coef[0,0]
# print 'Ktp2 = ', Ktp2;
# print 'Kvp2 = ', Kvp2;
# print 'Kfp2 = ', Kfp2;
# print 'Ktn2 = ', Ktn2;
# print 'Kvn2 = ', Kvn2;
# print 'Kfn2 = ', Kfn2;
# y = Ks*current-Ktp2*tau
# y[maskConstPosVel] = Ks*current[maskConstPosVel]-Ktp2*tau[maskConstPosVel]
# y[maskConstNegVel] = Ks*current[maskConstNegVel]-Ktn2*tau[maskConstNegVel]
# plt.figure()
# plt.axhline(0, color='black',lw=1)
# plt.axvline(0, color='black',lw=1)
# plt.plot(x ,y ,'.' ,lw=3,markersize=1,c='0.5');
# plt.plot(x[maskConstPosVel],y[maskConstPosVel],'rx',lw=3,markersize=1);
# plt.plot(x[maskConstNegVel],y[maskConstNegVel],'bx',lw=3,markersize=1);
# #plot identified lin model
# plt.plot([0.0,max(dq)],[ Kfp2,Kvp2*max(dq)+Kfp2],'g-')
# plt.plot([0.0,min(dq)],[-Kfn2,Kvn2*min(dq)-Kfn2],'g-')
# plt.ylabel(r'$i(t)-{K_t}{\tau(t)}$')
# plt.xlabel(r'$\dot{q}(t)$')
# plt.title('Variable Kt identification')
#
# Plot to compare identification with variable/fixed Kt *****************************
# plt.figure()
# plt.plot(Ks*current, label='current');
# plt.plot(Ktp*tau, '--', label='Ktp*tau')
# plt.plot(Kvp*dq, '--', label='Kvp*dq');
# plt.plot(Ktp*tau+Kvp*dq+Kfp, label='Ktp*tau+Kvp*dq+Kfp');
# plt.plot(Ktn*tau+Kvn*dq-Kfn, label='Ktn*tau+Kvn*dq-Kfn');
# plt.plot(Ktp2*tau+Kvp2*dq+Kfp2, label='Ktp2*tau+Kvp2*dq+Kfp2');
# plt.plot(Ktn2*tau+Kvn2*dq-Kfn2, label='Ktn2*tau+Kvn2*dq-Kfn2');
# plt.legend();
plt.show()
return (Kvp, Kvn, Kfp, Kfn, DeadZone, K_bemf)
def identify_motor_static(enc, dq, ctrl, current, tau, JOINT_ID, JOINT_NAME,
ZERO_VELOCITY_THRESHOLD,
ZERO_VELOCITY_THRESHOLD_SMALL,
SHOW_THRESHOLD_EFFECT):
# remove high velocity
maskConstAng = (abs(dq) < ZERO_VELOCITY_THRESHOLD)
# erode to get only steady phases where velocity is small
maskConstAng = ndimage.morphology.binary_erosion(maskConstAng, None, 100)
maskPosVel = (dq > ZERO_VELOCITY_THRESHOLD_SMALL)
maskNegVel = (dq < -ZERO_VELOCITY_THRESHOLD_SMALL)
maskConstPosAng = np.logical_and(maskConstAng, maskPosVel)
maskConstNegAng = np.logical_and(maskConstAng, maskNegVel)
if SHOW_THRESHOLD_EFFECT:
plt.figure()
plt.plot(enc, label='q')
q_const = enc.copy()
q_const[np.logical_not(maskConstAng)] = np.nan
plt.plot(q_const, label='q_const')
plt.legend()
# identify current sensor gain
x = current[maskConstAng]
y = ctrl[maskConstAng] / IN_OUT_GAIN
maskPosErr = np.logical_and(y - x > 0.0, np.abs(x) > 0.5)
maskNegErr = np.logical_and(y - x < 0.0, np.abs(x) > 0.5)
print "Number of samples with constant angle:", x.shape[0]
print "Number of samples with constant angle and pos vel:", x[
maskPosErr].shape[0]
print "Number of samples with constant angle and neg vel:", x[
maskNegErr].shape[0]
if (x[maskPosErr].shape[0] < 10):
(Ks, DZ) = solve1stOrderLeastSquare(x[maskNegErr], y[maskNegErr])
elif (x[maskNegErr].shape[0] < 10):
(Ks, DZ) = solve1stOrderLeastSquare(x[maskPosErr], y[maskPosErr])
else:
(Ksn, DZn) = solve1stOrderLeastSquare(x[maskNegErr], y[maskNegErr])
(Ksp, DZp) = solve1stOrderLeastSquare(x[maskPosErr], y[maskPosErr])
Ks = 0.5 * (Ksp + Ksn)
Ks = min([Ksp, Ksn])
DZ = 0.5 * (DZp - DZn)
print "Current sensor gains = ", Ksp, Ksn
print "Deadzones = ", DZp, -DZn
x_neg = x[maskNegErr]
y_neg = y[maskNegErr]
plt.figure()
plt.plot(x_neg, y_neg, '.', lw=3, markersize=1, c='0.5')
plt.plot([min(x_neg), max(x_neg)],
[Ksn * min(x_neg) + DZn, Ksn * max(x_neg) + DZn],
'g:',
lw=3)
plt.ylabel(r'$i(t)$')
plt.xlabel(r'$u(t)$')
plt.title('Negative current errors - Joint ' + JOINT_NAME)
x_pos = x[maskPosErr]
y_pos = y[maskPosErr]
plt.figure()
plt.plot(x_pos, y_pos, '.', lw=3, markersize=1, c='0.5')
plt.plot([min(x_pos), max(x_pos)],
[Ksp * min(x_pos) + DZp, Ksp * max(x_pos) + DZp],
'g:',
lw=3)
plt.ylabel(r'$i(t)$')
plt.xlabel(r'$u(t)$')
plt.title('Positive current errors - Joint ' + JOINT_NAME)
plt.show()
if (Ks < 0.0):
print "ERROR: estimated Ks is negative! Setting it to 1"
Ks = 1.0
# plot dead zone effect ********************************************
plt.figure()
plt.plot(Ks * current, label='current')
plt.plot(ctrl / IN_OUT_GAIN, label='control')
plt.legend()
plt.figure()
y = Ks * current[maskConstAng]
x = ctrl[maskConstAng] / IN_OUT_GAIN - Ks * current[maskConstAng]
plt.ylabel(r'$i(t)$')
plt.xlabel(r'$ctrl(t)-i(t)$')
plt.plot(x, y, '.', lw=3, markersize=1, c='0.5')
plt.plot(x[maskPosErr],
y[maskPosErr],
'rx',
lw=3,
markersize=1,
label='pos err')
plt.plot(x[maskNegErr],
y[maskNegErr],
'bx',
lw=3,
markersize=1,
label='neg err')
plt.legend()
plt.figure()
y = ctrl[maskConstAng] / IN_OUT_GAIN
x = ctrl[maskConstAng] / IN_OUT_GAIN - Ks * current[maskConstAng]
plt.ylabel(r'$ctrl(t)$')
plt.xlabel(r'$ctrl(t)-i(t)$')
plt.plot(x, y, '.', lw=3, markersize=1, c='0.5')
plt.plot(x[maskPosErr],
y[maskPosErr],
'rx',
lw=3,
markersize=1,
label='pos err')
plt.plot(x[maskNegErr],
y[maskNegErr],
'bx',
lw=3,
markersize=1,
label='neg err')
plt.legend()
plt.figure()
y = ctrl / IN_OUT_GAIN
x = Ks * current
plt.ylabel(r'$ctrl(t)$')
plt.xlabel(r'$i(t)$')
plt.plot(x, y, '.', lw=3, markersize=1, c='0.5')
plt.plot([-3, 3], [-3, 3])
plt.show()
# y = a. x + b
# i = Kt.tau + Kf
# Identification ***************************************************
y = current #*Ks
x = tau
(Ktp, Kfp) = solve1stOrderLeastSquare(x[maskConstPosAng],
y[maskConstPosAng])
(Ktn, b) = solve1stOrderLeastSquare(x[maskConstNegAng], y[maskConstNegAng])
Kfn = -b
# Plot *************************************************************
plt.figure()
plt.axhline(0, color='black', lw=1)
plt.axvline(0, color='black', lw=1)
plt.plot(x, y, '.', lw=3, markersize=1, c='0.5')
plt.plot(x[maskConstPosAng], y[maskConstPosAng], 'rx', lw=3, markersize=1)
plt.plot(x[maskConstNegAng], y[maskConstNegAng], 'bx', lw=3, markersize=1)
#plot identified lin model
plt.plot([min(x), max(x)], [Ktp * min(x) + Kfp, Ktp * max(x) + Kfp],
'g:',
lw=3)
plt.plot([min(x), max(x)], [Ktn * min(x) - Kfn, Ktn * max(x) - Kfn],
'g:',
lw=3)
plt.ylabel(r'$i(t)$')
plt.xlabel(r'$\tau(t)$')
plt.title('Static experiment - Joint ' + JOINT_NAME)
print "cur_sens_gain[%d] = %f" % (JOINT_ID, Ks)
print 'deadzone[%d] = %f' % (JOINT_ID, DZ)
print 'Kt_p[%d] = %f' % (JOINT_ID, Ktp)
print 'Kt_n[%d] = %f' % (JOINT_ID, Ktn)
print 'Kf_p[%d] = %f' % (JOINT_ID, Kfp)
print 'Kf_n[%d] = %f' % (JOINT_ID, Kfn)
print 'Kt_m[%d] = %f' % (JOINT_ID, (Ktp + Ktn) / 2.0)
print 'Kf_m[%d] = %f' % (JOINT_ID, (Kfp + Kfn) / 2.0)
return (Ktp, Ktn, Ks, DZ)
