Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Pierre Fernbach
tsid
Commits
58036e4a
Commit
58036e4a
authored
Jul 04, 2019
by
Andrea Del Prete
Browse files
Add foot trajectories to ex 4
parent
bd2811f7
Changes
5
Hide whitespace changes
Inline
Side-by-side
exercizes/ex_4_LIPM_to_TSID.py
0 → 100644
View file @
58036e4a
import
numpy
as
np
import
LMPC_walking.second_order.plot_utils
as
plot_utils
from
LMPC_walking.second_order.LIPM_to_whole_body
import
compute_foot_traj
,
interpolate_lipm_traj
import
matplotlib.pyplot
as
plt
import
ex_4_conf
as
conf
# READ COM-COP TRAJECTORIES COMPUTED WITH LIPM MODEL
data
=
np
.
load
(
conf
.
DATA_FILE_LIPM
)
com_state_x
=
data
[
'com_state_x'
]
com_state_y
=
data
[
'com_state_y'
]
cop_ref
=
data
[
'cop_ref'
]
cop_x
=
data
[
'cop_x'
]
cop_y
=
data
[
'cop_y'
]
foot_steps
=
data
[
'foot_steps'
]
# INTERPOLATE WITH TIME STEP OF CONTROLLER (TSID)
dt_ctrl
=
conf
.
dt
# time step used by TSID
com
,
dcom
,
ddcom
,
cop
,
contact_phase
,
foot_steps_ctrl
=
\
interpolate_lipm_traj
(
conf
.
T_step
,
conf
.
nb_steps
,
conf
.
dt_mpc
,
dt_ctrl
,
conf
.
h
,
conf
.
g
,
com_state_x
,
com_state_y
,
cop_ref
,
cop_x
,
cop_y
)
# COMPUTE TRAJECTORIES FOR FEET
N
=
conf
.
nb_steps
*
int
(
round
(
conf
.
T_step
/
conf
.
dt_mpc
))
# number of time steps for traj-opt
N_ctrl
=
int
((
N
*
conf
.
dt_mpc
)
/
dt_ctrl
)
# number of time steps for TSID
foot_steps_RF
=
foot_steps
[::
2
,:]
# assume first foot step corresponds to right foot
x_RF
,
dx_RF
,
ddx_RF
=
compute_foot_traj
(
foot_steps_RF
,
N_ctrl
,
dt_ctrl
,
conf
.
T_step
,
conf
.
step_height
,
'stance'
)
foot_steps_LF
=
foot_steps
[
1
::
2
,:]
x_LF
,
dx_LF
,
ddx_LF
=
compute_foot_traj
(
foot_steps_LF
,
N_ctrl
,
dt_ctrl
,
conf
.
T_step
,
conf
.
step_height
,
'swing'
)
# SAVE COMPUTED TRAJECTORIES IN NPY FILE FOR TSID
np
.
savez
(
conf
.
DATA_FILE_TSID
,
com
=
com
,
dcom
=
dcom
,
ddcom
=
ddcom
,
x_RF
=
x_RF
,
dx_RF
=
dx_RF
,
ddx_RF
=
ddx_RF
,
x_LF
=
x_LF
,
dx_LF
=
dx_LF
,
ddx_LF
=
ddx_LF
,
contact_phase
=
contact_phase
)
# PLOT STUFF
time_ctrl
=
np
.
arange
(
0
,
round
(
N_ctrl
*
dt_ctrl
,
2
),
dt_ctrl
)
for
i
in
range
(
3
):
plt
.
figure
()
plt
.
plot
(
time_ctrl
,
x_RF
[
i
,:
-
1
],
label
=
'x RF '
+
str
(
i
))
plt
.
plot
(
time_ctrl
,
x_LF
[
i
,:
-
1
],
label
=
'x LF '
+
str
(
i
))
plt
.
legend
()
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, dx_RF[i,:-1], label='dx RF '+str(i))
# plt.plot(time_ctrl, dx_LF[i,:-1], label='dx LF '+str(i))
# plt.legend()
#
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, ddx_RF[i,:-1], label='ddx RF '+str(i))
# plt.plot(time_ctrl, ddx_LF[i,:-1], label='ddx LF '+str(i))
# plt.legend()
time
=
np
.
arange
(
0
,
round
(
N
*
conf
.
dt_mpc
,
2
),
conf
.
dt_mpc
)
for
i
in
range
(
2
):
plt
.
figure
()
plt
.
plot
(
time_ctrl
,
cop
[
i
,:
-
1
],
label
=
'CoP'
)
# plt.plot(time_ctrl, foot_steps_ctrl[i,:-1], label='Foot step')
plt
.
plot
(
time_ctrl
,
com
[
i
,:
-
1
],
'g'
,
label
=
'CoM'
)
if
i
==
0
:
plt
.
plot
(
time
,
com_state_x
[:
-
1
,
0
],
':'
,
label
=
'CoM TO'
)
else
:
plt
.
plot
(
time
,
com_state_y
[:
-
1
,
0
],
':'
,
label
=
'CoM TO'
)
plt
.
legend
()
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, dcom[i,:-1], label='CoM vel')
# vel_fd = (com[i,1:] - com[i,:-1]) / dt_ctrl
# plt.plot(time_ctrl, vel_fd, ':', label='CoM vel fin-diff')
# plt.legend()
#
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, ddcom[i,:-1], label='CoM acc')
# acc_fd = (dcom[i,1:] - dcom[i,:-1]) / dt_ctrl
# plt.plot(time_ctrl, acc_fd, ':', label='CoM acc fin-diff')
# plt.legend()
foot_length
=
conf
.
lxn
+
conf
.
lxp
# foot size in the x-direction
foot_width
=
conf
.
lyn
+
conf
.
lyp
# foot size in the y-direciton
plot_utils
.
plot_xy
(
time_ctrl
,
N_ctrl
,
foot_length
,
foot_width
,
foot_steps_ctrl
.
T
,
cop
[
0
,:],
cop
[
1
,:],
com
[
0
,:].
reshape
((
N_ctrl
+
1
,
1
)),
com
[
1
,:].
reshape
((
N_ctrl
+
1
,
1
)))
plt
.
plot
(
com_state_x
[:,
0
],
com_state_y
[:,
0
],
'r* '
,
markersize
=
15
,)
exercizes/ex_4_conf.py
View file @
58036e4a
...
...
@@ -11,29 +11,20 @@ import os
np
.
set_printoptions
(
precision
=
3
,
linewidth
=
200
,
suppress
=
True
)
LINE_WIDTH
=
60
# cost weights in the objective function:
# ---------------------------------------
alpha
=
10
**
(
2
)
# CoP error squared cost weight
beta
=
0
# CoM position error squared cost weight
gamma
=
10
**
(
-
1
)
# CoM velocity error squared cost weight
h
=
0.58
# fixed CoM height (assuming walking on a flat terrain)
g
=
9.81
# norm of the gravity vector
foot_step_0
=
np
.
array
([
0.0
,
-
0.096
])
# initial foot step position in x-y
dt_mpc
=
0.1
# sampling time interval
T_step
=
1.2
# time needed for every step
step_length
=
0.001
# fixed step length in the xz-plane
nb_steps
=
4
# number of desired walking steps
#N_SIMULATION = 2000 # number of time steps simulated
dt
=
0.002
# controller time step
DATA_FILE
=
'romeo_walking_traj.npz'
DATA_FILE_LIPM
=
'romeo_walking_traj_lipm.npz'
DATA_FILE_TSID
=
'romeo_walking_traj_tsid.npz'
# robot parameters
# ----------------------------------------------
filename
=
str
(
os
.
path
.
dirname
(
os
.
path
.
abspath
(
__file__
)))
path
=
filename
+
'/../models/romeo'
urdf
=
path
+
'/urdf/romeo.urdf'
srdf
=
path
+
'/srdf/romeo_collision.srdf'
lxp
=
0.10
# foot length in positive x direction
lxn
=
0.05
# foot length in negative x direction
lyp
=
0.05
# foot length in positive y direction
lyn
=
0.05
# foot length in negative y direction
lz
=
0.0
# foot sole height with respect to ankle joint
lz
=
0.0
7
# foot sole height with respect to ankle joint
mu
=
0.3
# friction coefficient
fMin
=
5.0
# minimum normal force
fMax
=
1000.0
# maximum normal force
...
...
@@ -41,6 +32,24 @@ rf_frame_name = "RAnkleRoll" # right foot frame name
lf_frame_name
=
"LAnkleRoll"
# left foot frame name
contactNormal
=
np
.
matrix
([
0.
,
0.
,
1.
]).
T
# direction of the normal to the contact surface
# configuration for LIPM trajectory optimization
# ----------------------------------------------
alpha
=
10
**
(
2
)
# CoP error squared cost weight
beta
=
0
# CoM position error squared cost weight
gamma
=
10
**
(
-
1
)
# CoM velocity error squared cost weight
h
=
0.58
# fixed CoM height
g
=
9.81
# norm of the gravity vector
foot_step_0
=
np
.
array
([
0.0
,
-
0.096
])
# initial foot step position in x-y
dt_mpc
=
0.1
# sampling time interval
T_step
=
1.2
# time needed for every step
step_length
=
0.1
# fixed step length
step_height
=
0.05
# fixed step height
nb_steps
=
4
# number of desired walking steps
# configuration for TSID
# ----------------------------------------------
dt
=
0.002
# controller time step
w_com
=
1.0
# weight of center of mass task
w_foot
=
1e-1
# weight of the foot motion task
w_posture
=
1e-4
# weight of joint posture task
...
...
@@ -51,20 +60,8 @@ kp_foot = 10.0 # proportional gain of contact constraint
kp_com
=
10.0
# proportional gain of center of mass task
kp_posture
=
1.0
# proportional gain of joint posture task
# configuration for viewer
# ----------------------------------------------
PRINT_N
=
500
# print every PRINT_N time steps
DISPLAY_N
=
20
# update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM
=
[
4.0
,
-
0.2
,
0.4
,
0.5243823528289795
,
0.518651008605957
,
0.4620114266872406
,
0.4925136864185333
]
SPHERE_RADIUS
=
0.03
REF_SPHERE_RADIUS
=
0.03
COM_SPHERE_COLOR
=
(
1
,
0.5
,
0
,
0.5
)
COM_REF_SPHERE_COLOR
=
(
1
,
0
,
0
,
0.5
)
RF_SPHERE_COLOR
=
(
0
,
1
,
0
,
0.5
)
RF_REF_SPHERE_COLOR
=
(
0
,
1
,
0.5
,
0.5
)
LF_SPHERE_COLOR
=
(
0
,
0
,
1
,
0.5
)
LF_REF_SPHERE_COLOR
=
(
0.5
,
0
,
1
,
0.5
)
filename
=
str
(
os
.
path
.
dirname
(
os
.
path
.
abspath
(
__file__
)))
path
=
filename
+
'/../models/romeo'
urdf
=
path
+
'/urdf/romeo.urdf'
srdf
=
path
+
'/srdf/romeo_collision.srdf'
\ No newline at end of file
CAMERA_TRANSFORM
=
[
3.578777551651001
,
1.2937744855880737
,
0.8885031342506409
,
0.4116811454296112
,
0.5468055009841919
,
0.6109083890914917
,
0.3978860676288605
]
exercizes/ex_4_plan_LIPM_romeo.py
View file @
58036e4a
...
...
@@ -25,19 +25,14 @@ import LMPC_walking.second_order.motion_model as motion_model
import
LMPC_walking.second_order.cost_function
as
cost_function
import
LMPC_walking.second_order.constraints
as
constraints
import
LMPC_walking.second_order.plot_utils
as
plot_utils
from
LMPC_walking.second_order.motion_model
import
discrete_LIP_dynamics
import
matplotlib.pyplot
as
plt
import
ex_4_conf
as
conf
# Inverted pendulum parameters:
# ----------------------------
foot_length
=
conf
.
lxn
+
conf
.
lxp
# foot size in the x-direction
foot_width
=
conf
.
lyn
+
conf
.
lyp
# foot size in the y-direciton
nb_steps_per_T
=
int
(
round
(
conf
.
T_step
/
conf
.
dt_mpc
))
# walking parameters:
# ------------------
N
=
conf
.
nb_steps
*
nb_steps_per_T
# number of desired walking intervals
nb_dt_per_step
=
int
(
round
(
conf
.
T_step
/
conf
.
dt_mpc
))
N
=
conf
.
nb_steps
*
nb_dt_per_step
# number of desired walking intervals
# CoM initial state: [x_0, xdot_0].T
# [y_0, ydot_0].T
...
...
@@ -49,16 +44,16 @@ step_width = 2*np.absolute(y_0[0])
# compute CoP reference trajectory:
# --------------------------------
desiredF
oot_steps
=
reference_trajectories
.
manual_foot_placement
(
conf
.
foot_step_0
,
f
oot_steps
=
reference_trajectories
.
manual_foot_placement
(
conf
.
foot_step_0
,
conf
.
step_length
,
conf
.
nb_steps
)
desiredF
oot_steps
[
1
:,
0
]
-=
conf
.
step_length
desired_Z
_ref
=
reference_trajectories
.
create_CoP_trajectory
(
conf
.
nb_steps
,
desiredF
oot_steps
,
N
,
nb_
steps
_per_
T
)
f
oot_steps
[
1
:,
0
]
-=
conf
.
step_length
cop
_ref
=
reference_trajectories
.
create_CoP_trajectory
(
conf
.
nb_steps
,
f
oot_steps
,
N
,
nb_
dt
_per_
step
)
# used in case you want to have terminal constraints
# -------------------------------------------------
x_terminal
=
np
.
array
([
desired_Z
_ref
[
N
-
1
,
0
],
0.0
])
# CoM terminal constraint in x : [x, xdot].T
y_terminal
=
np
.
array
([
desired_Z
_ref
[
N
-
1
,
1
],
0.0
])
# CoM terminal constraint in y : [y, ydot].T
x_terminal
=
np
.
array
([
cop
_ref
[
N
-
1
,
0
],
0.0
])
# CoM terminal constraint in x : [x, xdot].T
y_terminal
=
np
.
array
([
cop
_ref
[
N
-
1
,
1
],
0.0
])
# CoM terminal constraint in y : [y, ydot].T
nb_terminal_constraints
=
4
terminal_index
=
N
-
1
...
...
@@ -67,10 +62,10 @@ terminal_index = N-1
[
P_ps
,
P_vs
,
P_pu
,
P_vu
]
=
motion_model
.
compute_recursive_matrices
(
conf
.
dt_mpc
,
conf
.
g
,
conf
.
h
,
N
)
[
Q
,
p_k
]
=
cost_function
.
compute_objective_terms
(
conf
.
alpha
,
conf
.
beta
,
conf
.
gamma
,
conf
.
T_step
,
nb_
steps
_per_
T
,
N
,
conf
.
step_length
,
step_width
,
P_ps
,
P_pu
,
P_vs
,
P_vu
,
x_0
,
y_0
,
desired_Z
_ref
)
conf
.
T_step
,
nb_
dt
_per_
step
,
N
,
conf
.
step_length
,
step_width
,
P_ps
,
P_pu
,
P_vs
,
P_vu
,
x_0
,
y_0
,
cop
_ref
)
[
A_zmp
,
b_zmp
]
=
constraints
.
add_ZMP_constraints
(
N
,
foot_length
,
foot_width
,
desired_Z
_ref
,
x_0
,
y_0
)
cop
_ref
,
x_0
,
y_0
)
# used in case you want to add both terminal add_ZMP_constraints
# --------------------------------------------------------------
...
...
@@ -83,159 +78,39 @@ b = np.concatenate((b_terminal, b_zmp), axis = 0)
# call quadprog solver:
# --------------------
U
=
solve_qp
(
Q
,
-
p_k
,
A
.
T
,
b
,
nb_terminal_constraints
)[
0
]
# uncomment to solve with 4 equality terminal constraints
#U = solve_qp(Q, -p_k, A_zmp.T, b_zmp)[0] # solve only with only CoP inequality constraints
Z_x_total
=
U
[
0
:
N
]
Z_y_total
=
U
[
N
:
2
*
N
]
cop_x
=
U
[
0
:
N
]
cop_y
=
U
[
N
:
2
*
N
]
# Trajectory optimization: (based on the initial state x_hat_0, y_hat_0)
# -------------------------------------------------------------------------
[
X_total
,
Y_total
]
=
motion_model
.
compute_recursive_dynamics
(
P_ps
,
P_vs
,
P_pu
,
[
com_state_x
,
com_state_y
]
=
motion_model
.
compute_recursive_dynamics
(
P_ps
,
P_vs
,
P_pu
,
P_vu
,
N
,
x_0
,
y_0
,
U
)
X_total
=
np
.
vstack
((
x_0
,
X_total
))
Y_total
=
np
.
vstack
((
y_0
,
Y_total
))
# ------------------------------------------------------------------------------
# visualize your open-loop trajectory:
# ------------------------------------------------------------------------------
time
=
np
.
arange
(
0
,
round
(
N
*
conf
.
dt_mpc
,
2
),
conf
.
dt_mpc
)
min_admissible_CoP
=
desired_Z_ref
-
np
.
tile
([
foot_length
/
2
,
foot_width
/
2
],
(
N
,
1
))
max_admissible_cop
=
desired_Z_ref
+
np
.
tile
([
foot_length
/
2
,
foot_width
/
2
],
(
N
,
1
))
min_admissible_CoP
=
cop_ref
-
np
.
tile
([
foot_length
/
2
,
foot_width
/
2
],
(
N
,
1
))
max_admissible_cop
=
cop_ref
+
np
.
tile
([
foot_length
/
2
,
foot_width
/
2
],
(
N
,
1
))
# time vs CoP and CoM in x: 'A.K.A run rabbit run !'
# -------------------------------------------------
#plot_utils.plot_x(time, N, min_admissible_CoP,
# max_admissible_cop, Z_x_total, X_total, desired_Z_ref)
#
## time VS CoP and CoM in y: 'A.K.A what goes up must go down'
## ----------------------------------------------------------
#plot_utils.plot_y(time, N, min_admissible_CoP,
# max_admissible_cop, Z_y_total, Y_total, desired_Z_ref)
#
## plot CoP, CoM in x Vs Cop, CoM in y:
## -----------------------------------
#plot_utils.plot_xy(time, N, foot_length, foot_width,
# desired_Z_ref, Z_x_total, Z_y_total, X_total, Y_total)
dt_ctrl
=
conf
.
dt
N_ctrl
=
int
((
N
*
conf
.
dt_mpc
)
/
dt_ctrl
)
(
A
,
B
)
=
discrete_LIP_dynamics
(
dt_ctrl
,
conf
.
g
,
conf
.
h
)
com
=
np
.
empty
((
3
,
N_ctrl
+
1
))
*
np
.
nan
dcom
=
np
.
zeros
((
3
,
N_ctrl
+
1
))
ddcom
=
np
.
zeros
((
3
,
N_ctrl
+
1
))
cop
=
np
.
empty
((
2
,
N_ctrl
+
1
))
*
np
.
nan
#foot_steps = np.empty((2,N_ctrl+1))*np.nan
contact_phase
=
(
N_ctrl
+
1
)
*
[
'right'
]
com
[
2
,:]
=
conf
.
h
N_inner
=
int
(
N_ctrl
/
N
)
for
i
in
range
(
N
):
com
[
0
,
i
*
N_inner
]
=
X_total
[
i
,
0
]
com
[
1
,
i
*
N_inner
]
=
Y_total
[
i
,
0
]
dcom
[
0
,
i
*
N_inner
]
=
X_total
[
i
,
1
]
dcom
[
1
,
i
*
N_inner
]
=
Y_total
[
i
,
1
]
if
(
i
>
0
):
if
np
.
linalg
.
norm
(
desired_Z_ref
[
i
,:]
-
desired_Z_ref
[
i
-
1
,:])
<
1e-10
:
contact_phase
[
i
*
N_inner
]
=
contact_phase
[
i
*
N_inner
-
1
]
else
:
if
contact_phase
[(
i
-
1
)
*
N_inner
]
==
'right'
:
contact_phase
[
i
*
N_inner
]
=
'left'
elif
contact_phase
[(
i
-
1
)
*
N_inner
]
==
'left'
:
contact_phase
[
i
*
N_inner
]
=
'right'
for
j
in
range
(
N_inner
):
ii
=
i
*
N_inner
+
j
(
A
,
B
)
=
discrete_LIP_dynamics
((
j
+
1
)
*
dt_ctrl
,
conf
.
g
,
conf
.
h
)
# foot_steps[:,ii] = desired_Z_ref[i,:].T
cop
[
0
,
ii
]
=
Z_x_total
[
i
]
cop
[
1
,
ii
]
=
Z_y_total
[
i
]
x_next
=
A
.
dot
(
X_total
[
i
,:])
+
B
.
dot
(
cop
[
0
,
ii
])
y_next
=
A
.
dot
(
Y_total
[
i
,:])
+
B
.
dot
(
cop
[
1
,
ii
])
com
[
0
,
ii
+
1
]
=
x_next
[
0
]
com
[
1
,
ii
+
1
]
=
y_next
[
0
]
dcom
[
0
,
ii
+
1
]
=
x_next
[
1
]
dcom
[
1
,
ii
+
1
]
=
y_next
[
1
]
ddcom
[:
2
,
ii
]
=
conf
.
g
/
conf
.
h
*
(
com
[:
2
,
ii
]
-
cop
[:,
ii
])
if
(
j
>
0
):
contact_phase
[
ii
]
=
contact_phase
[
ii
-
1
]
# Generate foot trajectories using polynomials with following constraints:
# x(0)=x0, x(T)=x1, dx(0)=dx(T)=0
# x(t) = a + b t + c t^2 + d t^3
# x(0) = a = x0
# dx(0) = b = 0
# dx(T) = 2 c T + 3 d T^2 = 0 => c = -3 d T^2 / (2 T) = -(3/2) d T
# x(T) = x0 + c T^2 + d T^3 = x1
# x0 -(3/2) d T^3 + d T^3 = x1
# -0.5 d T^3 = x1 - x0
# d = 2 (x0-x1) / T^3
# c = -(3/2) T 2 (x0-x1) / (T^3) = 3 (x1-x0) / T^2
def
compute_polynomial_traj
(
x0
,
x1
,
T
,
dt
):
a
=
x0
b
=
np
.
zeros_like
(
x0
)
c
=
3
*
(
x1
-
x0
)
/
(
T
**
2
)
d
=
2
*
(
x0
-
x1
)
/
(
T
**
3
)
N
=
int
(
T
/
dt
)
n
=
x0
.
shape
[
0
]
x
=
np
.
zeros
((
n
,
N
))
dx
=
np
.
zeros
((
n
,
N
))
ddx
=
np
.
zeros
((
n
,
N
))
for
i
in
range
(
N
):
t
=
i
*
dt
x
[:,
i
]
=
a
+
b
*
t
+
c
*
t
**
2
+
d
*
t
**
3
dx
[:,
i
]
=
b
+
2
*
c
*
t
+
3
*
d
*
t
**
2
ddx
[:,
i
]
=
2
*
c
+
6
*
d
*
t
return
x
,
dx
,
ddx
foot_steps
=
desiredFoot_steps
[::
2
,:]
#x = np.zeros((3,N_ctrl+1))
#dx = np.zeros((3,N_ctrl+1))
#ddx = np.zeros((3,N_ctrl+1))
#N_step = int(step_time/dt_ctrl)
#for s in range(foot_steps.shape[0]):
# x[0, s*2*N_step :s*2*N_step+N_step] = foot_steps[s,0]
# x[1, s*2*N_step :s*2*N_step+N_step] = foot_steps[s,1]
# x[:2, s*2*N_step+N_step:(s+1)*2*N_step] = compute_polynomial_traj(foot_steps[s,:], foot_steps[s+1,:],
# step_time, dt)
#np.savez('3_step_walking_traj', x=X_total, y=Y_total, z=desired_Z_ref)
np
.
savez
(
conf
.
DATA_FILE
,
com
=
com
,
dcom
=
dcom
,
ddcom
=
ddcom
,
contact_phase
=
contact_phase
,
foot_steps
=
foot_steps
)
time_ctrl
=
np
.
arange
(
0
,
round
(
N_ctrl
*
dt_ctrl
,
2
),
dt_ctrl
)
for
i
in
range
(
2
):
plt
.
figure
()
plt
.
plot
(
time_ctrl
,
cop
[
i
,:
-
1
],
label
=
'CoP'
)
# plt.plot(time_ctrl, foot_steps[i,:-1], label='Foot step')
plt
.
plot
(
time_ctrl
,
com
[
i
,:
-
1
],
'g'
,
label
=
'CoM'
)
if
i
==
0
:
plt
.
plot
(
time
,
X_total
[:
-
1
,
0
],
':'
,
label
=
'CoM TO'
)
else
:
plt
.
plot
(
time
,
Y_total
[:
-
1
,
0
],
':'
,
label
=
'CoM TO'
)
plt
.
legend
()
for
i
in
range
(
2
):
plt
.
figure
()
plt
.
plot
(
time_ctrl
,
dcom
[
i
,:
-
1
],
label
=
'CoM vel'
)
vel_fd
=
(
com
[
i
,
1
:]
-
com
[
i
,:
-
1
])
/
dt_ctrl
plt
.
plot
(
time_ctrl
,
vel_fd
,
':'
,
label
=
'CoM vel fin-diff'
)
plt
.
legend
()
for
i
in
range
(
2
):
plt
.
figure
()
plt
.
plot
(
time_ctrl
,
ddcom
[
i
,:
-
1
],
label
=
'CoM acc'
)
acc_fd
=
(
dcom
[
i
,
1
:]
-
dcom
[
i
,:
-
1
])
/
dt_ctrl
plt
.
plot
(
time_ctrl
,
acc_fd
,
':'
,
label
=
'CoM acc fin-diff'
)
plt
.
legend
()
#plot_utils.plot_xy(time_ctrl, N_ctrl, foot_length, foot_width,
# foot_steps.T, cop[0,:], cop[1,:],
# com[0,:].reshape((N_ctrl+1,1)),
# com[1,:].reshape((N_ctrl+1,1)))
#plt.plot( X_total[:,0], Y_total[:,0], 'r* ', markersize=15,)
plot_utils
.
plot_x
(
time
,
N
,
min_admissible_CoP
,
max_admissible_cop
,
cop_x
,
com_state_x
,
cop_ref
)
# time VS CoP and CoM in y: 'A.K.A what goes up must go down'
# ----------------------------------------------------------
plot_utils
.
plot_y
(
time
,
N
,
min_admissible_CoP
,
max_admissible_cop
,
cop_y
,
com_state_y
,
cop_ref
)
# plot CoP, CoM in x Vs Cop, CoM in y:
# -----------------------------------
plot_utils
.
plot_xy
(
time
,
N
,
foot_length
,
foot_width
,
cop_ref
,
cop_x
,
cop_y
,
com_state_x
,
com_state_y
)
com_state_x
=
np
.
vstack
((
x_0
,
com_state_x
))
com_state_y
=
np
.
vstack
((
y_0
,
com_state_y
))
np
.
savez
(
conf
.
DATA_FILE_LIPM
,
com_state_x
=
com_state_x
,
com_state_y
=
com_state_y
,
cop_ref
=
cop_ref
,
cop_x
=
cop_x
,
cop_y
=
cop_y
,
foot_steps
=
foot_steps
)
exercizes/ex_4_walking.py
View file @
58036e4a
...
...
@@ -12,7 +12,7 @@ print "".center(conf.LINE_WIDTH,'#')
print
" Test Walking "
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
),
'
\n
'
data
=
np
.
load
(
conf
.
DATA_FILE
)
data
=
np
.
load
(
conf
.
DATA_FILE
_TSID
)
tsid
=
TsidBiped
(
conf
)
...
...
@@ -25,21 +25,24 @@ f_LF = matlib.zeros((6, N))
cop_RF
=
matlib
.
zeros
((
2
,
N
))
cop_LF
=
matlib
.
zeros
((
2
,
N
))
foot_steps
=
np
.
asmatrix
(
data
[
'foot_steps'
])
contact_phase
=
data
[
'contact_phase'
]
com_pos_ref
=
np
.
asmatrix
(
data
[
'com'
])
com_vel_ref
=
np
.
asmatrix
(
data
[
'dcom'
])
com_acc_ref
=
np
.
asmatrix
(
data
[
'ddcom'
])
*
1.0
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'
])
com_acc_des
=
matlib
.
empty
((
3
,
N
))
*
nan
# acc_des = acc_ref - Kp*pos_err - Kd*vel_err
x_rf
=
tsid
.
get_placement_RF
().
translation
offset
=
matlib
.
zeros
((
3
,
1
))
offset
[:
2
,
0
]
=
x_rf
[:
2
,
0
]
-
foot_steps
[
0
,:].
T
#tsid.robot.com(tsid.formulation.data())
offset
=
x_rf
-
x_RF_ref
[:,
0
]
for
i
in
range
(
N
):
com_pos_ref
[:,
i
]
+=
offset
# foot_steps[:,i] += offset[:2,0]
sampleCom
=
tsid
.
trajCom
.
computeNext
()
x_RF_ref
[:,
i
]
+=
offset
x_LF_ref
[:,
i
]
+=
offset
t
=
0.0
q
,
v
=
tsid
.
q
,
tsid
.
v
...
...
@@ -52,7 +55,7 @@ for i in range(-2000, N):
tsid
.
remove_contact_LF
()
elif
i
>
0
:
if
contact_phase
[
i
]
!=
contact_phase
[
i
-
1
]:
print
"Changing contact phase from %s to %s"
%
(
contact_phase
[
i
-
1
],
contact_phase
[
i
])
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
()
...
...
@@ -61,15 +64,11 @@ for i in range(-2000, N):
tsid
.
remove_contact_LF
()
if
i
<
0
:
sampleC
om
.
pos
(
com_pos
_ref
[:,
0
])
tsid
.
set_com_ref
(
c
om
_
pos
_ref
[:,
0
],
0
*
com_vel_ref
[:,
0
],
0
*
com_acc
_ref
[:,
0
])
else
:
sampleCom
.
pos
(
com_pos_ref
[:,
i
])
sampleCom
.
vel
(
com_vel_ref
[:,
i
])
sampleCom
.
acc
(
com_acc_ref
[:,
i
])
tsid
.
comTask
.
setReference
(
sampleCom
)
tsid
.
rightFootTask
.
setReference
(
tsid
.
trajRF
.
computeNext
())
tsid
.
leftFootTask
.
setReference
(
tsid
.
trajLF
.
computeNext
())
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
])
HQPData
=
tsid
.
formulation
.
computeProblemData
(
t
,
q
,
v
)
...
...
@@ -133,24 +132,24 @@ for i in range(3):
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
()
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
,
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
)
#
(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()
#
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, 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)
(
f
,
ax
)
=
plut
.
create_empty_figure
(
2
,
1
)
for
i
in
range
(
2
):
...
...
@@ -161,14 +160,14 @@ for i in range(2):
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
,:].
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
)
#
(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)
plt
.
show
()
exercizes/tsid_biped.py
View file @
58036e4a
...
...
@@ -46,7 +46,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
]
q
[
2
]
-=
H_rf_ref
.
translation
[
2
,
0
]
-
conf
.
lz
formulation
.
computeProblemData
(
0.0
,
q
,
v
)
data
=
formulation
.
data
()
H_rf_ref
=
robot
.
position
(
data
,
self
.
RF
)
...
...
@@ -85,23 +85,31 @@ class TsidBiped:
formulation
.
addMotionTask
(
self
.
rightFootTask
,
self
.
conf
.
w_foot
,
1
,
0.0
)
com_ref
=
robot
.
com
(
data
)
trajCom
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_com"
,
com_ref
)
self
.
trajCom
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_com"
,
com_ref
)
self
.
sample_com
=
self
.
trajCom
.
computeNext
()
q_ref
=
q
[
7
:]
trajPosture
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_joint"
,
q_ref
)
postureTask
.
setReference
(
trajPosture
.
computeNext
())
self
.
trajPosture
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_joint"
,
q_ref
)
postureTask
.
setReference
(
self
.
trajPosture
.
computeNext
())
solver
=
tsid
.
SolverHQuadProgFast
(
"qp solver"
)
solver
.
resize
(
formulation
.
nVar
,
formulation
.
nEq
,
formulation
.
nIn
)
self
.
sampleLF
=
self
.
trajLF
.
computeNext
()
self
.
sample_LF_pos
=
self
.
sampleLF
.
pos
()
self
.
sample_LF_vel
=
self
.
sampleLF
.
vel
()
self
.
sample_LF_acc
=
self
.
sampleLF
.
acc
()
self
.
sampleRF
=
self
.
trajRF
.
computeNext
()
self
.
sample_RF_pos
=
self
.
sampleRF
.
pos
()
self
.
sample_RF_vel
=
self
.
sampleRF
.
vel
()
self
.
sample_RF_acc
=
self
.
sampleRF
.
acc
()
self
.
solver
=
tsid
.
SolverHQuadProgFast
(
"qp solver"
)