Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
tsid
Commits
07ceb45d
Commit
07ceb45d
authored
Mar 12, 2021
by
Andrea Del Prete
Browse files
[py] Add angular momentum task to tsid-biped, which improves the behavior during walking
parent
819353ad
Pipeline
#13502
passed with stage
in 32 minutes and 20 seconds
Changes
5
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
exercizes/ex_4_conf.py
View file @
07ceb45d
...
...
@@ -57,6 +57,7 @@ T_post = 1.5 # simulation time after walking
w_com
=
1.0
# weight of center of mass task
w_cop
=
0.0
# weight of center of pressure task
w_am
=
1e-4
# weight of angular momentum task
w_foot
=
1e0
# weight of the foot motion task
w_contact
=
1e2
# weight of the foot in contact
w_posture
=
1e-4
# weight of joint posture task
...
...
@@ -70,6 +71,7 @@ v_max_scaling = 0.8
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_am
=
10.0
# proportional gain of angular momentum task
kp_posture
=
1.0
# proportional gain of joint posture task
gain_vector
=
kp_posture
*
np
.
ones
(
nv
-
6
)
masks_posture
=
np
.
ones
(
nv
-
6
)
...
...
exercizes/ex_4_walking.py
View file @
07ceb45d
...
...
@@ -84,6 +84,7 @@ for i in range(-N_pre, N+N_post):
tsid
.
add_contact_RF
()
tsid
.
remove_contact_LF
()
if
i
<
0
:
tsid
.
set_com_ref
(
com_pos_ref
[:,
0
],
0
*
com_vel_ref
[:,
0
],
0
*
com_acc_ref
[:,
0
])
elif
i
<
N
:
...
...
exercizes/romeo_conf.py
View file @
07ceb45d
...
...
@@ -30,6 +30,7 @@ contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contac
w_com
=
1.0
# weight of center of mass task
w_cop
=
0.0
# weight of center of pressure task
w_am
=
0.0
# weight of angular momentum task
w_foot
=
1e-1
# weight of the foot motion task
w_contact
=
-
1.0
# weight of foot in contact (negative means infinite weight)
w_posture
=
1e-4
# weight of joint posture task
...
...
@@ -43,6 +44,7 @@ v_max_scaling = 0.8
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_am
=
10.0
# proportional gain of angular momentum task
kp_posture
=
1.0
# proportional gain of joint posture task
gain_vector
=
kp_posture
*
np
.
ones
(
nv
-
6
)
masks_posture
=
np
.
ones
(
nv
-
6
)
...
...
exercizes/test_cop_task.py
View file @
07ceb45d
...
...
@@ -88,7 +88,7 @@ for i in range(-N_pre, N+N_post):
print
(
"Starting to walk (remove contact left foot)"
)
tsid
.
remove_contact_LF
()
# activate CoP task only when robot starts walking
tsid
.
formulation
.
updateTaskWeight
(
tsid
.
copTask
.
name
,
1e-
3
)
tsid
.
formulation
.
updateTaskWeight
(
tsid
.
copTask
.
name
,
1e-
4
)
elif
i
>
0
and
i
<
N
-
1
:
if
contact_phase
[
i
]
!=
contact_phase
[
i
-
1
]:
print
(
"Time %.3f Changing contact phase from %s to %s"
%
(
t
,
contact_phase
[
i
-
1
],
contact_phase
[
i
]))
...
...
@@ -98,6 +98,12 @@ for i in range(-N_pre, N+N_post):
else
:
tsid
.
add_contact_RF
()
tsid
.
remove_contact_LF
()
elif
i
==
N
:
# switch to double support at the end
if
contact_phase
[
i
-
1
]
==
'left'
:
tsid
.
add_contact_RF
()
else
:
tsid
.
add_contact_LF
()
if
i
<
0
:
tsid
.
set_com_ref
(
com_pos_ref
[:,
0
],
0
*
com_vel_ref
[:,
0
],
0
*
com_acc_ref
[:,
0
])
...
...
exercizes/tsid_biped.py
View file @
07ceb45d
...
...
@@ -71,6 +71,14 @@ class TsidBiped:
copTask
=
tsid
.
TaskCopEquality
(
"task-cop"
,
robot
)
formulation
.
addForceTask
(
copTask
,
conf
.
w_cop
,
1
,
0.0
)
amTask
=
tsid
.
TaskAMEquality
(
"task-am"
,
robot
)
amTask
.
setKp
(
conf
.
kp_am
*
np
.
array
([
1.
,
1.
,
0.
]))
amTask
.
setKd
(
2.0
*
np
.
sqrt
(
conf
.
kp_am
*
np
.
array
([
1.
,
1.
,
0.
])))
formulation
.
addMotionTask
(
amTask
,
conf
.
w_am
,
1
,
0.
)
sampleAM
=
tsid
.
TrajectorySample
(
3
)
amTask
.
setReference
(
sampleAM
)
postureTask
=
tsid
.
TaskJointPosture
(
"task-posture"
,
robot
)
postureTask
.
setKp
(
conf
.
kp_posture
*
conf
.
gain_vector
)
...
...
@@ -127,6 +135,7 @@ class TsidBiped:
self
.
comTask
=
comTask
self
.
copTask
=
copTask
self
.
amTask
=
amTask
self
.
postureTask
=
postureTask
self
.
contactRF
=
contactRF
self
.
contactLF
=
contactLF
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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