Commit 0291ea25 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[python] Add test for CoP task

parent 6f8471e4
Pipeline #13415 passed with stage
in 35 minutes and 54 seconds
Subproject commit 0428d826ca92afb5933b7cb9deb521f2f4d8cfa1
Subproject commit 000a90190d1dca8b028f437095f2fddff0f9839b
......@@ -56,6 +56,7 @@ T_pre = 1.5 # simulation time before starting to walk
T_post = 1.5 # simulation time after walking
w_com = 1.0 # weight of center of mass task
w_cop = 0.0 # weight of center of pressure task
w_foot = 1e0 # weight of the foot motion task
w_contact = 1e2 # weight of the foot in contact
w_posture = 1e-4 # weight of joint posture task
......
......@@ -29,6 +29,7 @@ lf_frame_name = "LAnkleRoll" # left foot frame name
contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contact surface
w_com = 1.0 # weight of center of mass task
w_cop = 0.0 # weight of center of pressure task
w_foot = 1e-1 # weight of the foot motion task
w_contact = -1.0 # weight of foot in contact (negative means infinite weight)
w_posture = 1e-4 # weight of joint posture task
......
import numpy as np
from numpy import nan
from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import ex_4_conf as conf
#import ex_4_long_conf as conf
from tsid_biped import TsidBiped
print("".center(conf.LINE_WIDTH,'#'))
print(" Test Walking ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
PLOT_COM = 0
PLOT_COP = 1
PLOT_FOOT_TRAJ = 0
PLOT_TORQUES = 0
PLOT_JOINT_VEL = 0
data = np.load(conf.DATA_FILE_TSID)
tsid = TsidBiped(conf, conf.viewer)
N = data['com'].shape[1]
N_pre = int(conf.T_pre/conf.dt)
N_post = int(conf.T_post/conf.dt)
com_pos = np.empty((3, N+N_post))*nan
com_vel = np.empty((3, N+N_post))*nan
com_acc = np.empty((3, N+N_post))*nan
x_LF = np.empty((3, N+N_post))*nan
dx_LF = np.empty((3, N+N_post))*nan
ddx_LF = np.empty((3, N+N_post))*nan
ddx_LF_des = np.empty((3, N+N_post))*nan
x_RF = np.empty((3, N+N_post))*nan
dx_RF = np.empty((3, N+N_post))*nan
ddx_RF = np.empty((3, N+N_post))*nan
ddx_RF_des = np.empty((3, N+N_post))*nan
f_RF = np.zeros((6, N+N_post))
f_LF = np.zeros((6, N+N_post))
cop_RF = np.zeros((2, N+N_post))
cop_LF = np.zeros((2, N+N_post))
cop = np.zeros((2, N+N_post))
tau = np.zeros((tsid.robot.na, N+N_post))
q_log = np.zeros((tsid.robot.nq, N+N_post))
v_log = np.zeros((tsid.robot.nv, N+N_post))
contact_phase = data['contact_phase']
com_pos_ref = np.asarray(data['com'])
com_vel_ref = np.asarray(data['dcom'])
com_acc_ref = np.asarray(data['ddcom'])
x_RF_ref = np.asarray(data['x_RF'])
dx_RF_ref = np.asarray(data['dx_RF'])
ddx_RF_ref = np.asarray(data['ddx_RF'])
x_LF_ref = np.asarray(data['x_LF'])
dx_LF_ref = np.asarray(data['dx_LF'])
ddx_LF_ref = np.asarray(data['ddx_LF'])
cop_ref = np.asarray(data['cop'])
com_acc_des = np.empty((3, N+N_post))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
x_rf = tsid.get_placement_RF().translation
offset = x_rf - x_RF_ref[:,0]
for i in range(N):
com_pos_ref[:,i] += offset + np.array([0.,0.,0.0])
cop_ref[:,i] += offset[:2]
x_RF_ref[:,i] += offset
x_LF_ref[:,i] += offset
# remove nan from cop_ref
cop_ref[:,-1] = cop_ref[:,-2]
t = -conf.T_pre
q, v = tsid.q, tsid.v
for i in range(-N_pre, N+N_post):
time_start = time.time()
if i==0:
print("Starting to walk (remove contact left foot)")
tsid.remove_contact_LF()
# activate CoP task only when robot starts walking
tsid.formulation.updateTaskWeight(tsid.copTask.name, 1e-3)
elif i>0 and i<N-1:
if contact_phase[i] != contact_phase[i-1]:
print("Time %.3f Changing contact phase from %s to %s"%(t, contact_phase[i-1], contact_phase[i]))
if contact_phase[i] == 'left':
tsid.add_contact_LF()
tsid.remove_contact_RF()
else:
tsid.add_contact_RF()
tsid.remove_contact_LF()
if i<0:
tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
elif i<N:
tsid.set_com_ref(com_pos_ref[:,i], com_vel_ref[:,i], com_acc_ref[:,i])
tsid.set_LF_3d_ref(x_LF_ref[:,i], dx_LF_ref[:,i], ddx_LF_ref[:,i])
tsid.set_RF_3d_ref(x_RF_ref[:,i], dx_RF_ref[:,i], ddx_RF_ref[:,i])
tsid.copTask.setReference(np.concatenate([cop_ref[:,i], np.zeros(1)]))
HQPData = tsid.formulation.computeProblemData(t, q, v)
sol = tsid.solver.solve(HQPData)
if(sol.status!=0):
print("QP problem could not be solved! Error code:", sol.status)
break
if norm(v,2)>40.0:
print("Time %.3f Velocities are too high, stop everything!"%(t), norm(v))
break
if i>0:
q_log[:,i] = q
v_log[:,i] = v
tau[:,i] = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
if i>=0:
com_pos[:,i] = tsid.robot.com(tsid.formulation.data())
com_vel[:,i] = tsid.robot.com_vel(tsid.formulation.data())
com_acc[:,i] = tsid.comTask.getAcceleration(dv)
com_acc_des[:,i] = tsid.comTask.getDesiredAcceleration
x_LF[:,i], dx_LF[:,i], ddx_LF[:,i] = tsid.get_LF_3d_pos_vel_acc(dv)
if not tsid.contact_LF_active:
ddx_LF_des[:,i] = tsid.leftFootTask.getDesiredAcceleration[:3]
x_RF[:,i], dx_RF[:,i], ddx_RF[:,i] = tsid.get_RF_3d_pos_vel_acc(dv)
if not tsid.contact_RF_active:
ddx_RF_des[:,i] = tsid.rightFootTask.getDesiredAcceleration[:3]
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)
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)
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]
cop_LF_world = tsid.get_placement_LF().act(np.array([cop_LF[0,i], cop_LF[1,i], 0]))
cop_RF_world = tsid.get_placement_RF().act(np.array([cop_RF[0,i], cop_RF[1,i], 0]))
cop[:,i] = (cop_LF_world[:2]*f_LF[2,i] + cop_RF_world[:2]*f_RF[2,i]) / (f_LF[2,i]+f_RF[2,i])
if i%conf.PRINT_N == 0:
print("Time %.3f"%(t))
if tsid.formulation.checkContact(tsid.contactRF.name, sol) and i>=0:
print("\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), f_RF[2,i]))
if tsid.formulation.checkContact(tsid.contactLF.name, sol) and i>=0:
print("\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), f_LF[2,i]))
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.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+N_post)*conf.dt, conf.dt)
if PLOT_COM:
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_pos[i,:], label='CoM '+str(i))
ax[i].plot(time[:N], com_pos_ref[i,:], '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,:], label='CoM Vel '+str(i))
ax[i].plot(time[:N], com_vel_ref[i,:], '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()
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_acc[i,:], label='CoM Acc '+str(i))
ax[i].plot(time[:N], com_acc_ref[i,:], 'r:', label='CoM Acc Ref '+str(i))
ax[i].plot(time, com_acc_des[i,:], 'g--', label='CoM Acc Des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if PLOT_COP:
(f, ax) = plut.create_empty_figure(2,1)
for i in range(2):
ax[i].plot(time, cop_LF[i,:], label='CoP LF '+str(i))
ax[i].plot(time, cop_RF[i,:], label='CoP RF '+str(i))
if i==0:
ax[i].plot([time[0], time[-1]], [-conf.lxn, -conf.lxn], ':', label='CoP Lim '+str(i))
ax[i].plot([time[0], time[-1]], [conf.lxp, conf.lxp], ':', label='CoP Lim '+str(i))
elif i==1:
ax[i].plot([time[0], time[-1]], [-conf.lyn, -conf.lyn], ':', label='CoP Lim '+str(i))
ax[i].plot([time[0], time[-1]], [conf.lyp, conf.lyp], ':', label='CoP Lim '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoP (local) [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(2,1)
for i in range(2):
ax[i].plot(time[:N], cop_ref[i,:], label='CoP ref '+str(i))
ax[i].plot(time, cop[i,:], label='CoP '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoP (world) [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
#(f, ax) = plut.create_empty_figure(3,2)
#ax = ax.reshape((6))
#for i in range(6):
# ax[i].plot(time, f_LF[i,:], label='Force LF '+str(i))
# ax[i].plot(time, f_RF[i,:], label='Force RF '+str(i))
# ax[i].set_xlabel('Time [s]')
# ax[i].set_ylabel('Force [N/Nm]')
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
if PLOT_FOOT_TRAJ:
for i in range(3):
plt.figure()
plt.plot(time, x_RF[i,:], label='x RF '+str(i))
plt.plot(time[:N], x_RF_ref[i,:], ':', label='x RF ref '+str(i))
plt.plot(time, x_LF[i,:], label='x LF '+str(i))
plt.plot(time[:N], x_LF_ref[i,:], ':', label='x LF ref '+str(i))
plt.legend()
#for i in range(3):
# plt.figure()
# plt.plot(time, dx_RF[i,:], label='dx RF '+str(i))
# plt.plot(time[:N], dx_RF_ref[i,:], ':', label='dx RF ref '+str(i))
# plt.plot(time, dx_LF[i,:], label='dx LF '+str(i))
# plt.plot(time[:N], dx_LF_ref[i,:], ':', label='dx LF ref '+str(i))
# plt.legend()
#
#for i in range(3):
# plt.figure()
# plt.plot(time, ddx_RF[i,:], label='ddx RF '+str(i))
# plt.plot(time[:N], ddx_RF_ref[i,:], ':', label='ddx RF ref '+str(i))
# plt.plot(time, ddx_RF_des[i,:], '--', label='ddx RF des '+str(i))
# plt.plot(time, ddx_LF[i,:], label='ddx LF '+str(i))
# plt.plot(time[:N], ddx_LF_ref[i,:], ':', label='ddx LF ref '+str(i))
# plt.plot(time, ddx_LF_des[i,:], '--', label='ddx LF des '+str(i))
# plt.legend()
if(PLOT_TORQUES):
plt.figure()
for i in range(tsid.robot.na):
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])
plt.plot([time[0], time[-1]], 2*[-1.0], ':')
plt.plot([time[0], time[-1]], 2*[1.0], ':')
plt.gca().set_xlabel('Time [s]')
plt.gca().set_ylabel('Normalized Torque')
leg = plt.legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_JOINT_VEL):
plt.figure()
for i in range(tsid.robot.na):
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])
plt.plot([time[0], time[-1]], 2*[-1.0], ':')
plt.plot([time[0], time[-1]], 2*[1.0], ':')
plt.gca().set_xlabel('Time [s]')
plt.gca().set_ylabel('Normalized Joint Vel')
leg = plt.legend()
# leg.get_frame().set_alpha(0.5)
plt.show()
......@@ -30,7 +30,7 @@ class TsidBiped:
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contact_Point = np.ones((3, 4)) * conf.lz
contact_Point = np.ones((3, 4)) * (-conf.lz)
contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
......@@ -67,7 +67,10 @@ class TsidBiped:
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * np.ones(3))
formulation.addMotionTask(comTask, conf.w_com, 0, 0.0)
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
copTask = tsid.TaskCopEquality("task-cop", robot)
formulation.addForceTask(copTask, conf.w_cop, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * conf.gain_vector)
......@@ -123,6 +126,7 @@ class TsidBiped:
self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.comTask = comTask
self.copTask = copTask
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
......
Markdown is supported
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