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
Humanoid Path Planner
hpp-centroidal-dynamics
Commits
5f6a431b
Commit
5f6a431b
authored
Oct 12, 2021
by
Guilhem Saurel
Browse files
remove some warnings
parent
32aab997
Pipeline
#16528
passed with stage
in 5 minutes and 14 seconds
Changes
4
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
python/centroidal_dynamics_python.cpp
View file @
5f6a431b
...
...
@@ -55,10 +55,10 @@ BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
/** BEGIN eigenpy init**/
eigenpy
::
enableEigenPy
();
eigenpy
::
enableEigenPySpecific
<
MatrixX3ColMajor
,
MatrixX3ColMajor
>
();
eigenpy
::
enableEigenPySpecific
<
MatrixXXColMajor
,
MatrixXXColMajor
>
();
eigenpy
::
enableEigenPySpecific
<
Vector3
,
Vector3
>
();
eigenpy
::
enableEigenPySpecific
<
VectorX
,
VectorX
>
();
//
eigenpy::enableEigenPySpecific<MatrixX3ColMajor, MatrixX3ColMajor>();
//
eigenpy::enableEigenPySpecific<MatrixXXColMajor, MatrixXXColMajor>();
//
eigenpy::enableEigenPySpecific<Vector3, Vector3>();
//
eigenpy::enableEigenPySpecific<VectorX, VectorX>();
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
...
...
src/centroidal_dynamics.cpp
View file @
5f6a431b
...
...
@@ -19,12 +19,12 @@ bool Equilibrium::m_is_cdd_initialized = false;
Equilibrium
::
Equilibrium
(
const
Equilibrium
&
other
)
:
m_mass
(
other
.
m_mass
),
m_gravity
(
other
.
m_gravity
),
m_G_centr
(
other
.
m_G_centr
),
m_name
(
other
.
m_name
),
m_algorithm
(
other
.
m_algorithm
),
m_solver_type
(
other
.
m_solver_type
),
m_solver
(
Solver_LP_abstract
::
getNewSolver
(
other
.
m_solver_type
)),
m_generatorsPerContact
(
other
.
m_generatorsPerContact
),
m_G_centr
(
other
.
m_G_centr
),
m_H
(
other
.
m_H
),
m_h
(
other
.
m_h
),
m_is_cdd_stable
(
other
.
m_is_cdd_stable
),
...
...
@@ -118,7 +118,7 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
// compute generators
theta
=
pi_4
;
for
(
int
j
=
0
;
j
<
cg
;
j
++
)
{
for
(
unsigned
int
j
=
0
;
j
<
cg
;
j
++
)
{
G
.
col
(
j
)
=
frictionCoefficient
*
sin
(
theta
)
*
T1
+
frictionCoefficient
*
cos
(
theta
)
*
T2
+
contactNormals
.
row
(
i
).
transpose
();
G
.
col
(
j
).
normalize
();
...
...
@@ -132,7 +132,7 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
// Compute the coefficient to convert b0 to e_max
Vector3
f0
=
Vector3
::
Zero
();
for
(
int
j
=
0
;
j
<
cg
;
j
++
)
f0
+=
G
.
col
(
j
);
// sum of the contact generators
for
(
unsigned
int
j
=
0
;
j
<
cg
;
j
++
)
f0
+=
G
.
col
(
j
);
// sum of the contact generators
// Compute the distance between the friction cone boundaries and
// the sum of the contact generators, which is e_max when b0=1.
// When b0!=1 we just multiply b0 times this value.
...
...
@@ -184,7 +184,7 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
attempts
--
;
}
// resetting random because obviously libcdd always resets to 0
srand
(
time
(
NULL
));
srand
(
(
unsigned
int
)
time
(
NULL
));
if
(
!
m_is_cdd_stable
)
{
return
false
;
}
...
...
@@ -501,7 +501,7 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
return
LP_STATUS_ERROR
;
}
LP_status
Equilibrium
::
findExtremumInDirection
(
Cref_vector3
direction
,
Ref_vector3
com
,
double
e_max
)
{
LP_status
Equilibrium
::
findExtremumInDirection
(
Cref_vector3
/*
direction
*/
,
Ref_vector3
/*
com
*/
,
double
/*
e_max
*/
)
{
if
(
m_G_centr
.
cols
()
==
0
)
return
LP_STATUS_INFEASIBLE
;
SEND_ERROR_MSG
(
"findExtremumInDirection not implemented yet"
);
return
LP_STATUS_ERROR
;
...
...
src/stop-watch.cpp
View file @
5f6a431b
...
...
@@ -213,9 +213,9 @@ void Stopwatch::report(string perf_name, int precision, std::ostream& output) {
PerformanceData
&
perf_info
=
records_of
->
find
(
perf_name
)
->
second
;
const
int
MAX_NAME_LENGTH
=
60
;
const
long
unsigned
int
MAX_NAME_LENGTH
=
60
;
string
pad
=
""
;
for
(
int
i
=
perf_name
.
length
();
i
<
MAX_NAME_LENGTH
;
i
++
)
pad
.
append
(
" "
);
for
(
long
unsigned
int
i
=
perf_name
.
length
();
i
<
MAX_NAME_LENGTH
;
i
++
)
pad
.
append
(
" "
);
output
<<
perf_name
<<
pad
;
output
<<
std
::
fixed
<<
std
::
setprecision
(
precision
)
<<
(
perf_info
.
min_time
*
1e3
)
<<
"
\t
"
;
...
...
test/test_static_equilibrium.cpp
View file @
5f6a431b
...
...
@@ -150,7 +150,7 @@ int test_computeEquilibriumRobustness(Equilibrium* solver_1, Equilibrium* solver
* @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
*/
int
test_findExtremumOverLine
(
Equilibrium
*
solver_to_test
,
Equilibrium
*
solver_ground_truth
,
Cref_vector3
a0
,
int
N_TESTS
,
double
e_max
,
const
string
&
PERF_STRING_TEST
,
unsigned
int
N_TESTS
,
double
e_max
,
const
string
&
PERF_STRING_TEST
,
const
string
&
PERF_STRING_GROUND_TRUTH
,
int
verb
=
0
)
{
int
error_counter
=
0
;
centroidal_dynamics
::
Vector3
a
,
com
;
...
...
@@ -221,8 +221,8 @@ void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver, Cref
centroidal_dynamics
::
VectorX
minDistContactPoint
=
1e10
*
centroidal_dynamics
::
VectorX
::
Ones
(
4
*
N_CONTACTS
);
// create grid of com positions to test
for
(
unsigned
int
i
=
0
;
i
<
GRID_SIZE
;
i
++
)
{
for
(
unsigned
int
j
=
0
;
j
<
GRID_SIZE
;
j
++
)
{
for
(
int
i
=
0
;
i
<
GRID_SIZE
;
i
++
)
{
for
(
int
j
=
0
;
j
<
GRID_SIZE
;
j
++
)
{
// look for contact point positions on grid
for
(
long
k
=
0
;
k
<
4
*
N_CONTACTS
;
k
++
)
{
double
dist
=
(
p
.
block
<
1
,
2
>
(
k
,
0
)
-
comPositions
.
block
<
1
,
2
>
(
i
*
GRID_SIZE
+
j
,
0
)).
norm
();
...
...
@@ -237,8 +237,8 @@ void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver, Cref
cout
<<
"
\n
Contact point positions
\n
"
;
bool
contactPointDrawn
;
for
(
unsigned
int
i
=
0
;
i
<
GRID_SIZE
;
i
++
)
{
for
(
unsigned
int
j
=
0
;
j
<
GRID_SIZE
;
j
++
)
{
for
(
int
i
=
0
;
i
<
GRID_SIZE
;
i
++
)
{
for
(
int
j
=
0
;
j
<
GRID_SIZE
;
j
++
)
{
contactPointDrawn
=
false
;
for
(
long
k
=
0
;
k
<
4
*
N_CONTACTS
;
k
++
)
{
if
(
contactPointCoord
(
k
,
0
)
==
i
&&
contactPointCoord
(
k
,
1
)
==
j
)
{
...
...
@@ -256,7 +256,7 @@ void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver, Cref
int
grid_size
=
(
int
)
sqrt
(
comPositions
.
rows
());
double
rob
;
LP_status
status
;
for
(
unsigned
int
i
=
0
;
i
<
comPositions
.
rows
();
i
++
)
{
for
(
int
i
=
0
;
i
<
comPositions
.
rows
();
i
++
)
{
status
=
solver
->
computeEquilibriumRobustness
(
comPositions
.
row
(
i
).
transpose
(),
rob
);
if
(
status
!=
LP_STATUS_OPTIMAL
)
{
SEND_ERROR_MSG
(
"Faild to compute equilibrium robustness of com position "
+
toString
(
comPositions
.
row
(
i
))
+
...
...
@@ -392,7 +392,7 @@ int main() {
RPY_UPPER_BOUNDS
<<
+
2
*
gamma
,
+
2
*
gamma
,
+
M_PI
;
double
X_MARG
=
0.07
;
double
Y_MARG
=
0.07
;
const
int
GRID_SIZE
=
10
;
const
unsigned
int
GRID_SIZE
=
10
;
bool
DRAW_CONTACT_POINTS
=
false
;
/************************************ END USER PARAMETERS *****************************/
...
...
@@ -423,7 +423,7 @@ int main() {
RVector2
com_LB
,
com_UB
;
centroidal_dynamics
::
VectorX
x_range
(
GRID_SIZE
),
y_range
(
GRID_SIZE
);
MatrixXX
comPositions
(
GRID_SIZE
*
GRID_SIZE
,
3
);
for
(
unsigned
n_test
=
0
;
n_test
<
N_TESTS
;
n_test
++
)
{
for
(
unsigned
int
n_test
=
0
;
n_test
<
N_TESTS
;
n_test
++
)
{
generateContacts
(
N_CONTACTS
,
MIN_CONTACT_DISTANCE
,
LX
,
LY
,
CONTACT_POINT_LOWER_BOUNDS
,
CONTACT_POINT_UPPER_BOUNDS
,
RPY_LOWER_BOUNDS
,
RPY_UPPER_BOUNDS
,
p
,
N
);
...
...
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