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
loco-3d
timeopt
Commits
0d95bbf2
Commit
0d95bbf2
authored
Jan 17, 2019
by
ggory15
Browse files
locomote test update
parent
57440ba2
Changes
1
Hide whitespace changes
Inline
Side-by-side
script/locomote_test.py
View file @
0d95bbf2
...
...
@@ -12,7 +12,6 @@ cs = ContactSequenceHumanoid(0)
CONTACT_SEQUENCE_XML_TAG
=
"ContactSequence"
cs
.
loadFromXML
(
"contact_sequence.xml"
,
CONTACT_SEQUENCE_XML_TAG
)
q_init
=
cs
.
contact_phases
[
0
].
reference_configurations
[
0
].
copy
()
num_phases
=
cs
.
size
()
...
...
@@ -38,7 +37,6 @@ isInit = True
isFinal
=
True
for
k
in
range
(
num_phases
):
print
k
,
"th"
contact_phase
=
cs
.
contact_phases
[
k
]
init_guess_provided
=
(
len
(
contact_phase
.
state_trajectory
)
>
0
)
...
...
@@ -154,8 +152,9 @@ for i in range(len(RF)):
for
i
in
range
(
len
(
LF
)):
tp
.
setPhase
(
i
+
len
(
RF
),
timeopt
.
phase
(
timeopt
.
EndeffectorID
.
LF
,
LF_time
[
i
][
0
,
0
],
LF_time
[
i
][
0
,
1
],
LF
[
i
].
translation
,
LF
[
i
].
rotation
))
print
"set configuration file for time-optimization"
import
os
cfg_path
=
str
(
os
.
path
.
dirname
(
os
.
path
.
abspath
(
__file__
)))
+
'/../
../../../
config/'
+
'cfg_momSc_demo03.yaml'
cfg_path
=
str
(
os
.
path
.
dirname
(
os
.
path
.
abspath
(
__file__
)))
+
'/../config/'
+
'cfg_momSc_demo03.yaml'
tp
.
setConfigurationFile
(
cfg_path
)
tp
.
setTimeoptSolver
(
cfg_path
)
tp
.
solve
()
...
...
@@ -199,5 +198,6 @@ for k in range(tp.getTrajectorySize()):
x
[
6
:
9
]
=
(((
tp
.
getLMOM
(
k
)
/
MASS
)
-
(
tp
.
getLMOM
(
k
-
1
)
/
MASS
))
/
(
tp
.
getTime
(
k
)
-
tp
.
getTime
(
k
-
1
))).
tolist
()
cs
.
contact_phases
[
cnt
].
state_trajectory
.
append
(
np
.
matrix
(
x
))
print
"save resultant trajectories by time optimization"
cs
.
saveAsXML
(
"Result.xml"
,
"ContactSequence"
)
print
(
""
)
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