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
e479577d
Commit
e479577d
authored
Jul 17, 2019
by
Andrea Del Prete
Browse files
Improve ex4: increase weight of foot tasks, add plots
parent
4ffe22d4
Changes
3
Hide whitespace changes
Inline
Side-by-side
exercizes/ex_4_conf.py
View file @
e479577d
...
...
@@ -53,7 +53,7 @@ T_pre = 1.0 # simulation time before starting to walk
T_post
=
1.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_foot
=
1e
0
# weight of the foot motion task
w_posture
=
1e-4
# weight of joint posture task
w_forceRef
=
1e-5
# weight of force regularization task
...
...
exercizes/ex_4_walking.py
View file @
e479577d
...
...
@@ -24,6 +24,14 @@ 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
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
f_RF
=
matlib
.
zeros
((
6
,
N
+
N_post
))
f_LF
=
matlib
.
zeros
((
6
,
N
+
N_post
))
cop_RF
=
matlib
.
zeros
((
2
,
N
+
N_post
))
...
...
@@ -92,6 +100,13 @@ for i in range(-N_pre, N+N_post):
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
)
...
...
@@ -159,6 +174,12 @@ for i in range(3):
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
()
...
...
@@ -174,4 +195,30 @@ for i in range(2):
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
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()
plt
.
show
()
exercizes/tsid_biped.py
View file @
e479577d
...
...
@@ -170,6 +170,20 @@ class TsidBiped:
self
.
sampleLF
.
acc
(
self
.
sample_LF_acc
)
self
.
leftFootTask
.
setReference
(
self
.
sampleLF
)
def
get_LF_3d_pos_vel_acc
(
self
,
dv
):
data
=
self
.
formulation
.
data
()
H
=
self
.
robot
.
position
(
data
,
self
.
LF
)
v
=
self
.
robot
.
velocity
(
data
,
self
.
LF
)
a
=
self
.
leftFootTask
.
getAcceleration
(
dv
)
return
H
.
translation
,
v
.
linear
,
a
[:
3
]
def
get_RF_3d_pos_vel_acc
(
self
,
dv
):
data
=
self
.
formulation
.
data
()
H
=
self
.
robot
.
position
(
data
,
self
.
RF
)
v
=
self
.
robot
.
velocity
(
data
,
self
.
RF
)
a
=
self
.
rightFootTask
.
getAcceleration
(
dv
)
return
H
.
translation
,
v
.
linear
,
a
[:
3
]
def
remove_contact_RF
(
self
,
transition_time
=
0.0
):
H_rf_ref
=
self
.
robot
.
position
(
self
.
formulation
.
data
(),
self
.
RF
)
self
.
trajRF
.
setReference
(
H_rf_ref
)
...
...
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