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
bd2811f7
Commit
bd2811f7
authored
Jul 03, 2019
by
Andrea Del Prete
Browse files
Add example of Romeo walking
parent
d8754355
Changes
3
Hide whitespace changes
Inline
Side-by-side
exercizes/ex_4_conf.py
0 → 100644
View file @
bd2811f7
# -*- coding: utf-8 -*-
"""
Created on Thu Apr 18 09:47:07 2019
@author: student
"""
import
numpy
as
np
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'
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
mu
=
0.3
# friction coefficient
fMin
=
5.0
# minimum normal force
fMax
=
1000.0
# maximum normal force
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
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
w_forceRef
=
1e-5
# weight of force regularization task
kp_contact
=
10.0
# proportional gain of contact constraint
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
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
exercizes/ex_4_plan_LIPM_romeo.py
0 → 100644
View file @
bd2811f7
# LMPC_walking is a python software implementation of some of the linear MPC
# algorithms based presented in:
# https://groups.csail.mit.edu/robotics-center/public_papers/Wieber15.pdf
# Copyright (C) 2019 @ahmad gazar
# LMPC_walking is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
# LMPC_walking is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
# headers:
# -------
import
numpy
as
np
from
quadprog
import
solve_qp
import
LMPC_walking.second_order.reference_trajectories
as
reference_trajectories
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
# CoM initial state: [x_0, xdot_0].T
# [y_0, ydot_0].T
# ----------------------------------
x_0
=
np
.
array
([
conf
.
foot_step_0
[
0
],
0.0
])
y_0
=
np
.
array
([
conf
.
foot_step_0
[
1
],
0.0
])
step_width
=
2
*
np
.
absolute
(
y_0
[
0
])
# compute CoP reference trajectory:
# --------------------------------
desiredFoot_steps
=
reference_trajectories
.
manual_foot_placement
(
conf
.
foot_step_0
,
conf
.
step_length
,
conf
.
nb_steps
)
desiredFoot_steps
[
1
:,
0
]
-=
conf
.
step_length
desired_Z_ref
=
reference_trajectories
.
create_CoP_trajectory
(
conf
.
nb_steps
,
desiredFoot_steps
,
N
,
nb_steps_per_T
)
# 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
nb_terminal_constraints
=
4
terminal_index
=
N
-
1
# construct your preview system: 'Go pokemon !'
# --------------------------------------------
[
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
)
[
A_zmp
,
b_zmp
]
=
constraints
.
add_ZMP_constraints
(
N
,
foot_length
,
foot_width
,
desired_Z_ref
,
x_0
,
y_0
)
# used in case you want to add both terminal add_ZMP_constraints
# --------------------------------------------------------------
[
A_terminal
,
b_terminal
]
=
constraints
.
add_terminal_constraints
(
N
,
terminal_index
,
x_0
,
y_0
,
x_terminal
,
y_terminal
,
P_ps
,
P_vs
,
P_pu
,
P_vu
)
A
=
np
.
concatenate
((
A_terminal
,
A_zmp
),
axis
=
0
)
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
]
# 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
,
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
))
# 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,)
exercizes/ex_4_walking.py
0 → 100644
View file @
bd2811f7
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
from
tsid_biped
import
TsidBiped
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
" Test Walking "
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
),
'
\n
'
data
=
np
.
load
(
conf
.
DATA_FILE
)
tsid
=
TsidBiped
(
conf
)
N
=
data
[
'com'
].
shape
[
1
]
com_pos
=
matlib
.
empty
((
3
,
N
))
*
nan
com_vel
=
matlib
.
empty
((
3
,
N
))
*
nan
com_acc
=
matlib
.
empty
((
3
,
N
))
*
nan
f_RF
=
matlib
.
zeros
((
6
,
N
))
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_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())
for
i
in
range
(
N
):
com_pos_ref
[:,
i
]
+=
offset
# foot_steps[:,i] += offset[:2,0]
sampleCom
=
tsid
.
trajCom
.
computeNext
()
t
=
0.0
q
,
v
=
tsid
.
q
,
tsid
.
v
for
i
in
range
(
-
2000
,
N
):
time_start
=
time
.
time
()
if
i
==
0
:
print
"Starting to walk (remove contact left foot)"
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
])
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
:
sampleCom
.
pos
(
com_pos_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
())
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
)
>
20.0
:
print
"Time %.3f Velocities are too high, stop everything!"
%
(
t
),
norm
(
v
)
break
tau
=
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
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
)
if
tsid
.
formulation
.
checkContact
(
tsid
.
contactRF
.
name
,
sol
):
print
"
\t
normal force %s: %.1f"
%
(
tsid
.
contactRF
.
name
.
ljust
(
20
,
'.'
),
f_RF
[
2
,
i
])
if
tsid
.
formulation
.
checkContact
(
tsid
.
contactLF
.
name
,
sol
):
print
"
\t
normal force %s: %.1f"
%
(
tsid
.
contactLF
.
name
.
ljust
(
20
,
'.'
),
f_LF
[
2
,
i
])
print
"
\t
tracking 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
()
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
):
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
))
ax
[
i
].
set_xlabel
(
'Time [s]'
)
ax
[
i
].
set_ylabel
(
'CoP [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
,:].
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
()
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment