ex_4_walking.py 10.3 KB
Newer Older
1
2
3
4
5
6
7
8
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
import plot_utils as plut
import time
import ex_4_conf as conf
Andrea Del Prete's avatar
Andrea Del Prete committed
9
#import ex_4_long_conf as conf
10
11
12
13
14
15
from tsid_biped import TsidBiped

print "".center(conf.LINE_WIDTH,'#')
print " Test Walking ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'

16
17
18
19
20
21
PLOT_COM = 1
PLOT_COP = 1
PLOT_FOOT_TRAJ = 0
PLOT_TORQUES = 1
PLOT_JOINT_VEL = 1

22
data = np.load(conf.DATA_FILE_TSID)
23
24
25
26

tsid = TsidBiped(conf)

N = data['com'].shape[1]
Andrea Del Prete's avatar
Andrea Del Prete committed
27
28
29
30
31
32
N_pre  = int(conf.T_pre/conf.dt)
N_post = int(conf.T_post/conf.dt)

com_pos = matlib.empty((3, N+N_post))*nan
com_vel = matlib.empty((3, N+N_post))*nan
com_acc = matlib.empty((3, N+N_post))*nan
33
34
35
36
37
38
39
40
x_LF   = matlib.empty((3, N+N_post))*nan
dx_LF  = matlib.empty((3, N+N_post))*nan
ddx_LF = matlib.empty((3, N+N_post))*nan
ddx_LF_des = matlib.empty((3, N+N_post))*nan
x_RF   = matlib.empty((3, N+N_post))*nan
dx_RF  = matlib.empty((3, N+N_post))*nan
ddx_RF = matlib.empty((3, N+N_post))*nan
ddx_RF_des = matlib.empty((3, N+N_post))*nan
Andrea Del Prete's avatar
Andrea Del Prete committed
41
42
43
44
f_RF = matlib.zeros((6, N+N_post))
f_LF = matlib.zeros((6, N+N_post))
cop_RF = matlib.zeros((2, N+N_post))
cop_LF = matlib.zeros((2, N+N_post))
45
46
47
tau    = matlib.zeros((tsid.robot.na, N+N_post))
q_log  = matlib.zeros((tsid.robot.nq, N+N_post))
v_log  = matlib.zeros((tsid.robot.nv, N+N_post))
48
49
50
51

contact_phase = data['contact_phase']
com_pos_ref = np.asmatrix(data['com'])
com_vel_ref = np.asmatrix(data['dcom'])
52
53
54
55
56
57
58
com_acc_ref = np.asmatrix(data['ddcom'])
x_RF_ref    = np.asmatrix(data['x_RF'])
dx_RF_ref   = np.asmatrix(data['dx_RF'])
ddx_RF_ref  = np.asmatrix(data['ddx_RF'])
x_LF_ref    = np.asmatrix(data['x_LF'])
dx_LF_ref   = np.asmatrix(data['dx_LF'])
ddx_LF_ref  = np.asmatrix(data['ddx_LF'])
Andrea Del Prete's avatar
Andrea Del Prete committed
59
com_acc_des = matlib.empty((3, N+N_post))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
60
61

x_rf   = tsid.get_placement_RF().translation
62
offset = x_rf - x_RF_ref[:,0]
63
64
for i in range(N):
    com_pos_ref[:,i] += offset
65
66
    x_RF_ref[:,i] += offset
    x_LF_ref[:,i] += offset
67

Andrea Del Prete's avatar
Andrea Del Prete committed
68
t = -conf.T_pre
69
70
q, v = tsid.q, tsid.v

Andrea Del Prete's avatar
Andrea Del Prete committed
71
for i in range(-N_pre, N+N_post):
72
73
74
75
76
    time_start = time.time()
    
    if i==0:
        print "Starting to walk (remove contact left foot)"
        tsid.remove_contact_LF()
Andrea Del Prete's avatar
Andrea Del Prete committed
77
    elif i>0 and i<N-1:
78
        if contact_phase[i] != contact_phase[i-1]:
79
            print "Time %.3f Changing contact phase from %s to %s"%(t, contact_phase[i-1], contact_phase[i])
80
81
82
83
84
85
86
87
            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:
88
        tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
Andrea Del Prete's avatar
Andrea Del Prete committed
89
    elif i<N:
90
91
92
        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])
93
94
95
96
97
98
99
    
    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
Andrea Del Prete's avatar
Andrea Del Prete committed
100
    if norm(v,2)>40.0:
101
102
103
        print "Time %.3f Velocities are too high, stop everything!"%(t), norm(v)
        break
    
104
105
106
107
    if i>0:
        q_log[:,i] = q
        v_log[:,i] = v
        tau[:,i] = tsid.formulation.getActuatorForces(sol)
108
109
110
111
112
113
114
    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
115
116
117
118
119
120
121
        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]
        
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
        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]

    if i%conf.PRINT_N == 0:
        print "Time %.3f"%(t)
Andrea Del Prete's avatar
Andrea Del Prete committed
137
        if tsid.formulation.checkContact(tsid.contactRF.name, sol) and i>=0:
138
139
            print "\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), f_RF[2,i])

Andrea Del Prete's avatar
Andrea Del Prete committed
140
        if tsid.formulation.checkContact(tsid.contactLF.name, sol) and i>=0:
141
142
143
144
145
146
147
148
149
150
151
152
153
154
            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.robot_display.display(q)

    time_spent = time.time() - time_start
    if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
    
# PLOT STUFF
Andrea Del Prete's avatar
Andrea Del Prete committed
155
time = np.arange(0.0, (N+N_post)*conf.dt, conf.dt)
156

157
158
159
160
161
162
163
164
165
if PLOT_COM:
    (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[:N], 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)
Andrea Del Prete's avatar
Andrea Del Prete committed
166
    
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
    (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[:N], 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()
        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,:].A1, label='CoM Acc '+str(i))
        ax[i].plot(time[:N], com_acc_ref[i,:].A1, 'r:', label='CoM Acc Ref '+str(i))
        ax[i].plot(time, com_acc_des[i,:].A1, '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)
185

186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
if PLOT_COP:
    (f, ax) = plut.create_empty_figure(2,1)
    for i in range(2):
        ax[i].plot(time, cop_LF[i,:].A1, label='CoP LF '+str(i))
        ax[i].plot(time, cop_RF[i,:].A1, 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 [m]')
        leg = ax[i].legend()
        leg.get_frame().set_alpha(0.5)
201
    
202
203
204
205
206
207
208
209
210
#(f, ax) = plut.create_empty_figure(3,2)
#ax = ax.reshape((6))
#for i in range(6):
#    ax[i].plot(time, f_LF[i,:].A1, label='Force LF '+str(i))
#    ax[i].plot(time, f_RF[i,:].A1, 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)
211
    
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
if PLOT_FOOT_TRAJ:
    for i in range(3):
        plt.figure()
        plt.plot(time, x_RF[i,:].A1, label='x RF '+str(i))
        plt.plot(time[:N], x_RF_ref[i,:].A1, ':', label='x RF ref '+str(i))
        plt.plot(time, x_LF[i,:].A1, label='x LF '+str(i))
        plt.plot(time[:N], x_LF_ref[i,:].A1, ':', label='x LF ref '+str(i))
        plt.legend()
        
    #for i in range(3):
    #    plt.figure()
    #    plt.plot(time, dx_RF[i,:].A1, label='dx RF '+str(i))
    #    plt.plot(time[:N], dx_RF_ref[i,:].A1, ':', label='dx RF ref '+str(i))
    #    plt.plot(time, dx_LF[i,:].A1, label='dx LF '+str(i))
    #    plt.plot(time[:N], dx_LF_ref[i,:].A1, ':', label='dx LF ref '+str(i))
    #    plt.legend()
    #    
    #for i in range(3):
    #    plt.figure()
    #    plt.plot(time, ddx_RF[i,:].A1, label='ddx RF '+str(i))
    #    plt.plot(time[:N], ddx_RF_ref[i,:].A1, ':', label='ddx RF ref '+str(i))
    #    plt.plot(time, ddx_RF_des[i,:].A1, '--', label='ddx RF des '+str(i))
    #    plt.plot(time, ddx_LF[i,:].A1, label='ddx LF '+str(i))
    #    plt.plot(time[:N], ddx_LF_ref[i,:].A1, ':', label='ddx LF ref '+str(i))
    #    plt.plot(time, ddx_LF_des[i,:].A1, '--', label='ddx LF des '+str(i))
    #    plt.legend()
    
if(PLOT_TORQUES):        
240
    plt.figure()
241
242
243
244
245
246
247
248
249
250
251
    for i in range(tsid.robot.na):
        tau_normalized = 2*(tau[i,:].A1-tsid.tau_min[i,0]) / (tsid.tau_max[i,0]-tsid.tau_min[i,0]) - 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)
252
    
253
254
255
256
257
258
259
260
261
262
263
264
265
if(PLOT_JOINT_VEL):
    plt.figure()
    for i in range(tsid.robot.na):
        v_normalized = 2*(v_log[6+i,:].A1-tsid.v_min[i,0]) / (tsid.v_max[i,0]-tsid.v_min[i,0]) - 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)
266
    
267
plt.show()