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
bab0e4c2
Commit
bab0e4c2
authored
Jan 24, 2019
by
ggory15
Browse files
update viapoint ftn
parent
cbc5ad9d
Changes
4
Hide whitespace changes
Inline
Side-by-side
bindings/python/timeopt/problem.hpp
View file @
bab0e4c2
...
...
@@ -38,6 +38,7 @@ namespace timeopt{
.
def
(
"setPhase"
,
&
Problem
::
setPhase
,
(
bp
::
arg
(
"index"
),
bp
::
arg
(
"phase"
)),
"Set ith phase."
)
.
def
(
"setMass"
,
&
Problem
::
setMass
,
(
bp
::
args
(
"mass"
)),
"Set robot mass."
)
.
def
(
"setFinalCOM"
,
&
Problem
::
setFinalCOM
,
(
bp
::
arg
(
"COM"
)),
"Set Final COM (Vector3d)."
)
.
def
(
"setViapoint"
,
&
Problem
::
setViapoint
,
(
bp
::
args
(
"time"
,
"viapoint(Vector3d)"
)),
"Set Viapoint (time, Vector3d)."
)
.
def
(
"resize"
,
&
Problem
::
resize
,
(
bp
::
args
(
"size"
)),
"Resize phases size."
)
.
def
(
"getTrajectorySize"
,
&
Problem
::
getNumSize
,
"get Trajectory length"
)
...
...
include/timeopt/interface/contactplanner.hpp
View file @
bab0e4c2
...
...
@@ -24,21 +24,7 @@ using namespace std;
typedef
Eigen
::
Matrix
<
double
,
8
,
1
>
Vector8d
;
public:
InitialState
()
:
ini_com_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
amom_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
lmom_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
eef_frc_rf_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.5
))
,
eef_frc_lf_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.5
))
,
eef_frc_rh_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
eef_frc_lh_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
{
eef_rf
.
setZero
();
eef_lf
.
setZero
();
eef_rh
.
setZero
();
eef_lh
.
setZero
();
};
InitialState
();
~
InitialState
(){};
inline
void
setCOM
(
const
Eigen
::
Vector3d
&
com
)
{
...
...
@@ -61,7 +47,10 @@ using namespace std;
private:
Eigen
::
Vector3d
ini_com_
,
amom_
,
lmom_
;
Eigen
::
Vector3d
eef_frc_rf_
,
eef_frc_lf_
,
eef_frc_lh_
,
eef_frc_rh_
;
Eigen
::
Vector3d
eef_frc_rf_
;
Eigen
::
Vector3d
eef_frc_lf_
;
Eigen
::
Vector3d
eef_frc_lh_
;
Eigen
::
Vector3d
eef_frc_rh_
;
Vector8d
eef_rf
,
eef_lf
,
eef_rh
,
eef_lh
;
EndeffectorID
ee_id
;
YAML
::
Node
init_cfg
;
...
...
@@ -112,13 +101,11 @@ using namespace std;
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
Eigen
::
Matrix
<
double
,
4
,
1
>
Viapoint
;
typedef
std
::
vector
<
Viapoint
,
Eigen
::
aligned_allocator
<
Viapoint
>>
ViapointVector
;
public:
ContactPlanner
()
:
time_horizon_
(
0.0
)
,
robot_mass_
(
0.0
)
,
com_displacement_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
{};
ContactPlanner
();
~
ContactPlanner
(){};
void
initialize
(
const
std
::
string
cfg_path
,
const
InitialState
&
init_state
,
const
ContactState
&
contact_state
);
...
...
@@ -132,11 +119,19 @@ using namespace std;
inline
void
setFinalCOM
(
const
Eigen
::
Vector3d
&
end_com
){
end_com_
=
end_com
;
}
inline
void
setViapoint
(
const
double
&
time
,
const
Eigen
::
Vector3d
&
via
){
Viapoint
v
;
v
<<
time
,
via
(
0
),
via
(
1
),
via
(
2
);
viapoints_
.
push_back
(
v
);
}
void
saveToFile
();
private:
double
robot_mass_
,
time_horizon_
;
Eigen
::
Vector3d
com_displacement_
,
end_com_
;
unsigned
int
num_com_viapoints_
;
ViapointVector
viapoints_
;
std
::
string
file_location_
;
momentumopt
::
PlannerSetting
planner_setting_
;
...
...
include/timeopt/problem.hpp
View file @
bab0e4c2
...
...
@@ -56,6 +56,9 @@
final_com_
=
com
;
planner_
->
setFinalCOM
(
com
);
}
void
setViapoint
(
const
double
&
time
,
const
Vector3d
&
point
){
planner_
->
setViapoint
(
time
,
point
);
}
void
resize
(
const
Index
index
){
phases_
.
resize
(
index
);
contact_state_
->
resize
(
index
);
...
...
src/timeopt/interface/contactplanner.cpp
View file @
bab0e4c2
...
...
@@ -2,6 +2,20 @@
namespace
timeopt
{
InitialState
::
InitialState
()
:
ini_com_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
amom_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
lmom_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
eef_frc_rf_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.5
))
,
eef_frc_lf_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.5
))
,
eef_frc_rh_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
,
eef_frc_lh_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
{
eef_rf
.
setZero
();
eef_lf
.
setZero
();
eef_rh
.
setZero
();
eef_lh
.
setZero
();
}
void
InitialState
::
setEEForceRatio
(
const
Eigen
::
Vector3d
&
ratio
,
EndeffectorID
ee
){
switch
(
ee
){
case
RF
:
...
...
@@ -148,6 +162,11 @@ namespace timeopt
file_out
<<
contact_cfg
;
}
ContactPlanner
::
ContactPlanner
()
:
time_horizon_
(
0.0
)
,
robot_mass_
(
0.0
)
,
com_displacement_
(
Eigen
::
Vector3d
(
.0
,
.0
,
.0
))
{}
void
ContactPlanner
::
initialize
(
const
std
::
string
cfg_path
,
const
InitialState
&
init_state
,
const
ContactState
&
contact_state
){
planner_setting_
.
initialize
(
cfg_path
);
file_location_
=
cfg_path
;
...
...
@@ -170,6 +189,13 @@ namespace timeopt
com_displacement_
=
end_com_
-
init_com
;
cfg_pars
[
"planner_variables"
][
"com_displacement"
]
=
com_displacement_
;
cfg_pars
[
"planner_variables"
][
"com_displacement"
].
SetStyle
(
YAML
::
EmitterStyle
::
Flow
);
cfg_pars
[
"planner_variables"
][
"num_com_viapoints"
]
=
viapoints_
.
size
();
for
(
size_t
i
=
0
;
i
<
viapoints_
.
size
();
i
++
){
cfg_pars
[
"planner_variables"
][
"com_viapoints"
][
"via"
+
std
::
to_string
(
i
)]
=
viapoints_
[
i
];
cfg_pars
[
"planner_variables"
][
"com_viapoints"
][
"via"
+
std
::
to_string
(
i
)].
SetStyle
(
YAML
::
EmitterStyle
::
Flow
);
}
std
::
ofstream
file_out
(
file_location_
.
substr
(
0
,
file_location_
.
size
()
-
5
)
+
"_final.yaml"
);
file_out
<<
init_cfg
<<
endl
<<
endl
<<
contact_cfg
<<
endl
<<
endl
<<
cfg_pars
;
}
...
...
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