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
682eb860
Commit
682eb860
authored
Jul 18, 2019
by
Andrea Del Prete
Browse files
Improve ex 4 (add torque and vel limits, add plots)
parent
e479577d
Changes
5
Hide whitespace changes
Inline
Side-by-side
exercizes/ex_4_conf.py
View file @
682eb860
...
...
@@ -50,12 +50,17 @@ nb_steps = 4 # number of desired walking steps
# ----------------------------------------------
dt
=
0.002
# controller time step
T_pre
=
1.0
# simulation time before starting to walk
T_post
=
1
.0
# simulation time after walking
T_post
=
2
.0
# simulation time after walking
w_com
=
1.0
# weight of center of mass task
w_foot
=
1e0
# weight of the foot motion task
w_foot
=
1e0
# weight of the foot motion task
w_posture
=
1e-4
# weight of joint posture task
w_forceRef
=
1e-5
# weight of force regularization task
w_torque_bounds
=
0.0
# weight of the torque bounds
w_joint_bounds
=
0.0
tau_max_scaling
=
1.45
# scaling factor of torque bounds
v_max_scaling
=
0.8
kp_contact
=
10.0
# proportional gain of contact constraint
kp_foot
=
10.0
# proportional gain of contact constraint
...
...
exercizes/ex_4_long_conf.py
View file @
682eb860
...
...
@@ -49,11 +49,18 @@ nb_steps = 6 # number of desired walking steps
# configuration for TSID
# ----------------------------------------------
dt
=
0.002
# controller time step
T_pre
=
1.0
# simulation time before starting to walk
T_post
=
2.0
# simulation time after walking
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
w_torque_bounds
=
0.0
# weight of the torque bounds
w_joint_bounds
=
0.0
tau_max_scaling
=
1.55
# scaling factor of torque bounds
v_max_scaling
=
0.8
kp_contact
=
10.0
# proportional gain of contact constraint
kp_foot
=
10.0
# proportional gain of contact constraint
...
...
exercizes/ex_4_walking.py
View file @
682eb860
...
...
@@ -13,6 +13,12 @@ print "".center(conf.LINE_WIDTH,'#')
print
" Test Walking "
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
),
'
\n
'
PLOT_COM
=
1
PLOT_COP
=
1
PLOT_FOOT_TRAJ
=
0
PLOT_TORQUES
=
1
PLOT_JOINT_VEL
=
1
data
=
np
.
load
(
conf
.
DATA_FILE_TSID
)
tsid
=
TsidBiped
(
conf
)
...
...
@@ -36,6 +42,9 @@ 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
))
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
))
contact_phase
=
data
[
'contact_phase'
]
com_pos_ref
=
np
.
asmatrix
(
data
[
'com'
])
...
...
@@ -92,7 +101,10 @@ for i in range(-N_pre, N+N_post):
print
"Time %.3f Velocities are too high, stop everything!"
%
(
t
),
norm
(
v
)
break
tau
=
tsid
.
formulation
.
getActuatorForces
(
sol
)
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
:
...
...
@@ -142,48 +154,50 @@ for i in range(-N_pre, N+N_post):
# PLOT STUFF
time
=
np
.
arange
(
0.0
,
(
N
+
N_post
)
*
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
[:
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
)
(
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
)
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
)
(
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
)
(
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
)
(
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
)
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
)
#(f, ax) = plut.create_empty_figure(3,2)
#ax = ax.reshape((6))
...
...
@@ -195,30 +209,59 @@ for i in range(2):
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
for
i
in
range
(
3
):
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
):
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
(
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
)
#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_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
)
plt
.
show
()
exercizes/plot_utils.py
View file @
682eb860
...
...
@@ -78,6 +78,7 @@ mpl.rcdefaults()
mpl
.
rcParams
[
'lines.linewidth'
]
=
DEFAULT_LINE_WIDTH
;
mpl
.
rcParams
[
'lines.markersize'
]
=
DEFAULT_MARKER_SIZE
;
mpl
.
rcParams
[
'patch.linewidth'
]
=
1
;
mpl
.
rcParams
[
'axes.grid'
]
=
True
mpl
.
rcParams
[
'font.family'
]
=
DEFAULT_FONT_FAMILY
;
mpl
.
rcParams
[
'font.size'
]
=
DEFAULT_FONT_SIZE
;
mpl
.
rcParams
[
'font.serif'
]
=
DEFAULT_FONT_SERIF
;
...
...
exercizes/tsid_biped.py
View file @
682eb860
...
...
@@ -84,6 +84,20 @@ class TsidBiped:
self
.
trajRF
=
tsid
.
TrajectorySE3Constant
(
"traj-right-foot"
,
H_rf_ref
)
formulation
.
addMotionTask
(
self
.
rightFootTask
,
self
.
conf
.
w_foot
,
1
,
0.0
)
self
.
tau_max
=
conf
.
tau_max_scaling
*
robot
.
model
().
effortLimit
[
-
robot
.
na
:]
self
.
tau_min
=
-
self
.
tau_max
actuationBoundsTask
=
tsid
.
TaskActuationBounds
(
"task-actuation-bounds"
,
robot
)
actuationBoundsTask
.
setBounds
(
self
.
tau_min
,
self
.
tau_max
)
if
(
conf
.
w_torque_bounds
>
0.0
):
formulation
.
addActuationTask
(
actuationBoundsTask
,
conf
.
w_torque_bounds
,
0
,
0.0
)
jointBoundsTask
=
tsid
.
TaskJointBounds
(
"task-joint-bounds"
,
robot
,
conf
.
dt
)
self
.
v_max
=
conf
.
v_max_scaling
*
robot
.
model
().
velocityLimit
[
-
robot
.
na
:]
self
.
v_min
=
-
self
.
v_max
jointBoundsTask
.
setVelocityBounds
(
self
.
v_min
,
self
.
v_max
)
if
(
conf
.
w_joint_bounds
>
0.0
):
formulation
.
addMotionTask
(
jointBoundsTask
,
conf
.
w_joint_bounds
,
0
,
0.0
)
com_ref
=
robot
.
com
(
data
)
self
.
trajCom
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_com"
,
com_ref
)
self
.
sample_com
=
self
.
trajCom
.
computeNext
()
...
...
@@ -109,6 +123,8 @@ class TsidBiped:
self
.
postureTask
=
postureTask
self
.
contactRF
=
contactRF
self
.
contactLF
=
contactLF
self
.
actuationBoundsTask
=
actuationBoundsTask
self
.
jointBoundsTask
=
jointBoundsTask
self
.
formulation
=
formulation
self
.
q
=
q
self
.
v
=
v
...
...
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