Skip to content
GitLab
Commit 90c2a23c authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Continue update scripts in folder exercizes to use numpy.array instead of matrix.

parent 8eb54948
Hide whitespace changes
Inline Side-by-side
exercizes/ex_1_ur5.py
View file @ 90c2a23c
import numpy as np
import numpy.matlib as matlib
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
......@@ -23,28 +22,28 @@ PLOT_TORQUES = 1
tsid = TsidManipulator(conf)
N = conf.N_SIMULATION
tau = matlib.empty((tsid.robot.na, N))*nan
q = matlib.empty((tsid.robot.nq, N+1))*nan
v = matlib.empty((tsid.robot.nv, N+1))*nan
ee_pos = matlib.empty((3, N))*nan
ee_vel = matlib.empty((3, N))*nan
ee_acc = matlib.empty((3, N))*nan
ee_pos_ref = matlib.empty((3, N))*nan
ee_vel_ref = matlib.empty((3, N))*nan
ee_acc_ref = matlib.empty((3, N))*nan
ee_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
tau = np.empty((tsid.robot.na, N))*nan
q = np.empty((tsid.robot.nq, N+1))*nan
v = np.empty((tsid.robot.nv, N+1))*nan
ee_pos = np.empty((3, N))*nan
ee_vel = np.empty((3, N))*nan
ee_acc = np.empty((3, N))*nan
ee_pos_ref = np.empty((3, N))*nan
ee_vel_ref = np.empty((3, N))*nan
ee_acc_ref = np.empty((3, N))*nan
ee_acc_des = np.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
sampleEE = tsid.trajEE.computeNext()
samplePosture = tsid.trajPosture.computeNext()
offset = sampleEE.pos()
offset[:3,0] += conf.offset
two_pi_f_amp = np.multiply(conf.two_pi_f, conf.amp)
two_pi_f_squared_amp = np.multiply(conf.two_pi_f, two_pi_f_amp)
offset[:3] += conf.offset
two_pi_f_amp = conf.two_pi_f * conf.amp
two_pi_f_squared_amp = conf.two_pi_f * two_pi_f_amp
pEE = offset.copy()
vEE = matlib.zeros((6,1))
aEE = matlib.zeros((6,1))
vEE = np.zeros(6)
aEE = np.zeros(6)
tsid.gui.addSphere('world/ee', conf.SPHERE_RADIUS, conf.EE_SPHERE_COLOR)
tsid.gui.addSphere('world/ee_ref', conf.REF_SPHERE_RADIUS, conf.EE_REF_SPHERE_COLOR)
......@@ -55,9 +54,9 @@ q[:,0], v[:,0] = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
pEE[:3,0] = offset[:3,0] + np.multiply(conf.amp, matlib.sin(conf.two_pi_f*t + conf.phi))
vEE[:3,0] = np.multiply(two_pi_f_amp, matlib.cos(conf.two_pi_f*t + conf.phi))
aEE[:3,0] = np.multiply(two_pi_f_squared_amp, -matlib.sin(conf.two_pi_f*t + conf.phi))
pEE[:3] = offset[:3] + conf.amp * np.sin(conf.two_pi_f*t + conf.phi)
vEE[:3] = two_pi_f_amp * np.cos(conf.two_pi_f*t + conf.phi)
aEE[:3] = -two_pi_f_squared_amp * np.sin(conf.two_pi_f*t + conf.phi)
sampleEE.pos(pEE)
sampleEE.vel(vEE)
sampleEE.acc(aEE)
......@@ -76,11 +75,11 @@ for i in range(0, N):
ee_pos[:,i] = tsid.robot.framePosition(tsid.formulation.data(), tsid.EE).translation
ee_vel[:,i] = tsid.robot.frameVelocityWorldOriented(tsid.formulation.data(), tsid.EE).linear
ee_acc[:,i] = tsid.eeTask.getAcceleration(dv)[:3,0]
ee_pos_ref[:,i] = sampleEE.pos()[:3,0]
ee_vel_ref[:,i] = sampleEE.vel()[:3,0]
ee_acc_ref[:,i] = sampleEE.acc()[:3,0]
ee_acc_des[:,i] = tsid.eeTask.getDesiredAcceleration[:3,0]
ee_acc[:,i] = tsid.eeTask.getAcceleration(dv)[:3]
ee_pos_ref[:,i] = sampleEE.pos()[:3]
ee_vel_ref[:,i] = sampleEE.vel()[:3]
ee_acc_ref[:,i] = sampleEE.acc()[:3]
ee_acc_des[:,i] = tsid.eeTask.getDesiredAcceleration[:3]
if i%conf.PRINT_N == 0:
print(("Time %.3f"%(t)))
......@@ -91,8 +90,8 @@ for i in range(0, N):
if i%conf.DISPLAY_N == 0:
tsid.robot_display.display(q[:,i])
tsid.gui.applyConfiguration('world/ee', ee_pos[:,i].A1.tolist()+[0,0,0,1.])
tsid.gui.applyConfiguration('world/ee_ref', ee_pos_ref[:,i].A1.tolist()+[0,0,0,1.])
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)
......@@ -103,8 +102,8 @@ 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,:].A1, label='EE '+str(i))
ax[i].plot(time, ee_pos_ref[i,:].A1, 'r:', label='EE Ref '+str(i))
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()
......@@ -113,8 +112,8 @@ if(PLOT_EE_POS):
if(PLOT_EE_VEL):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_vel[i,:].A1, label='EE Vel '+str(i))
ax[i].plot(time, ee_vel_ref[i,:].A1, 'r:', label='EE Vel Ref '+str(i))
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]')
leg = ax[i].legend()
......@@ -123,9 +122,9 @@ if(PLOT_EE_VEL):
if(PLOT_EE_ACC):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_acc[i,:].A1, label='EE Acc '+str(i))
ax[i].plot(time, ee_acc_ref[i,:].A1, 'r:', label='EE Acc Ref '+str(i))
ax[i].plot(time, ee_acc_des[i,:].A1, 'g--', label='EE Acc Des '+str(i))
ax[i].plot(time, ee_acc[i,:], label='EE Acc '+str(i))
ax[i].plot(time, ee_acc_ref[i,:], 'r:', label='EE Acc Ref '+str(i))
ax[i].plot(time, ee_acc_des[i,:], 'g--', label='EE Acc Des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE Acc [m/s^2]')
leg = ax[i].legend()
......@@ -135,9 +134,9 @@ if(PLOT_TORQUES):
(f, ax) = plut.create_empty_figure(int(tsid.robot.nv/2),2)
ax = ax.reshape(tsid.robot.nv)
for i in range(tsid.robot.nv):
ax[i].plot(time, tau[i,:].A1, label='Torque '+str(i))
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_min[i,0]], ':')
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_max[i,0]], ':')
ax[i].plot(time, tau[i,:], label='Torque '+str(i))
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_min[i]], ':')
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_max[i]], ':')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Torque [Nm]')
leg = ax[i].legend()
......@@ -147,9 +146,9 @@ if(PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(int(tsid.robot.nv/2),2)
ax = ax.reshape(tsid.robot.nv)
for i in range(tsid.robot.nv):
ax[i].plot(time, v[i,:-1].A1, label='Joint vel '+str(i))
ax[i].plot([time[0], time[-1]], 2*[tsid.v_min[i,0]], ':')
ax[i].plot([time[0], time[-1]], 2*[tsid.v_max[i,0]], ':')
ax[i].plot(time, v[i,:-1], label='Joint vel '+str(i))
ax[i].plot([time[0], time[-1]], 2*[tsid.v_min[i]], ':')
ax[i].plot([time[0], time[-1]], 2*[tsid.v_max[i]], ':')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Joint velocity [rad/s]')
leg = ax[i].legend()
......
exercizes/ex_2.py
View file @ 90c2a23c
......@@ -26,8 +26,8 @@ com_acc_des = np.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
offset = tsid.robot.com(tsid.formulation.data())
amp = np.array([0.0, 0.05, 0.0])
two_pi_f = 2*np.pi*np.array([0.0, 0.5, 0.0])
two_pi_f_amp = np.multiply(two_pi_f,amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
two_pi_f_amp = two_pi_f * amp
two_pi_f_squared_amp = two_pi_f * two_pi_f_amp
sampleCom = tsid.trajCom.computeNext()
samplePosture = tsid.trajPosture.computeNext()
......@@ -38,9 +38,9 @@ q, v = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
sampleCom.pos(offset + np.multiply(amp, np.sin(two_pi_f*t)))
sampleCom.vel(np.multiply(two_pi_f_amp, np.cos(two_pi_f*t)))
sampleCom.acc(np.multiply(two_pi_f_squared_amp, -np.sin(two_pi_f*t)))
sampleCom.pos(offset + amp * np.sin(two_pi_f*t))
sampleCom.vel( two_pi_f_amp * np.cos(two_pi_f*t))
sampleCom.acc(-two_pi_f_squared_amp * np.sin(two_pi_f*t))
tsid.comTask.setReference(sampleCom)
tsid.postureTask.setReference(samplePosture)
......
exercizes/ex_3_biped_balance_with_gui.py
View file @ 90c2a23c
......@@ -166,8 +166,8 @@ def run_simu():
x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]
tsid.gui.applyConfiguration('world/com', x_com.tolist()+[0,0,0,1.])
tsid.gui.applyConfiguration('world/com_ref', x_com_ref.tolist()+[0,0,0,1.])
tsid.gui.applyConfiguration('world/rf', pin.se3ToXYZQUATtuple(H_rf))
tsid.gui.applyConfiguration('world/lf', pin.se3ToXYZQUATtuple(H_lf))
tsid.gui.applyConfiguration('world/rf', pin.SE3ToXYZQUATtuple(H_rf))
tsid.gui.applyConfiguration('world/lf', pin.SE3ToXYZQUATtuple(H_lf))
tsid.gui.applyConfiguration('world/rf_ref', x_rf_ref.tolist()+[0,0,0,1.])
tsid.gui.applyConfiguration('world/lf_ref', x_lf_ref.tolist()+[0,0,0,1.])
......
exercizes/ex_4_walking.py
View file @ 90c2a23c
......@@ -121,13 +121,13 @@ for i in range(-N_pre, N+N_post):
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
T_RF = tsid.contactRF.getForceGeneratorMatrix
f_RF[:,i] = T_RF * tsid.formulation.getContactForce(tsid.contactRF.name, sol)
f_RF[:,i] = T_RF @ tsid.formulation.getContactForce(tsid.contactRF.name, sol)
if(f_RF[2,i]>1e-3):
cop_RF[0,i] = f_RF[4,i] / f_RF[2,i]
cop_RF[1,i] = -f_RF[3,i] / f_RF[2,i]
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
T_LF = tsid.contactRF.getForceGeneratorMatrix
f_LF[:,i] = T_LF * tsid.formulation.getContactForce(tsid.contactLF.name, sol)
f_LF[:,i] = T_LF @ tsid.formulation.getContactForce(tsid.contactLF.name, sol)
if(f_LF[2,i]>1e-3):
cop_LF[0,i] = f_LF[4,i] / f_LF[2,i]
cop_LF[1,i] = -f_LF[3,i] / f_LF[2,i]
......@@ -240,7 +240,7 @@ if PLOT_FOOT_TRAJ:
if(PLOT_TORQUES):
plt.figure()
for i in range(tsid.robot.na):
tau_normalized = 2*(tau[i,:]-tsid.tau_min[i,0]) / (tsid.tau_max[i,0]-tsid.tau_min[i,0]) - 1
tau_normalized = 2*(tau[i,:]-tsid.tau_min[i]) / (tsid.tau_max[i]-tsid.tau_min[i]) - 1
# plot torques only for joints that reached 50% of max torque
if np.max(np.abs(tau_normalized))>0.5:
plt.plot(time, tau_normalized, alpha=0.5, label=tsid.model.names[i+2])
......@@ -254,7 +254,7 @@ if(PLOT_TORQUES):
if(PLOT_JOINT_VEL):
plt.figure()
for i in range(tsid.robot.na):
v_normalized = 2*(v_log[6+i,:]-tsid.v_min[i,0]) / (tsid.v_max[i,0]-tsid.v_min[i,0]) - 1
v_normalized = 2*(v_log[6+i,:]-tsid.v_min[i]) / (tsid.v_max[i]-tsid.v_min[i]) - 1
# plot v only for joints that reached 50% of max v
if np.max(np.abs(v_normalized))>0.5:
plt.plot(time, v_normalized, alpha=0.5, label=tsid.model.names[i+2])
......@@ -263,6 +263,6 @@ if(PLOT_JOINT_VEL):
plt.gca().set_xlabel('Time [s]')
plt.gca().set_ylabel('Normalized Joint Vel')
leg = plt.legend()
leg.get_frame().set_alpha(0.5)
# leg.get_frame().set_alpha(0.5)
plt.show()
exercizes/tsid_biped.py
View file @ 90c2a23c
......@@ -45,7 +45,7 @@ class TsidBiped:
H_rf_ref = robot.position(data, self.RF)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2,0] - conf.lz
q[2] -= H_rf_ref.translation[2] - conf.lz
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
H_rf_ref = robot.position(data, self.RF)
......@@ -168,18 +168,18 @@ class TsidBiped:
self.comTask.setReference(self.sample_com)
def set_RF_3d_ref(self, pos, vel, acc):
self.sample_RF_pos[:3,0] = pos
self.sample_RF_vel[:3,0] = vel
self.sample_RF_acc[:3,0] = acc
self.sample_RF_pos[:3] = pos
self.sample_RF_vel[:3] = vel
self.sample_RF_acc[:3] = acc
self.sampleRF.pos(self.sample_RF_pos)
self.sampleRF.vel(self.sample_RF_vel)
self.sampleRF.acc(self.sample_RF_acc)
self.rightFootTask.setReference(self.sampleRF)
def set_LF_3d_ref(self, pos, vel, acc):
self.sample_LF_pos[:3,0] = pos
self.sample_LF_vel[:3,0] = vel
self.sample_LF_acc[:3,0] = acc
self.sample_LF_pos[:3] = pos
self.sample_LF_vel[:3] = vel
self.sample_LF_acc[:3] = acc
self.sampleLF.pos(self.sample_LF_pos)
self.sampleLF.vel(self.sample_LF_vel)
self.sampleLF.acc(self.sample_LF_acc)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment