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
4ae17076
Commit
4ae17076
authored
Jan 17, 2019
by
ggory15
Browse files
upload locomote test
parent
a5215264
Changes
5
Expand all
Hide whitespace changes
Inline
Side-by-side
bindings/python/timeopt/problem.hpp
View file @
4ae17076
...
...
@@ -50,6 +50,8 @@ namespace timeopt{
.
def
(
"setConfigurationFile"
,
&
Problem
::
setConfigurationFile
,
bp
::
args
(
"string"
),
"Set Configuration File for time optimization solver"
)
.
def
(
"setTimeoptSolver"
,
&
Problem
::
setTimeoptSolver
,
bp
::
args
(
"string"
),
"Set Time optimization Problem"
)
.
def
(
"solve"
,
&
Problem
::
solve
,
"Solve Time optimization Problem"
)
.
def
(
"getMass"
,
&
Problem
::
getMass
,
"Get Robot Mass"
)
;
}
...
...
include/timeopt/problem.hpp
View file @
4ae17076
...
...
@@ -78,7 +78,9 @@
int
getNumSize
()
{
return
std
::
floor
(
planner_setting_
.
get
(
momentumopt
::
PlannerDoubleParam_TimeHorizon
)
/
planner_setting_
.
get
(
momentumopt
::
PlannerDoubleParam_TimeStep
));
}
double
getMass
(){
return
mass_
;
}
private:
PhaseInfoVector
phases_
;
Vector3d
init_com_
,
final_com_
;
...
...
script/contact_sequence.xml
0 → 100644
View file @
4ae17076
This diff is collapsed.
Click to expand it.
script/locomote_test.py
0 → 100644
View file @
4ae17076
from
configs.talos_nav
import
*
import
numpy
as
np
import
timeopt
import
locomote
from
numpy.linalg
import
norm
#from config import *
from
locomote
import
WrenchCone
,
SOC6
,
ControlType
,
IntegratorType
,
ContactPatch
,
ContactPhaseHumanoid
,
ContactSequenceHumanoid
print
"load previous contact sequence"
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
()
com0
=
cs
.
contact_phases
[
0
].
init_state
[:
3
]
q_ref
=
cs
.
contact_phases
[
0
].
reference_configurations
[
0
]
posture_l
=
[]
posture_l
.
append
(
q_ref
)
t0
=
0.
RF_time
=
[];
LF_time
=
[];
time_lf
=
0.0
;
time_rf
=
0.0
;
duration_ds_r
=
0.0
;
duration_ds_l
=
0.0
;
duration_ss_r
=
0.0
;
duration_ss_l
=
0.0
;
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
)
if
k
<
num_phases
-
1
:
next_phase
=
cs
.
contact_phases
[
k
+
1
]
num_active_phases
=
contact_phase
.
numActivePatches
()
assert
(
num_active_phases
>
0
)
phase_type
=
locomote
.
HumanoidPhaseType
.
HUMANOID_PHASE_UNDEFINED
# Determine the phase type
if
num_active_phases
==
1
:
phase_type
=
locomote
.
HumanoidPhaseType
.
SINGLE_SUPPORT
elif
num_active_phases
==
2
:
phase_type
=
locomote
.
HumanoidPhaseType
.
DOUBLE_SUPPORT
elif
num_active_phases
==
3
:
phase_type
=
locomote
.
HumanoidPhaseType
.
TRIPLE_SUPPORT
elif
num_active_phases
==
4
:
phase_type
=
locomote
.
HumanoidPhaseType
.
QUADRUPLE_SUPPORT
else
:
assert
False
,
"Must never happened"
if
phase_type
==
locomote
.
HumanoidPhaseType
.
SINGLE_SUPPORT
:
assert
k
>
0
,
"This phase must no be the initial one"
assert
k
<
num_phases
-
1
,
"This phase must no be the final one"
if
contact_phase
.
RF_patch
.
active
:
if
init_guess_provided
:
duration_ss_r
=
max
(
contact_phase
.
time_trajectory
[
0
],
DURATION_SS
)
else
:
duration_ss_r
=
DURATION_SS
time_lf
=
time_lf
+
duration_ss_r
elif
contact_phase
.
LF_patch
.
active
:
if
init_guess_provided
:
duration_ss_l
=
max
(
contact_phase
.
time_trajectory
[
0
],
DURATION_SS
)
else
:
duration_ss_l
=
DURATION_SS
time_rf
=
time_rf
+
duration_ss_l
elif
phase_type
==
locomote
.
HumanoidPhaseType
.
DOUBLE_SUPPORT
:
if
not
next_phase
.
RF_patch
.
active
:
if
init_guess_provided
:
duration_ds_l
=
max
(
contact_phase
.
time_trajectory
[
0
],
DURATION_DS
)
else
:
duration_ds_l
=
DURATION_DS
elif
not
next_phase
.
LF_patch
.
active
:
if
init_guess_provided
:
duration_ds_r
=
max
(
contact_phase
.
time_trajectory
[
0
],
DURATION_DS
)
else
:
duration_ds_r
=
DURATION_DS
if
k
==
0
and
isInit
:
if
not
next_phase
.
RF_patch
.
active
:
duration_ds_l
=
DURATION_INIT
elif
not
next_phase
.
RF_patch
.
active
:
duration_ds_r
=
DURATION_INIT
if
k
==
num_phases
-
1
and
isFinal
:
# last phase
duration_ds_fin
=
DURATION_FINAL
if
not
next_phase
.
RF_patch
.
active
:
RF_time
.
append
(
np
.
matrix
([
time_rf
,
time_rf
+
duration_ds_l
+
duration_ss_r
+
duration_ds_r
]))
time_rf
=
time_rf
+
duration_ds_l
+
duration_ss_r
+
duration_ds_r
elif
not
next_phase
.
LF_patch
.
active
:
LF_time
.
append
(
np
.
matrix
([
time_lf
,
time_lf
+
duration_ds_l
+
duration_ss_l
+
duration_ds_r
]))
time_lf
=
time_lf
+
duration_ds_l
+
duration_ss_l
+
duration_ds_r
if
k
==
num_phases
-
1
:
if
cs
.
contact_phases
[
k
-
1
].
RF_patch
.
active
:
RF_time
.
append
(
np
.
matrix
([
time_rf
,
time_rf
+
duration_ds_fin
+
duration_ss_r
+
duration_ds_r
]))
LF_time
.
append
(
np
.
matrix
([
time_lf
,
time_lf
+
duration_ds_fin
]))
elif
cs
.
contact_phases
[
k
-
1
].
LF_patch
.
active
:
LF_time
.
append
(
np
.
matrix
([
time_lf
,
time_lf
+
duration_ds_fin
+
duration_ss_l
+
duration_ds_l
]))
RF_time
.
append
(
np
.
matrix
([
time_rf
,
time_rf
+
duration_ds_fin
]))
elif
phase_type
==
locomote
.
HumanoidPhaseType
.
TRIPLE_SUPPORT
:
duration
=
max
(
contact_phase
.
time_trajectory
[
0
],
DURATION_TS
)
else
:
assert
False
,
"Must never happened"
RF
=
[];
LF
=
[];
for
k
in
range
((
num_phases
-
1
)
/
2
):
if
not
cs
.
contact_phases
[
2
*
k
+
1
].
RF_patch
.
active
:
RF
.
append
(
cs
.
contact_phases
[
2
*
k
].
RF_patch
.
placement
)
if
not
cs
.
contact_phases
[
2
*
k
+
1
].
LF_patch
.
active
:
LF
.
append
(
cs
.
contact_phases
[
2
*
k
].
LF_patch
.
placement
)
RF
.
append
(
cs
.
contact_phases
[
num_phases
-
1
].
RF_patch
.
placement
)
LF
.
append
(
cs
.
contact_phases
[
num_phases
-
1
].
LF_patch
.
placement
)
tp
=
timeopt
.
problem
(
len
(
RF
)
+
len
(
LF
))
rot
=
np
.
matrix
(
np
.
zeros
((
3
,
3
)))
rot
[
0
,
0
]
=
1.0
rot
[
1
,
1
]
=
1.0
rot
[
2
,
2
]
=
1.0
tp
.
setInitialCOM
(
cs
.
contact_phases
[
0
].
init_state
[:
3
])
tp
.
setInitialPose
(
True
,
np
.
matrix
((
0.086
,
0.15
,
-
0.92
)).
transpose
(),
rot
,
timeopt
.
EndeffectorID
.
RF
)
tp
.
setInitialPose
(
True
,
np
.
matrix
((
-
0.086
,
0.15
,
-
0.92
)).
transpose
(),
rot
,
timeopt
.
EndeffectorID
.
LF
)
tp
.
setInitialPose
(
True
,
np
.
matrix
((
0.4
,
0.3
,
0.0
)).
transpose
(),
rot
,
timeopt
.
EndeffectorID
.
RH
)
tp
.
setInitialPose
(
True
,
np
.
matrix
((
-
0.4
,
0.3
,
0.0
)).
transpose
(),
rot
,
timeopt
.
EndeffectorID
.
LH
)
tp
.
setMass
(
60.0
);
tp
.
getMass
();
tp
.
setFinalCOM
(
cs
.
contact_phases
[
num_phases
-
1
].
final_state
[
0
:
3
])
for
i
in
range
(
len
(
RF
)):
tp
.
setPhase
(
i
,
timeopt
.
phase
(
timeopt
.
EndeffectorID
.
RF
,
RF_time
[
i
][
0
,
0
],
RF_time
[
i
][
0
,
1
],
RF
[
i
].
translation
,
RF
[
i
].
rotation
))
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
))
import
os
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
()
state
=
[];
s_time
=
0.0
;
cnt
=
0
;
for
k
in
range
(
tp
.
getTrajectorySize
()):
if
norm
(
tp
.
getContactForce
(
0
,
k
))
>
0.0
and
norm
(
tp
.
getContactForce
(
1
,
k
))
>
0.0
:
state
.
append
(
'ds'
)
elif
norm
(
tp
.
getContactForce
(
0
,
k
))
>
0.0
and
norm
(
tp
.
getContactForce
(
1
,
k
))
==
0.0
:
state
.
append
(
'ssr'
)
elif
norm
(
tp
.
getContactForce
(
1
,
k
))
>
0.0
and
norm
(
tp
.
getContactForce
(
0
,
k
))
==
0.0
:
state
.
append
(
'ssl'
)
u
=
[
0
]
*
6
x
=
[
0
]
*
9
MASS
=
tp
.
getMass
()
for
k
in
range
(
tp
.
getTrajectorySize
()):
if
k
==
0
:
a
=
1
# cs.contact_phases[cnt].time_trajectory[0] = tp.getTime(k)
else
:
if
not
state
[
k
-
1
]
==
state
[
k
]:
cnt
=
cnt
+
1
# cs.contact_phases[cnt].time_trajectory[0] = (tp.getTime(k))
else
:
cnt
=
cnt
# cs.contact_phases[cnt].time_trajectory.append(tp.getTime(k))
u
[
0
:
3
]
=
tp
.
getLMOM
(
k
).
tolist
()
u
[
3
:
6
]
=
tp
.
getAMOM
(
k
).
tolist
()
#cs.contact_phases[cnt].control_trajectory.append(np.matrix(u))
x
[
0
:
3
]
=
tp
.
getCOM
(
k
).
tolist
()
x
[
3
:
6
]
=
tp
.
getLMOM
(
k
).
tolist
()
if
k
==
0
:
x
[
6
:
9
]
=
((
tp
.
getLMOM
(
k
)
/
MASS
)
/
tp
.
getTime
(
k
)).
tolist
()
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))
#filename = OUTPUT_DIR + "/" + '1' + OUTPUT_SEQUENCE_FILE
#cs.saveAsXML(filename, "ContactSequence")
print
(
""
)
src/timeopt/problem.cpp
View file @
4ae17076
...
...
@@ -61,6 +61,12 @@ namespace timeopt
dyn_optimizer_
.
initialize
(
planner_setting_
,
dynamic_state_
,
&
contact_plan
);
dyn_optimizer_
.
optimize
(
ref_sequence_
);
time_traj_
.
resize
(
getNumSize
());
int
size
=
getNumSize
();
time_traj_
(
0
)
=
dyn_optimizer_
.
dynamicsSequence
().
dynamicsState
(
0
).
time
();
for
(
int
i
=
1
;
i
<
size
;
i
++
)
time_traj_
(
i
)
=
dyn_optimizer_
.
dynamicsSequence
().
dynamicsState
(
i
).
time
()
+
time_traj_
(
i
-
1
);
}
...
...
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