def test_force_jacobians(device, estimator_ft, torque_ctrl, traj_gen,
ctrl_manager, inv_dyn, dt):
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
ctrl_manager.setCtrlMode("rhr", "torque")
ctrl_manager.setCtrlMode("rhp", "torque")
ctrl_manager.setCtrlMode("rhy", "torque")
ctrl_manager.setCtrlMode("rk", "torque")
ctrl_manager.setCtrlMode("rap", "torque")
ctrl_manager.setCtrlMode("rar", "torque")
inv_dyn.Kp.value = NJ * (0.0, )
inv_dyn.Kd.value = NJ * (0.0, )
inv_dyn.Kf.value = (6 * 4) * (1.0, )
N = 1
#10*1000;
axis = 2
FORCE_MAX = 0.1
tauFB1 = np.zeros((N, 30))
tauFB2 = np.zeros((N, 30))
traj_gen.startForceSinusoid('rf', axis, FORCE_MAX, 1.5)
#name, axis, final force, time
for i in range(N):
if (i == 1500):
traj_gen.stopForce('rf')
traj_gen.startForceSinusoid('rf', axis, -FORCE_MAX, 1.5)
#name, axis, final force, time
device.increment(dt)
sleep(dt)
if (USE_ROBOT_VIEWER and i % 100 == 0):
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [
0.0,
] * 10)
inv_dyn.tauFB2.recompute(i)
tauFB1[i, :] = inv_dyn.tauFB.value
tauFB2[i, :] = inv_dyn.tauFB2.value
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(tauFB1[:, 0], 'r')
ax[0].plot(tauFB2[:, 0], 'b--')
ax[1].plot(tauFB1[:, 1], 'r')
ax[1].plot(tauFB2[:, 1], 'b--')
ax[2].plot(tauFB1[:, 2], 'r')
ax[2].plot(tauFB2[:, 2], 'b--')
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(tauFB1[:, 3], 'r')
ax[0].plot(tauFB2[:, 3], 'b--')
ax[1].plot(tauFB1[:, 4], 'r')
ax[1].plot(tauFB2[:, 4], 'b--')
ax[2].plot(tauFB1[:, 5], 'r')
ax[2].plot(tauFB2[:, 5], 'b--')
plt.show()
def plot_bounded_joint_quantity(time,
x,
X_MIN,
X_MAX,
name,
xlabel='',
ylabel=''):
mpl.rcParams['font.size'] = 30
mpl.rcParams['axes.labelsize'] = 30
f, ax = plut.create_empty_figure(4, 2)
ax = ax.reshape(8)
for j in range(7):
ax[j].plot(time, x[j, :].T, linewidth=LW)
ax[j].plot([time[0], time[-1]], [X_MIN[j], X_MIN[j]], 'r--')
ax[j].plot([time[0], time[-1]], [X_MAX[j], X_MAX[j]], 'r--')
ax[j].set_title('Joint ' + str(j))
ax[j].set_ylabel(ylabel)
ax[j].set_ylim([
np.min(x[j, :]) - 0.1 * (X_MAX[j] - X_MIN[j]),
np.max(x[j, :]) + 0.1 * (X_MAX[j] - X_MIN[j])
])
ax[6].set_xlabel(xlabel)
ax[7].set_xlabel(xlabel)
ax[0].set_title(name)
#plut.saveFigure(TEST_NAME+'_'+name);
plut.saveFigureandParameterinDateFolder(GARBAGE_FOLDER,
TEST_NAME + '_' + name, PARAMS)
return ax
def test_admittance_ctrl(device, ctrl_manager, traj_gen, estimator_ft, adm_ctrl, dt=0.001):
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
N = 10*1000;
axis = 1;
fDes = np.zeros((N,6));
f = np.zeros((N,6));
ctrl_manager.setCtrlMode('all', 'adm');
traj_gen.startForceSinusoid('lf',axis, 100, 1.5); #name, axis, final force, time
adm_ctrl.fLeftFoot.value = 6*(0,);
adm_ctrl.fRightFoot.value = 6*(0,);
for i in range(N):
if(i==1500):
traj_gen.stopForce('lf');
traj_gen.startForceSinusoid('lf',axis, -100, 1.5); #name, axis, final force, time
device.increment (dt);
sleep(dt);
fDes[i,:] = traj_gen.fLeftFoot.value;
f[i,:] = estimator_ft.contactWrenchLeftFoot.value;
if(USE_ROBOT_VIEWER and i%30==0):
viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
if(i%100==0):
# print "f(%.3f) = %.3f, \tfDes = %.3f" % (device.robotState.time*dt,f[i,axis],fDes[i,axis]);
print ("fLeftFootError", adm_ctrl.fLeftFootError.value);
print ("fRightFootError", adm_ctrl.fRightFootError.value);
print ("dqDes", adm_ctrl.dqDes.value);
(fig,ax) = create_empty_figure(3,1);
ax[0].plot(fDes[:,0],'r'); ax[0].plot(f[:,0],'b');
ax[1].plot(fDes[:,1],'r'); ax[1].plot(f[:,1],'b');
ax[2].plot(fDes[:,2],'r'); ax[2].plot(f[:,2],'b');
plt.show();
def plot_polytope(A, b, V=None, color='b', ax=None, plotLines=True, lw=4):
if(ax==None):
f, ax = plut.create_empty_figure();
if(plotLines):
plot_inequalities(A, b, [-1,1], [-1,1], color=color, ls='--', ax=ax, lw=lw);
n = b.shape[0];
if(n<2):
return (ax,None);
if(V==None):
V = np.zeros((n,2));
for i in range(n):
V[i,:] = find_intersection(A[i,:], b[i], A[(i+1)%n,:], b[(i+1)%n]);
xx = np.zeros(2);
yy = np.zeros(2);
for i in range(n):
xx[0] = V[i,0];
xx[1] = V[(i+1)%n,0];
yy[0] = V[i,1];
yy[1] = V[(i+1)%n,1];
line, = ax.plot(xx, yy, color=color, ls='-', lw=2*lw);
return (ax, line);
def test_sinusoid(device, traj_gen):
print "\nGonna start sinusoid to 0.2...";
traj_gen.startSinusoid('rhp', 0.2, tt);
qSin = np.zeros(4*N);
dqSin = np.zeros(4*N);
ddqSin = np.zeros(4*N);
q = np.zeros(4*N);
dq = np.zeros(4*N);
for i in range(4*N):
device.increment (dt);
qSin[i] = traj_gen.q.value[jid];
dqSin[i] = traj_gen.dq.value[jid];
ddqSin[i] = traj_gen.ddq.value[jid];
q[i] = device.robotState.value[6+jid];
dq[i] = device.jointsVelocities.value[jid];
if(USE_ROBOT_VIEWER and i%30==0):
viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
if(i%100==0):
print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],device.control.value[jid]);
traj_gen.stop('rhp');
print "q(%.3f) = %f\n" % (device.robotState.time*dt,device.robotState.value[6+2]);
dq_fd = np.diff(qSin)/dt;
ddq_fd = np.diff(dqSin)/dt;
(fig,ax) = create_empty_figure(3,1);
ax[0].plot(qSin,'r'); ax[0].plot(q,'b');
ax[1].plot(dqSin,'r'); ax[1].plot(dq,'b'); ax[1].plot(dq_fd,'g--');
ax[2].plot(ddqSin,'r'); ax[2].plot(ddq_fd,'g--');
plt.show();
def test_min_jerk(device, traj_gen):
print "\nGonna move joint to -1.5...";
tt = 1.5;
jid = 2;
traj_gen.moveJoint('rhp', -1.5, tt);
N = int(tt/dt);
qMinJerk = np.zeros(N+2);
dqMinJerk = np.zeros(N+2);
ddqMinJerk = np.zeros(N+2);
q = np.zeros(N+2);
dq = np.zeros(N+2);
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
for i in range(N+2):
device.increment (dt);
qMinJerk[i] = traj_gen.q.value[jid];
dqMinJerk[i] = traj_gen.dq.value[jid];
ddqMinJerk[i] = traj_gen.ddq.value[jid];
q[i] = device.robotState.value[6+jid];
dq[i] = device.jointsVelocities.value[jid];
if(USE_ROBOT_VIEWER and i%30==0):
viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
if(i%100==0):
print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]);
dq_fd = np.diff(qMinJerk)/dt;
ddq_fd = np.diff(dqMinJerk)/dt;
(fig,ax) = create_empty_figure(3,1);
ax[0].plot(qMinJerk,'r'); ax[0].plot(q,'b');
ax[1].plot(dqMinJerk,'r'); ax[1].plot(dq,'b'); ax[1].plot(dq_fd,'g--');
ax[2].plot(ddqMinJerk,'r'); ax[2].plot(ddq_fd,'g--');
plt.show();
def plot_polytope(A, b, V=None, color='b', ax=None, plotLines=True, lw=4):
A = np.asarray(A)
b = np.asarray(b)
if (ax == None):
f, ax = plut.create_empty_figure()
if (V == None):
try:
V = poly_face_to_span(A, b).T
except (ValueError, NotPolyFace) as e:
print "WARNING: " + str(e)
if (V == None):
X_MIN = -1.
X_MAX = 1.
Y_MIN = -1.
Y_MAX = 1.
else:
X_MIN = np.min(V[:, 0])
X_MAX = np.max(V[:, 0])
X_MIN -= 0.1 * (X_MAX - X_MIN)
X_MAX += 0.1 * (X_MAX - X_MIN)
Y_MIN = np.min(V[:, 1])
Y_MAX = np.max(V[:, 1])
Y_MIN -= 0.1 * (Y_MAX - Y_MIN)
Y_MAX += 0.1 * (Y_MAX - Y_MIN)
if (plotLines):
plot_inequalities(A,
b, [X_MIN, X_MAX], [Y_MIN, Y_MAX],
color=color,
ls='--',
ax=ax,
lw=lw)
n = b.shape[0]
if (n < 2):
return (ax, None)
line = None
if (V != None):
xx = np.zeros(2)
yy = np.zeros(2)
for i in range(n):
xx[0] = V[i, 0]
xx[1] = V[(i + 1) % n, 0]
yy[0] = V[i, 1]
yy[1] = V[(i + 1) % n, 1]
line, = ax.plot(xx, yy, color='r', ls='o', markersize=30)
#, lw=2*lw);
ax.set_xlim([X_MIN, X_MAX])
ax.set_ylim([Y_MIN, Y_MAX])
return (ax, line)
def test_chirp(device, traj_gen, estimator_ft):
device.after.addDownsampledSignal('estimator_ft.ftSensRightFootPrediction',
1)
print "Start linear chirp from %f to -1.0" % device.robotState.value[6 + 2]
tt = 4
jid = 1
traj_gen.startLinChirp('rhr', -0.6, 0.2, 2.0, tt)
N = int(tt / dt)
qChirp = np.zeros(N + 2)
dqChirp = np.zeros(N + 2)
ddqChirp = np.zeros(N + 2)
q = np.zeros(N + 2)
dq = np.zeros(N + 2)
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [
0.0,
] * 10)
for i in range(N + 2):
device.increment(dt)
qChirp[i] = traj_gen.q.value[jid]
dqChirp[i] = traj_gen.dq.value[jid]
ddqChirp[i] = traj_gen.ddq.value[jid]
q[i] = device.robotState.value[6 + jid]
dq[i] = device.jointsVelocities.value[jid]
# print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]);
# print "dq(%.3f) = %.3f, \tdqDes = %.3f" % (device.robotState.time*dt,device.jointsVelocities.value[6+jid],traj_gen.dq.value[jid]);
# print "tauDes(%.3f)= %.3f" % (device.robotState.time*dt,inv_dyn.tauDes.value[jid]);
# sleep(0.1);
if (USE_ROBOT_VIEWER and i % 30 == 0):
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [
0.0,
] * 10)
if (i % 100 == 0):
print("FT sensor right foot prediction: ",
estimator_ft.ftSensRightFootPrediction.value)
# print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]);
dq_fd = np.diff(qChirp) / dt
ddq_fd = np.diff(dqChirp) / dt
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(qChirp, 'r')
ax[0].plot(q, 'b')
ax[1].plot(dqChirp, 'r')
ax[1].plot(dq, 'b')
ax[1].plot(dq_fd, 'g--')
ax[2].plot(ddqChirp, 'r')
ax[2].plot(ddq_fd, 'g--')
plt.show()
def plot_inequalities(A,
b,
x_bounds,
y_bounds,
ls='--',
color='k',
ax=None,
lw=8):
if (A.shape[1] != 2):
print "[ERROR in plot_inequalities] matrix does not have 2 columns"
return
# if(A.shape[0]!=len(b)):
# print "[ERROR in plot_inequalities] matrix and vector does not have the same number of rows";
# return;
if (ax == None):
f, ax = plut.create_empty_figure()
p = np.zeros(2)
# p height
p_x = np.zeros(2)
p_y = np.zeros(2)
for i in range(A.shape[0]):
if (np.abs(A[i, 1]) > 1e-13):
p_x[0] = x_bounds[0]
# p x coordinate
p_x[1] = x_bounds[1]
# p x coordinate
p[0] = p_x[0]
p[1] = 0
p_y[0] = (b[i] - np.dot(A[i, :], p)) / A[i, 1]
p[0] = p_x[1]
p_y[1] = (b[i] - np.dot(A[i, :], p)) / A[i, 1]
ax.plot(p_x, p_y, ls=ls, color=color, linewidth=lw)
elif (np.abs(A[i, 0]) > 1e-13):
p_y[0] = y_bounds[0]
p_y[1] = y_bounds[1]
p[0] = 0
p[1] = p_y[0]
p_x[0] = (b[i] - np.dot(A[i, :], p)) / A[i, 0]
p[1] = p_y[1]
p_x[1] = (b[i] - np.dot(A[i, :], p)) / A[i, 0]
ax.plot(p_x, p_y, ls=ls, color=color, linewidth=lw)
else:
pass
# print "[WARNING] Could not print one inequality as all coefficients are 0: A[%d,:]=[%f,%f]" % (i,A[i,0],A[i,1]);
return ax
def test_sinusoid(device, traj_gen, dt=0.001):
tt = 1.5
jid = 2
N = int(tt / dt)
print("\nGonna start sinusoid to 0.2...")
traj_gen.startSinusoid('rhp', 0.2, tt)
qSin = np.zeros(4 * N)
dqSin = np.zeros(4 * N)
ddqSin = np.zeros(4 * N)
q = np.zeros(4 * N)
dq = np.zeros(4 * N)
for i in range(4 * N):
device.increment(dt)
qSin[i] = traj_gen.q.value[jid]
dqSin[i] = traj_gen.dq.value[jid]
ddqSin[i] = traj_gen.ddq.value[jid]
q[i] = device.robotState.value[6 + jid]
dq[i] = device.jointsVelocities.value[jid]
if USE_ROBOT_VIEWER and i % 30 == 0:
viewer.updateElementConfig('hrp_device',
list(device.state.value) +
[0.0] * 10) # noqa
if i % 100 == 0:
print(
"q(%.3f) = %.3f, \tqDes = %.3f" %
(device.robotState.time * dt, device.robotState.value[6 + jid],
device.control.value[jid]))
traj_gen.stop('rhp')
print("q(%.3f) = %f\n" %
(device.robotState.time * dt, device.robotState.value[6 + 2]))
dq_fd = np.diff(qSin) / dt
ddq_fd = np.diff(dqSin) / dt
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(qSin, 'r')
ax[0].plot(q, 'b')
ax[1].plot(dqSin, 'r')
ax[1].plot(dq, 'b')
ax[1].plot(dq_fd, 'g--')
ax[2].plot(ddqSin, 'r')
ax[2].plot(ddq_fd, 'g--')
plt.show()
def main(dt):
np.set_printoptions(precision=2, suppress=True)
jid = 0
T = 2.0
x_0 = (1.0, 2.0, 3.0)
x_f = (2.0, -3.0, 4.0)
traj_gen = NdTrajectoryGenerator('traj_gen')
traj_gen.init(dt, 3)
print "Gonna start sinusoid from %.1f to %.1f" % (x_0[jid], x_f[jid])
traj_gen.initial_value.value = x_0
traj_gen.x.recompute(0)
traj_gen.startSinusoid(jid, x_f[jid], T)
N = int(T / dt)
xSin = np.zeros(2 * N)
dxSin = np.zeros(2 * N)
ddxSin = np.zeros(2 * N)
for i in range(2 * N):
traj_gen.x.recompute(i)
traj_gen.dx.recompute(i)
traj_gen.ddx.recompute(i)
xSin[i] = traj_gen.x.value[jid]
dxSin[i] = traj_gen.dx.value[jid]
ddxSin[i] = traj_gen.ddx.value[jid]
if (i % 10 == 0):
print "x(%.3f) = %.3f, \tdx = %.3f" % (i * dt, xSin[i], dxSin[i])
traj_gen.stop(jid)
dx_fd = np.diff(xSin) / dt
ddx_fd = np.diff(dxSin) / dt
time = np.arange(0.0, 2 * T, dt)
print time.shape, xSin.shape
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(time, xSin, 'r')
ax[1].plot(time, dxSin, 'r')
ax[1].plot(time[:-1], dx_fd, 'g--')
ax[2].plot(time, ddxSin, 'r')
ax[2].plot(time[:-1], ddx_fd, 'g--')
plt.show()
return (traj_gen)
def plot_inequalities(F_com, f_com, x_bounds, y_bounds, ls='--', color='k', ax=None, lw=8):
if(F_com.shape[1]!=2):
print "[ERROR in plot_inequalities] matrix does not have 2 columns";
return;
# if(F_com.shape[0]!=len(f_com)):
# print "[ERROR in plot_inequalities] matrix and vector does not have the same number of rows";
# return;
if(ax==None):
f, ax = plut.create_empty_figure();
com = np.zeros(2); # com height
com_x = np.zeros(2);
com_y = np.zeros(2);
for i in range(F_com.shape[0]):
if(np.abs(F_com[i,1])>1e-13):
com_x[0] = x_bounds[0]; # com x coordinate
com_x[1] = x_bounds[1]; # com x coordinate
com[0] = com_x[0];
com[1] = 0;
com_y[0] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,1];
com[0] = com_x[1];
com_y[1] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,1];
ax.plot(com_x, com_y, ls=ls, color=color, linewidth=lw);
elif(np.abs(F_com[i,0])>1e-13):
com_y[0] = y_bounds[0];
com_y[1] = y_bounds[1];
com[0] = 0;
com[1] = com_y[0];
com_x[0] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,0];
com[1] = com_y[1];
com_x[1] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,0];
ax.plot(com_x, com_y, ls=ls, color=color, linewidth=lw);
else:
pass;
# print "[WARNING] Could not print one inequality as all coefficients are 0: F_com[%d,:]=[%f,%f]" % (i,F_com[i,0],F_com[i,1]);
return ax;
def test_min_jerk(device, traj_gen, dt=0.001):
print("\nGonna move joint to -1.5...")
tt = 1.5
jid = 2
traj_gen.moveJoint('rhp', -1.5, tt)
N = int(tt / dt)
qMinJerk = np.zeros(N + 2)
dqMinJerk = np.zeros(N + 2)
ddqMinJerk = np.zeros(N + 2)
q = np.zeros(N + 2)
dq = np.zeros(N + 2)
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
for i in range(N + 2):
device.increment(dt)
qMinJerk[i] = traj_gen.q.value[jid]
dqMinJerk[i] = traj_gen.dq.value[jid]
ddqMinJerk[i] = traj_gen.ddq.value[jid]
q[i] = device.robotState.value[6 + jid]
dq[i] = device.jointsVelocities.value[jid]
if (USE_ROBOT_VIEWER and i % 30 == 0):
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [0.0] * 10)
if i % 100 == 0:
print("q(%.3f) = %.3f, \tqDes = %.3f" %
(device.robotState.time * dt,
device.robotState.value[6 + jid], traj_gen.q.value[jid]))
dq_fd = np.diff(qMinJerk) / dt
ddq_fd = np.diff(dqMinJerk) / dt
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(qMinJerk, 'r')
ax[0].plot(q, 'b')
ax[1].plot(dqMinJerk, 'r')
ax[1].plot(dq, 'b')
ax[1].plot(dq_fd, 'g--')
ax[2].plot(ddqMinJerk, 'r')
ax[2].plot(ddq_fd, 'g--')
plt.show()
def main(dt):
np.set_printoptions(precision=2, suppress=True);
jid = 0;
T = 2.0;
x_0 = (1.0, 2.0, 3.0);
x_f = (2.0, -3.0, 4.0);
traj_gen = NdTrajectoryGenerator('traj_gen');
traj_gen.init(dt, 3);
print "Gonna start sinusoid from %.1f to %.1f" % (x_0[jid], x_f[jid]);
traj_gen.initial_value.value = x_0;
traj_gen.x.recompute(0);
traj_gen.startSinusoid(jid, x_f[jid], T);
N = int(T/dt);
xSin = np.zeros(2*N);
dxSin = np.zeros(2*N);
ddxSin = np.zeros(2*N);
for i in range(2*N):
traj_gen.x.recompute(i);
traj_gen.dx.recompute(i);
traj_gen.ddx.recompute(i);
xSin[i] = traj_gen.x.value[jid];
dxSin[i] = traj_gen.dx.value[jid];
ddxSin[i] = traj_gen.ddx.value[jid];
if(i%10==0):
print "x(%.3f) = %.3f, \tdx = %.3f" % (i*dt, xSin[i], dxSin[i]);
traj_gen.stop(jid);
dx_fd = np.diff(xSin)/dt;
ddx_fd = np.diff(dxSin)/dt;
time = np.arange(0.0, 2*T, dt);
print time.shape, xSin.shape
(fig,ax) = create_empty_figure(3,1);
ax[0].plot(time, xSin,'r');
ax[1].plot(time, dxSin,'r'); ax[1].plot(time[:-1], dx_fd,'g--');
ax[2].plot(time, ddxSin,'r'); ax[2].plot(time[:-1], ddx_fd,'g--');
plt.show();
return (traj_gen);
# print "Time %d viable acc range is %f" % (i, min(MAX_ACC,ddqUB_viab[i]) - max(-MAX_ACC,ddqLB_viab[i]));
# ddq[i] = 0.5*(ddqUB_viab[i]+ddqLB_viab[i]);
else:
ddq[i] = random(1)*(ddqMaxFinal - ddqMinFinal) + ddqMinFinal;
dq[i+1] = dq[i] + DT*ddq[i] #+ error_trigger*(DT*MAX_ACC*fraction*random(1) - DT*MAX_ACC*fraction*random(1)); # adding error;
q[i+1] = q[i] + DT*dq[i] + 0.5*(DT**2)*ddq[i] #+ error_trigger*(0.5*DT**2*MAX_ACC*fraction*random(1) - 0.5*DT**2*MAX_ACC*fraction*random(1)); # adding error;
print("Minimum ddq margin from viability upper bound: %f" % np.min(ddqUB_viab-ddq));
print("Minimum ddq margin from viability lower bound: %f" % np.min(ddq-ddqLB_viab));
if(PLOT_STATE_SPACE_PROBABILITY):
std_dev = MAX_ACC;
cmap=brewer2mpl.get_map('OrRd', 'sequential', 4, reverse=False).mpl_colormap;
(f,ax) = create_empty_figure(1,1, [0,0]);
# plot viability constraints
qMid = 0.5*(qMin+qMax);
q_plot = np.arange(qMid, qMax+Q_INTERVAL, Q_INTERVAL);
dq_plot = np.sqrt(np.max([np.zeros(q_plot.shape),2*MAX_ACC*(qMax-q_plot)],0));
ax.plot(q_plot,dq_plot, 'r--');
q_plot = np.arange(qMid, qMin-Q_INTERVAL, -Q_INTERVAL);
dq_plot = -np.sqrt(np.max([np.zeros(q_plot.shape),2*MAX_ACC*(q_plot-qMin)],0));
ax.plot(q_plot,dq_plot, 'r--');
q_plot = np.arange(qMid, qMax+Q_INTERVAL, Q_INTERVAL);
dq_plot = np.sqrt(np.max([np.zeros(q_plot.shape),1.4*MAX_ACC*(qMax-q_plot)],0));
ax.plot(q_plot,dq_plot, 'b--');
q_plot = np.arange(qMid, qMin-Q_INTERVAL, -Q_INTERVAL);
dq_plot = -np.sqrt(np.max([np.zeros(q_plot.shape),1.4*MAX_ACC*(q_plot-qMin)],0));
if i % conf.DISPLAY_N == 0:
tsid.robot_display.display(q[:, i])
tsid.gui.applyConfiguration('world/ee',
ee_pos[:, i].tolist() + [0, 0, 0, 1.])
tsid.gui.applyConfiguration('world/ee_ref',
ee_pos_ref[:, i].tolist() + [0, 0, 0, 1.])
time_spent = time.time() - time_start
if (time_spent < conf.dt): time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
if (PLOT_EE_POS):
(f, ax) = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, ee_pos[i, :], label='EE ' + str(i))
ax[i].plot(time, ee_pos_ref[i, :], 'r:', label='EE Ref ' + str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if (PLOT_EE_VEL):
(f, ax) = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, ee_vel[i, :], label='EE Vel ' + str(i))
ax[i].plot(time, ee_vel_ref[i, :], 'r:', label='EE Vel Ref ' + str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE Vel [m/s]')
print('Final e-e pose x(T))',
m2q(M).T)
print('Difference between desired and measured e-e pose: M.inverse()*M_des',
m2q(M.inverse() * M_des).T)
if (PLAY_TRAJECTORY_AT_THE_END):
if (CAPTURE_IMAGES):
robot.startCapture(IMAGES_FILE_NAME, path=GARBAGE_FOLDER + '/img/')
robot.play(q, DT)
if (CAPTURE_IMAGES):
robot.stopCapture()
time = np.arange(0, NT * DT, DT)
if (PLOT_END_EFFECTOR_POS):
f, ax = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, x[i, :].T, linewidth=LW)
ax[i].plot([time[0], time[-1]], [x_des[i], x_des[i]], 'r--')
#ax[0].legend(['Measured', 'Desired']);
ax[0].set_title('End-effector position', fontsize=25)
ax[0].set_ylabel('X [m]')
lege10 = mpatches.Patch(color='red', label='Desidered Position')
lege20 = mpatches.Patch(color='blue', label='Measured Position')
ax[0].legend(handles=[lege10, lege20],
loc='upper center',
bbox_to_anchor=(0.5, 1.0),
bbox_transform=plt.gcf().transFigure,
ncol=5,
fontsize=30)
ax[1].set_ylabel('Y [m]')
print "\ttracking err %s: %.3f" % (tsid.comTask.name.ljust(
20, '.'), norm(tsid.comTask.position_error, 2))
print "\t||v||: %.3f\t ||dv||: %.3f" % (norm(v, 2), norm(dv))
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
if i % conf.DISPLAY_N == 0: tsid.robot_display.display(q)
time_spent = time.time() - time_start
if (time_spent < conf.dt): time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
(f, ax) = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, com_pos[i, :].A1, label='CoM ' + str(i))
ax[i].plot(time, com_pos_ref[i, :].A1, 'r:', label='CoM Ref ' + str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3, 1)
for i in range(3):
ax[i].plot(time, com_vel[i, :].A1, label='CoM Vel ' + str(i))
ax[i].plot(time, com_vel_ref[i, :].A1, 'r:', label='CoM Vel Ref ' + str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Vel [m/s]')
leg = ax[i].legend()
plot_utils.DEFAULT_LINE_WIDTH = 5
N = 15000
f0 = 0.3
f1 = 3
tt = 15.0
dt = 0.001
phi_0 = np.pi * tt * (f0 - f1)
t = 0
x = np.zeros(N)
f = np.zeros(N)
phi = np.zeros(N)
k = 2 * (f1 - f0) / tt
for i in range(N):
if (t < 0.5 * tt):
f[i] = f0 + k * t
phi[i] = 2 * np.pi * t * (f0 + 0.5 * k * t)
else:
f[i] = f1 + 0.5 * k * tt - k * t
phi[i] = phi_0 + 2 * np.pi * t * (f1 + 0.5 * k * tt - 0.5 * k * t)
x[i] = 0.5 * (1.0 - np.cos(phi[i]))
t = t + dt
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(x)
ax[1].plot(f)
ax[2].plot(phi)
plt.show()
v_mean = v[:, i] + 0.5 * dt * dv[:, i]
v[:, i + 1] = v[:, i] + dt * dv[:, i]
q[:, i + 1] = se3.integrate(model, q[:, i], dt * v_mean)
t += conf.dt
if i % conf.DISPLAY_N == 0:
robot_display.display(q[:, i])
time_spent = time.time() - time_start
if (time_spent < conf.dt): time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
if (PLOT_JOINT_POS):
(f, ax) = plut.create_empty_figure(robot.nv / 2, 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, q[i, :-1].A1, label='Joint pos ' + str(i))
ax[i].plot(time, q_ref[i, :].A1, '--', label='Joint ref pos ' + str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint angles [rad]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if (PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(robot.nv / 2, 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, v[i, :-1].A1, label='Joint vel ' + str(i))
ax[i].plot(time, v_ref[i, :].A1, '--', label='Joint ref vel ' + str(i))
r'$w_i$')
ax.legend(handles=[lege1, lege2, lege3],
loc='upper center',
bbox_to_anchor=(0.5, 1.0),
bbox_transform=plt.gcf().transFigure,
ncol=5,
fontsize=30)
plut.saveFigureandParameterinDateFolder(GARBAGE_FOLDER,
TEST_NAME + '_j' + str(j),
PARAMS)
# Better for manage image
if (PLOT_SINGULAR):
(f, ax_poss) = create_empty_figure(1)
ax_poss.plot([time[0], time[-1]], [Q_MAX[j], Q_MAX[j]], 'r--')
ax_poss.plot([time[0], time[-1]], [Q_MIN[j], Q_MIN[j]], 'r--')
ax_poss.plot(time,
q[j, :, nt].squeeze(),
line_styles[nt],
linewidth=LW,
alpha=LINE_ALPHA**nt,
label=r'$\delta t=$' + str(int(DT_SAFE[nt] / DT)) +
'x')
ax_poss.set_ylabel(r'$q$ [rad]')
ax_poss.yaxis.set_ticks([Q_MIN[j], Q_MAX[j]])
ax_poss.yaxis.set_major_formatter(
ticker.FormatStrFormatter('%0.0f'))
ax_poss.set_ylim([np.min(q[j, :, :]), 1.1 * np.max(q[j, :, :])])
ax_poss.set_title('Position joint ' + str(j))
Z_viab_up[i,j]-= E;
(Z_low[i,j], Z_up[i,j]) = computeAccLimits_2(Q[i,j], DQ[i,j], qMin, qMax, MAX_VEL, MAX_ACC, DT,E);
if(Z_pos_up[i,j] == Z_up[i,j]):
Z_regions[i,j] = 2.3;
#print "Position bound is constraining at ", Q[i,j], DQ[i,j];
elif(Z_vel_up[i,j] == Z_up[i,j]):
Z_regions[i,j] = 1;
elif(MAX_ACC == Z_up[i,j]):
Z_regions[i,j] = 3;
elif(Z_viab_up[i,j] == Z_up[i,j]):
Z_regions[i,j] = 4;
else:
print("Error acc ub unidentified")
print("Min value of ddqMaxPos-ddqMaxViab = %f (should be >=0 if pos bound are redundant)" % np.min(Z_pos_up-Z_viab_up));
print("Max value of ddqMinPos-ddqMinViab = %f (should be <=0 if pos bound are redundant)" % np.max(Z_pos_low-Z_viab_low));
f, ax = create_empty_figure();
CF = ax.contourf(Q, DQ, Z_regions, 10, cmap=plt.get_cmap('Paired_r'));
#f.colorbar(CF, ax=ax, shrink=0.9);
ax.yaxis.set_major_formatter(ticker.FormatStrFormatter('%0.0f'));
ax.set_xlabel(r'$q$');
ax.set_ylabel(r'$\dot{q}$');
ax.xaxis.set_ticks([qMin, qMax]);
ax.yaxis.set_ticks([-MAX_VEL, 0, MAX_VEL]);
ax.set_ylim([-MAX_VEL, MAX_VEL]);
#ax.set_title('Acc regions');
plut.saveFigure('state_space_regions');
plt.show();
drone.set_state(np.array([0.0,0.0,1.0,0.0,0.0,0.0]),np.array([0.0,0.0,0.0,0.0,0.0,0.0]),np.array([620.6107625,-620.6107625,620.6107625,-620.6107625]))
q[:6,0] = np.array([0,0,1,0,0,0])
# simulate rotor and drone dynamics
for i in range(N+1):
Torque[:4,i]= np.array([0.04390797988,-0.043907979880,0.04390797988,-0.04390797988])
drone.simulate_rotors(Torque[:4,i])
drone.simulate_drone(drone.p())
q[:6,i] = drone.q()
#Plot stuffs
f, ax = plut.create_empty_figure(1)
time = np.arange(0.0, T+dt, dt)
time = time[:N+1]
ax.plot(time, q[0,:N+1], label ='x')
ax.legend()
matplot.pyplot.xlabel('Time [s]')
print("Final height", q[0,-1])
f, ax = plut.create_empty_figure(1)
time = np.arange(0.0, T+dt, dt)
time = time[:N+1]
ax.plot(time, q[1,:N+1], label ='y')
ax.legend()
matplot.pyplot.xlabel('Time [s]')
v_mean = v[:, i] + 0.5 * dt * dv[:, i]
v[:, i + 1] = v[:, i] + dt * dv[:, i]
q[:, i + 1] = se3.integrate(model, q[:, i], dt * v_mean)
t += conf.dt
if i % conf.DISPLAY_N == 0:
robot_display.display(q[:, i])
time_spent = time.time() - time_start
if (time_spent < conf.dt): time.sleep(conf.dt - time_spent)
# PLOT STUFF
time = np.arange(0.0, N * conf.dt, conf.dt)
if (PLOT_JOINT_POS):
(f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, q[i, :-1].A1, label='Joint pos ' + str(i))
ax[i].plot(time, q_ref[i, :].A1, '--', label='Joint ref pos ' + str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint angles [rad]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if (PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(int(robot.nv / 2), 2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, v[i, :-1].A1, label='Joint vel ' + str(i))
ax[i].plot(time, v_ref[i, :].A1, '--', label='Joint ref vel ' + str(i))
print('#' * 60)
print('############ Maximum Deceleration ############ ')
print('#' * 60)
LW = 2
def square_drawer(qmin, qmax, dqmin, dqmax, ax, color='r--'):
ax.plot([qmin, qmax], [dqmin, dqmin], color, linewidth=LW)
ax.plot([qmin, qmax], [dqmax, dqmax], color, linewidth=LW)
ax.plot([qmin, qmin], [dqmin, dqmax], color, linewidth=LW)
ax.plot([qmax, qmax], [dqmin, dqmax], color, linewidth=LW)
return
(f, ax1) = plut.create_empty_figure(1)
square_drawer(X_all[0], X_all[1], 0, -X_all[3], ax1)
square_drawer(X_all[0], X_all[1], 0, X_all[3], ax1)
ax1.set_xlim([-0.0025, 0.5025])
ax1.set_ylim([-0.01, 2.01])
PP = np.load('q_viable.npy')
RR = np.load('q_not_viable_neg.npy')
PP_line = []
RR_line = []
PP = PP.tolist()
for i in range(int(size(PP) / 2) - 1):
if (PP[0][0] == PP[1][0]):
PP.pop(0)
ddq[i] += E
#*(random(1)/2-random(1)/2);
dq[i + 1] = dq[i] + DT * ddq[i]
q[i + 1] = q[i] + DT * dq[i] + 0.5 * (DT**2) * ddq[i]
print("Minimum ddq margin from viability upper bound: %f" %
np.min(ddqUB_viab - ddq))
print("Minimum ddq margin from viability lower bound: %f" %
np.min(ddq - ddqLB_viab))
if (PLOT_STATE_SPACE_PROBABILITY):
std_dev = MAX_ACC
cmap = brewer2mpl.get_map('OrRd', 'sequential', 4,
reverse=False).mpl_colormap
(f, ax) = create_empty_figure(1, 1, [0, 0])
# plot viability constraints
qMid = 0.5 * (qMin + qMax)
q_plot = np.arange(qMid, qMax + Q_INTERVAL, Q_INTERVAL)
dq_plot = np.sqrt(
np.max([np.zeros(q_plot.shape), 2 * MAX_ACC * (qMax - q_plot)], 0))
ax.plot(q_plot, dq_plot, 'r--')
q_plot = np.arange(qMid, qMin - Q_INTERVAL, -Q_INTERVAL)
dq_plot = -np.sqrt(
np.max([np.zeros(q_plot.shape), 2 * MAX_ACC * (q_plot - qMin)], 0))
ax.plot(q_plot, dq_plot, 'r--')
q_plot = np.arange(qMid, qMax + Q_INTERVAL, Q_INTERVAL)
dq_plot = np.sqrt(
np.max([np.zeros(q_plot.shape), 1.4 * MAX_ACC * (qMax - q_plot)], 0))
