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
Gabriele Buondonno
pinocchio
Commits
6dc4913e
Commit
6dc4913e
authored
Sep 10, 2015
by
Valenza Florian
Browse files
[Marginal] Removing warning signed vs unsigned
parent
144d9c21
Changes
8
Hide whitespace changes
Inline
Side-by-side
benchmark/timings.cpp
View file @
6dc4913e
...
@@ -27,7 +27,7 @@ int main(int argc, const char ** argv)
...
@@ -27,7 +27,7 @@ int main(int argc, const char ** argv)
using
namespace
se3
;
using
namespace
se3
;
StackTicToc
timer
(
StackTicToc
::
US
);
StackTicToc
timer
(
StackTicToc
::
US
);
const
in
t
NBT
=
1000
*
100
;
const
size_
t
NBT
=
1000
*
100
;
se3
::
Model
model
;
se3
::
Model
model
;
std
::
string
filename
=
PINOCCHIO_SOURCE_DIR
"/models/simple_humanoid.urdf"
;
std
::
string
filename
=
PINOCCHIO_SOURCE_DIR
"/models/simple_humanoid.urdf"
;
...
...
src/multibody/model.hpp
View file @
6dc4913e
...
@@ -290,7 +290,7 @@ namespace se3
...
@@ -290,7 +290,7 @@ namespace se3
const
std
::
string
&
Model
::
getBodyName
(
Model
::
Index
index
)
const
const
std
::
string
&
Model
::
getBodyName
(
Model
::
Index
index
)
const
{
{
assert
(
(
index
>=
0
)
&&
(
index
<
(
Model
::
Index
)
nbody
)
);
assert
(
index
<
(
Model
::
Index
)
nbody
);
return
names
[
index
];
return
names
[
index
];
}
}
...
...
unittest/cholesky.cpp
View file @
6dc4913e
...
@@ -124,12 +124,12 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -124,12 +124,12 @@ BOOST_AUTO_TEST_CASE ( test_timings )
StackTicToc
timer
(
StackTicToc
::
US
);
StackTicToc
timer
(
StackTicToc
::
US
);
#ifdef NDEBUG
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
#ifdef _INTENSE_TESTING_
const
in
t
NBT
=
1000
*
1000
;
const
size_
t
NBT
=
1000
*
1000
;
#else
#else
const
in
t
NBT
=
10
;
const
size_
t
NBT
=
10
;
#endif
#endif
#else
#else
const
in
t
NBT
=
1
;
const
size_
t
NBT
=
1
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
;
#endif
#endif
...
@@ -164,7 +164,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -164,7 +164,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if
(
flag
>>
2
&
31
)
if
(
flag
>>
2
&
31
)
{
{
std
::
vector
<
Eigen
::
VectorXd
>
randvec
(
NBT
);
std
::
vector
<
Eigen
::
VectorXd
>
randvec
(
NBT
);
for
(
in
t
i
=
0
;
i
<
NBT
;
++
i
)
randvec
[
(
std
::
size_t
)
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nv
);
for
(
size_
t
i
=
0
;
i
<
NBT
;
++
i
)
randvec
[
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nv
);
Eigen
::
VectorXd
zero
=
Eigen
::
VectorXd
(
model
.
nv
);
Eigen
::
VectorXd
zero
=
Eigen
::
VectorXd
(
model
.
nv
);
Eigen
::
VectorXd
res
(
model
.
nv
);
Eigen
::
VectorXd
res
(
model
.
nv
);
...
@@ -174,7 +174,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -174,7 +174,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer
.
tic
();
timer
.
tic
();
SMOOTH
(
NBT
)
SMOOTH
(
NBT
)
{
{
se3
::
cholesky
::
solve
(
model
,
data
,
randvec
[
(
std
::
size_t
)
_smooth
]);
se3
::
cholesky
::
solve
(
model
,
data
,
randvec
[
_smooth
]);
}
}
if
(
verbose
)
std
::
cout
<<
"solve =
\t\t
"
;
if
(
verbose
)
std
::
cout
<<
"solve =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
...
@@ -185,7 +185,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -185,7 +185,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer
.
tic
();
timer
.
tic
();
SMOOTH
(
NBT
)
SMOOTH
(
NBT
)
{
{
se3
::
cholesky
::
Uv
(
model
,
data
,
randvec
[
(
std
::
size_t
)
_smooth
]);
se3
::
cholesky
::
Uv
(
model
,
data
,
randvec
[
_smooth
]);
}
}
if
(
verbose
)
std
::
cout
<<
"Uv =
\t\t
"
;
if
(
verbose
)
std
::
cout
<<
"Uv =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
...
@@ -196,7 +196,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -196,7 +196,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer
.
tic
();
timer
.
tic
();
SMOOTH
(
NBT
)
SMOOTH
(
NBT
)
{
{
se3
::
cholesky
::
Uiv
(
model
,
data
,
randvec
[
(
std
::
size_t
)
_smooth
]);
se3
::
cholesky
::
Uiv
(
model
,
data
,
randvec
[
_smooth
]);
}
}
if
(
verbose
)
std
::
cout
<<
"Uiv =
\t\t
"
;
if
(
verbose
)
std
::
cout
<<
"Uiv =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
...
@@ -207,7 +207,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -207,7 +207,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
Eigen
::
VectorXd
res
;
Eigen
::
VectorXd
res
;
SMOOTH
(
NBT
)
SMOOTH
(
NBT
)
{
{
res
=
se3
::
cholesky
::
Mv
(
model
,
data
,
randvec
[
(
std
::
size_t
)
_smooth
]);
res
=
se3
::
cholesky
::
Mv
(
model
,
data
,
randvec
[
_smooth
]);
}
}
if
(
verbose
)
std
::
cout
<<
"Mv =
\t\t
"
;
if
(
verbose
)
std
::
cout
<<
"Mv =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
...
@@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer
.
tic
();
timer
.
tic
();
SMOOTH
(
NBT
)
SMOOTH
(
NBT
)
{
{
se3
::
cholesky
::
Mv
(
model
,
data
,
randvec
[
(
std
::
size_t
)
_smooth
],
true
);
se3
::
cholesky
::
Mv
(
model
,
data
,
randvec
[
_smooth
],
true
);
}
}
if
(
verbose
)
std
::
cout
<<
"UDUtv =
\t\t
"
;
if
(
verbose
)
std
::
cout
<<
"UDUtv =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
...
...
unittest/com.cpp
View file @
6dc4913e
...
@@ -92,12 +92,12 @@ BOOST_AUTO_TEST_CASE ( test_com )
...
@@ -92,12 +92,12 @@ BOOST_AUTO_TEST_CASE ( test_com )
// StackTicToc timer(StackTicToc::US);
// StackTicToc timer(StackTicToc::US);
// #ifdef NDEBUG
// #ifdef NDEBUG
// #ifdef _INTENSE_TESTING_
// #ifdef _INTENSE_TESTING_
// const
in
t NBT = 1000*1000;
// const
size_
t NBT = 1000*1000;
// #else
// #else
// const
in
t NBT = 10;
// const
size_
t NBT = 10;
// #endif
// #endif
// #else
// #else
// const
in
t NBT = 1;
// const
size_
t NBT = 1;
// std::cout << "(the time score in debug mode is not relevant) " ;
// std::cout << "(the time score in debug mode is not relevant) " ;
// #endif
// #endif
//
//
...
...
unittest/crba.cpp
View file @
6dc4913e
...
@@ -47,9 +47,9 @@ BOOST_AUTO_TEST_CASE ( test_crba )
...
@@ -47,9 +47,9 @@ BOOST_AUTO_TEST_CASE ( test_crba )
#ifdef NDEBUG
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
#ifdef _INTENSE_TESTING_
const
in
t
NBT
=
1000
*
1000
;
const
size_
t
NBT
=
1000
*
1000
;
#else
#else
const
in
t
NBT
=
10
;
const
size_
t
NBT
=
10
;
#endif
#endif
Eigen
::
VectorXd
q
=
Eigen
::
VectorXd
::
Zero
(
model
.
nq
);
Eigen
::
VectorXd
q
=
Eigen
::
VectorXd
::
Zero
(
model
.
nq
);
...
@@ -62,7 +62,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
...
@@ -62,7 +62,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
#else
#else
in
t
NBT
=
1
;
const
size_
t
NBT
=
1
;
using
namespace
Eigen
;
using
namespace
Eigen
;
using
namespace
se3
;
using
namespace
se3
;
...
...
unittest/jacobian.cpp
View file @
6dc4913e
...
@@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
...
@@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd
qdot
=
VectorXd
::
Random
(
model
.
nv
);
VectorXd
qdot
=
VectorXd
::
Random
(
model
.
nv
);
VectorXd
qddot
=
VectorXd
::
Zero
(
model
.
nv
);
VectorXd
qddot
=
VectorXd
::
Zero
(
model
.
nv
);
rnea
(
model
,
data
,
q
,
qdot
,
qddot
);
rnea
(
model
,
data
,
q
,
qdot
,
qddot
);
Motion
v
=
data
.
oMi
[
(
std
::
size_t
)
idx
].
act
(
data
.
v
[
(
std
::
size_t
)
idx
]
);
Motion
v
=
data
.
oMi
[
idx
].
act
(
data
.
v
[
idx
]
);
is_matrix_absolutely_closed
(
v
.
toVector
(),
Jrh
*
qdot
,
1e-12
);
is_matrix_absolutely_closed
(
v
.
toVector
(),
Jrh
*
qdot
,
1e-12
);
...
@@ -60,7 +60,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
...
@@ -60,7 +60,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
MatrixXd
rhJrh
(
6
,
model
.
nv
);
rhJrh
.
fill
(
0
);
MatrixXd
rhJrh
(
6
,
model
.
nv
);
rhJrh
.
fill
(
0
);
getJacobian
<
true
>
(
model
,
data
,
idx
,
rhJrh
);
getJacobian
<
true
>
(
model
,
data
,
idx
,
rhJrh
);
MatrixXd
XJrh
(
6
,
model
.
nv
);
MatrixXd
XJrh
(
6
,
model
.
nv
);
motionSet
::
se3Action
(
data
.
oMi
[
(
std
::
size_t
)
idx
].
inverse
(),
Jrh
,
XJrh
);
motionSet
::
se3Action
(
data
.
oMi
[
idx
].
inverse
(),
Jrh
,
XJrh
);
is_matrix_absolutely_closed
(
XJrh
,
rhJrh
,
1e-12
);
is_matrix_absolutely_closed
(
XJrh
,
rhJrh
,
1e-12
);
...
@@ -84,12 +84,12 @@ BOOST_AUTO_TEST_CASE ( test_timings )
...
@@ -84,12 +84,12 @@ BOOST_AUTO_TEST_CASE ( test_timings )
StackTicToc
timer
(
StackTicToc
::
US
);
StackTicToc
timer
(
StackTicToc
::
US
);
#ifdef NDEBUG
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
#ifdef _INTENSE_TESTING_
const
in
t
NBT
=
1000
*
1000
;
const
size_
t
NBT
=
1000
*
1000
;
#else
#else
const
in
t
NBT
=
10
;
const
size_
t
NBT
=
10
;
#endif
#endif
#else
#else
const
in
t
NBT
=
1
;
const
size_
t
NBT
=
1
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
;
#endif
#endif
...
...
unittest/rnea.cpp
View file @
6dc4913e
...
@@ -67,9 +67,9 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
...
@@ -67,9 +67,9 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
VectorXd
a
=
VectorXd
::
Random
(
model
.
nv
);
VectorXd
a
=
VectorXd
::
Random
(
model
.
nv
);
#ifdef NDEBUG
#ifdef NDEBUG
in
t
NBT
=
10000
;
const
size_
t
NBT
=
10000
;
#else
#else
in
t
NBT
=
1
;
const
size_
t
NBT
=
1
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
;
#endif
#endif
...
...
unittest/symmetric.cpp
View file @
6dc4913e
...
@@ -209,19 +209,19 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
...
@@ -209,19 +209,19 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
// Time test
// Time test
{
{
const
in
t
NBT
=
100000
;
const
size_
t
NBT
=
100000
;
Symmetric3
S
=
Symmetric3
::
RandomPositive
();
Symmetric3
S
=
Symmetric3
::
RandomPositive
();
std
::
vector
<
Symmetric3
>
Sres
(
NBT
);
std
::
vector
<
Symmetric3
>
Sres
(
NBT
);
std
::
vector
<
Matrix3
>
Rs
(
NBT
);
std
::
vector
<
Matrix3
>
Rs
(
NBT
);
for
(
in
t
i
=
0
;
i
<
NBT
;
++
i
)
for
(
size_
t
i
=
0
;
i
<
NBT
;
++
i
)
Rs
[
(
std
::
size_t
)
i
]
=
(
Eigen
::
Quaterniond
(
Eigen
::
Matrix
<
double
,
4
,
1
>::
Random
())).
normalized
().
matrix
();
Rs
[
i
]
=
(
Eigen
::
Quaterniond
(
Eigen
::
Matrix
<
double
,
4
,
1
>::
Random
())).
normalized
().
matrix
();
std
::
cout
<<
"Pinocchio: "
;
std
::
cout
<<
"Pinocchio: "
;
StackTicToc
timer
(
StackTicToc
::
US
);
timer
.
tic
();
StackTicToc
timer
(
StackTicToc
::
US
);
timer
.
tic
();
SMOOTH
(
NBT
)
SMOOTH
(
NBT
)
{
{
timeSym3
(
S
,
Rs
[
(
std
::
size_t
)
_smooth
],
Sres
[
(
std
::
size_t
)
_smooth
]);
timeSym3
(
S
,
Rs
[
_smooth
],
Sres
[
_smooth
]);
}
}
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
}
}
...
@@ -249,10 +249,10 @@ BOOST_AUTO_TEST_CASE ( test_metapod_LTI )
...
@@ -249,10 +249,10 @@ BOOST_AUTO_TEST_CASE ( test_metapod_LTI )
timeLTI
(
S
,
R
,
S2
);
timeLTI
(
S
,
R
,
S2
);
is_matrix_absolutely_closed
(
S2
.
toMatrix
(),
R
.
toMatrix
().
transpose
()
*
S
.
toMatrix
()
*
R
.
toMatrix
(),
1e-12
);
is_matrix_absolutely_closed
(
S2
.
toMatrix
(),
R
.
toMatrix
().
transpose
()
*
S
.
toMatrix
()
*
R
.
toMatrix
(),
1e-12
);
const
in
t
NBT
=
100000
;
const
size_
t
NBT
=
100000
;
std
::
vector
<
Sym3
>
Sres
(
NBT
);
std
::
vector
<
Sym3
>
Sres
(
NBT
);
std
::
vector
<
R3
>
Rs
(
NBT
);
std
::
vector
<
R3
>
Rs
(
NBT
);
for
(
in
t
i
=
0
;
i
<
NBT
;
++
i
)
for
(
size_
t
i
=
0
;
i
<
NBT
;
++
i
)
Rs
[
i
].
randomInit
();
Rs
[
i
].
randomInit
();
std
::
cout
<<
"Metapod: "
;
std
::
cout
<<
"Metapod: "
;
...
@@ -297,17 +297,17 @@ BOOST_AUTO_TEST_CASE ( test_eigen_SelfAdj )
...
@@ -297,17 +297,17 @@ BOOST_AUTO_TEST_CASE ( test_eigen_SelfAdj )
is_matrix_absolutely_closed
(
Masa1
,
Masa2
,
1e-16
);
is_matrix_absolutely_closed
(
Masa1
,
Masa2
,
1e-16
);
}
}
const
in
t
NBT
=
100000
;
const
size_
t
NBT
=
100000
;
std
::
vector
<
Eigen
::
Matrix3d
>
Sres
(
NBT
);
std
::
vector
<
Eigen
::
Matrix3d
>
Sres
(
NBT
);
std
::
vector
<
Eigen
::
Matrix3d
>
Rs
(
NBT
);
std
::
vector
<
Eigen
::
Matrix3d
>
Rs
(
NBT
);
for
(
in
t
i
=
0
;
i
<
NBT
;
++
i
)
for
(
size_
t
i
=
0
;
i
<
NBT
;
++
i
)
Rs
[
(
std
::
size_t
)
i
]
=
(
Eigen
::
Quaterniond
(
Eigen
::
Matrix
<
double
,
4
,
1
>::
Random
())).
normalized
().
matrix
();
Rs
[
i
]
=
(
Eigen
::
Quaterniond
(
Eigen
::
Matrix
<
double
,
4
,
1
>::
Random
())).
normalized
().
matrix
();
std
::
cout
<<
"Eigen: "
;
std
::
cout
<<
"Eigen: "
;
StackTicToc
timer
(
StackTicToc
::
US
);
timer
.
tic
();
StackTicToc
timer
(
StackTicToc
::
US
);
timer
.
tic
();
SMOOTH
(
NBT
)
SMOOTH
(
NBT
)
{
{
timeSelfAdj
(
Rs
[
(
std
::
size_t
)
_smooth
],
M
,
Sres
[
(
std
::
size_t
)
_smooth
]);
timeSelfAdj
(
Rs
[
_smooth
],
M
,
Sres
[
_smooth
]);
}
}
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
toc
(
std
::
cout
,
NBT
);
}
}
...
...
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