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
Guilhem Saurel
pinocchio
Commits
14ae4bf0
Unverified
Commit
14ae4bf0
authored
Oct 29, 2018
by
Justin Carpentier
Committed by
GitHub
Oct 29, 2018
Browse files
Merge pull request #569 from jcarpent/devel
Version 2.0.0-alpha
parents
d2026ed4
5f900aea
Pipeline
#3186
failed with stage
in 4 minutes and 46 seconds
Changes
201
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
.travis
@
083fa2cb
Compare
00d90b0f
...
083fa2cb
Subproject commit 0
0d90b0f0c3aa0579667d963cc91cae4290add3c
Subproject commit 0
83fa2cb0fa4ad594926d9bb3d246075e62ce9ee
.travis.yml
View file @
14ae4bf0
...
...
@@ -3,6 +3,8 @@ python:
-
"
2.7"
sudo
:
required
dist
:
trusty
git
:
depth
:
false
compiler
:
-
gcc
# - clang
...
...
CMakeLists.txt
View file @
14ae4bf0
...
...
@@ -26,6 +26,7 @@ INCLUDE(cmake/test.cmake)
SET
(
PROJECT_NAME pinocchio
)
SET
(
PROJECT_DESCRIPTION
"Rigid multi body dynamics algorithms"
)
SET
(
PROJECT_URL
"http://github.com/stack-of-tasks/pinocchio"
)
SET
(
PROJECT_CUSTOM_HEADER_EXTENSION
"hpp"
)
OPTION
(
INSTALL_DOCUMENTATION
"Generate and install the documentation"
ON
)
SET
(
DOXYGEN_USE_MATHJAX YES
)
...
...
@@ -83,9 +84,24 @@ ENDMACRO(TAG_LIBRARY_VERSION)
# --- DEPENDANCIES -----------------------------------
# ----------------------------------------------------
ADD_REQUIRED_DEPENDENCY
(
"eigen3 >= 3.0.5"
)
ADD_OPTIONAL_DEPENDENCY
(
"metapod >= 1.0.7"
)
ADD_OPTIONAL_DEPENDENCY
(
"urdfdom >= 0.2.0"
)
ADD_OPTIONAL_DEPENDENCY
(
"hpp-fcl >= 0.5.1"
)
ADD_OPTIONAL_DEPENDENCY
(
"cppad"
)
IF
(
CPPAD_FOUND
)
ADD_DEFINITIONS
(
-DPINOCCHIO_WITH_CPPAD_SUPPORT
)
PKG_CONFIG_APPEND_CFLAGS
(
"-DPINOCCHIO_WITH_CPPAD_SUPPORT"
)
IF
(
NOT
${
CPPAD_VERSION
}
VERSION_GREATER
"3.3.0"
)
ADD_DEFINITIONS
(
-DPINOCCHIO_CPPAD_REQUIRES_MATRIX_BASE_PLUGIN
)
PKG_CONFIG_APPEND_CFLAGS
(
"-DPINOCCHIO_CPPAD_REQUIRES_MATRIX_BASE_PLUGIN"
)
ENDIF
(
NOT
${
CPPAD_VERSION
}
VERSION_GREATER
"3.3.0"
)
ADD_OPTIONAL_DEPENDENCY
(
"cppadcg"
)
IF
(
CPPADCG_FOUND
)
ADD_DEFINITIONS
(
-DPINOCCHIO_WITH_CPPADCG_SUPPORT
)
PKG_CONFIG_APPEND_CFLAGS
(
"-DPINOCCHIO_WITH_CPPADCG_SUPPORT"
)
ENDIF
(
CPPADCG_FOUND
)
ENDIF
(
CPPAD_FOUND
)
# Special care of urdfdom version
IF
(
URDFDOM_FOUND
)
...
...
@@ -155,19 +171,29 @@ SET(${PROJECT_NAME}_MATH_HEADERS
math/sincos.hpp
math/quaternion.hpp
math/matrix.hpp
math/cppad.hpp
math/cppadcg.hpp
)
SET
(
${
PROJECT_NAME
}
_UTILS_HEADERS
utils/axis-label.hpp
utils/eigen-fix.hpp
utils/timer.hpp
utils/string-generator.hpp
utils/file-explorer.hpp
utils/version.hpp
)
SET
(
${
PROJECT_NAME
}
_CODEGEN_HEADERS
codegen/code-generator-base.hpp
codegen/code-generator-algo.hpp
)
SET
(
${
PROJECT_NAME
}
_SPATIAL_HEADERS
spatial/symmetric3.hpp
spatial/se3.hpp
spatial/se3-base.hpp
spatial/se3-tpl.hpp
spatial/motion.hpp
spatial/motion-base.hpp
spatial/motion-dense.hpp
...
...
@@ -185,6 +211,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
spatial/act-on-set.hpp
spatial/act-on-set.hxx
spatial/explog.hpp
spatial/explog-quaternion.hpp
spatial/cartesian-axis.hpp
spatial/spatial-axis.hpp
)
...
...
@@ -192,6 +219,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
SET
(
${
PROJECT_NAME
}
_MULTIBODY_JOINT_HEADERS
multibody/joint/fwd.hpp
multibody/joint/joint-base.hpp
multibody/joint/joint-collection.hpp
multibody/joint/joint-revolute.hpp
multibody/joint/joint-revolute-unaligned.hpp
multibody/joint/joint-revolute-unbounded.hpp
...
...
@@ -202,8 +230,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-planar.hpp
multibody/joint/joint-translation.hpp
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
multibody/joint/joint.hpp
multibody/joint/joint-generic.hpp
multibody/joint/joint-basic-visitors.hpp
multibody/joint/joint-basic-visitors.hxx
multibody/joint/joint-composite.hpp
...
...
@@ -211,14 +238,17 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
)
SET
(
${
PROJECT_NAME
}
_MULTIBODY_LIEGROUP_HEADERS
multibody/liegroup/fwd.hpp
multibody/liegroup/liegroup.hpp
multibody/liegroup/liegroup-algo.hpp
multibody/liegroup/liegroup-algo.hxx
multibody/liegroup/liegroup-collection.hpp
multibody/liegroup/liegroup-generic.hpp
multibody/liegroup/liegroup-variant.hpp
multibody/liegroup/liegroup-variant-visitors.hpp
multibody/liegroup/liegroup-variant-visitors.hxx
multibody/liegroup/
operation
-base.hpp
multibody/liegroup/
operation
-base.hxx
multibody/liegroup/
liegroup
-base.hpp
multibody/liegroup/
liegroup
-base.hxx
multibody/liegroup/vector-space.hpp
multibody/liegroup/cartesian-product.hpp
multibody/liegroup/special-orthogonal.hpp
...
...
@@ -244,6 +274,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/aba-derivatives.hxx
algorithm/centroidal.hpp
algorithm/centroidal.hxx
algorithm/centroidal-derivatives.hpp
algorithm/centroidal-derivatives.hxx
algorithm/rnea.hpp
algorithm/rnea.hxx
algorithm/rnea-derivatives.hpp
...
...
@@ -285,11 +317,14 @@ SET(${PROJECT_NAME}_PARSERS_HEADERS
parsers/sample-models.hpp
parsers/utils.hpp
parsers/srdf.hpp
parsers/srdf.hxx
)
IF
(
URDFDOM_FOUND
)
LIST
(
APPEND
${
PROJECT_NAME
}
_PARSERS_HEADERS
parsers/urdf.hpp
parsers/urdf/model.hxx
parsers/urdf/geometry.hxx
parsers/urdf/utils.hpp
parsers/urdf/types.hpp
)
...
...
@@ -337,9 +372,12 @@ SET(HEADERS
${${
PROJECT_NAME
}
_PARSERS_HEADERS
}
${${
PROJECT_NAME
}
_ALGORITHM_HEADERS
}
${${
PROJECT_NAME
}
_CONTAINER_HEADERS
}
${${
PROJECT_NAME
}
_CODEGEN_HEADERS
}
exception.hpp
assert.hpp
macros.hpp
eigen-macros.hpp
fwd.hpp
)
LIST
(
REMOVE_DUPLICATES HEADERS
)
...
...
@@ -355,6 +393,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
MAKE_DIRECTORY
(
"
${${
PROJECT_NAME
}
_BINARY_DIR
}
/include/pinocchio/utils"
)
MAKE_DIRECTORY
(
"
${${
PROJECT_NAME
}
_BINARY_DIR
}
/include/pinocchio/algorithm"
)
MAKE_DIRECTORY
(
"
${${
PROJECT_NAME
}
_BINARY_DIR
}
/include/pinocchio/container"
)
MAKE_DIRECTORY
(
"
${${
PROJECT_NAME
}
_BINARY_DIR
}
/include/pinocchio/codegen"
)
FOREACH
(
header
${
HEADERS
}
)
GET_FILENAME_COMPONENT
(
headerName
${
header
}
NAME
)
...
...
NEWS
deleted
100644 → 0
View file @
d2026ed4
New in 1.0.1, 04/30/2015:
* Added computation of non-linear-effects
* Now handle fixed joint when parsing urdf files and keep trace of fixed bodies
* Added Lua parser to build models
* Added planar and translation joints
* Fixed function se3ToXYZQUAT used with python
\ No newline at end of file
benchmark/CMakeLists.txt
View file @
14ae4bf0
...
...
@@ -20,23 +20,32 @@
ADD_CUSTOM_TARGET
(
bench
)
MACRO
(
ADD_BENCH bench_name
)
IF
(
BUILD_BENCHMARK
)
ADD_EXECUTABLE
(
${
bench_name
}
${
bench_name
}
.cpp
)
ELSE
(
BUILD_BENCHMARK
)
ADD_EXECUTABLE
(
${
bench_name
}
EXCLUDE_FROM_ALL
${
bench_name
}
.cpp
)
ENDIF
(
BUILD_BENCHMARK
)
SET
(
ExtraMacroArgs
${
ARGN
}
)
LIST
(
LENGTH ExtraMacroArgs NumExtraMacroArgs
)
IF
(
NumExtraMacroArgs GREATER 0
)
SET
(
link_to_main_lib
${
ARGV1
}
)
IF
(
link_to_main_lib
)
SET_TARGET_PROPERTIES
(
${
bench_name
}
PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR=
"
${${
PROJECT_NAME
}
_SOURCE_DIR
}
"
)
TARGET_LINK_LIBRARIES
(
${
bench_name
}
${
PROJECT_NAME
}
)
PKG_CONFIG_USE_DEPENDENCY
(
${
bench_name
}
eigen3
)
ENDIF
(
link_to_main_lib
)
ENDIF
()
IF
(
BUILD_BENCHMARK
)
ADD_EXECUTABLE
(
${
bench_name
}
${
bench_name
}
.cpp
)
ELSE
(
BUILD_BENCHMARK
)
ADD_EXECUTABLE
(
${
bench_name
}
EXCLUDE_FROM_ALL
${
bench_name
}
.cpp
)
ENDIF
(
BUILD_BENCHMARK
)
SET
(
ExtraMacroArgs
${
ARGN
}
)
LIST
(
LENGTH ExtraMacroArgs NumExtraMacroArgs
)
IF
(
NumExtraMacroArgs GREATER 0
)
SET
(
link_to_main_lib
${
ARGV1
}
)
IF
(
link_to_main_lib
)
SET_TARGET_PROPERTIES
(
${
bench_name
}
PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR=
"
${${
PROJECT_NAME
}
_SOURCE_DIR
}
"
)
TARGET_LINK_LIBRARIES
(
${
bench_name
}
${
PROJECT_NAME
}
)
PKG_CONFIG_USE_DEPENDENCY
(
${
bench_name
}
eigen3
)
ADD_DEPENDENCIES
(
bench
${
bench_name
}
)
IF
(
HPP_FCL_FOUND
)
PKG_CONFIG_USE_DEPENDENCY
(
${
bench_name
}
hpp-fcl
)
ENDIF
(
HPP_FCL_FOUND
)
IF
(
URDFDOM_FOUND
)
PKG_CONFIG_USE_DEPENDENCY
(
${
bench_name
}
urdfdom
)
ENDIF
(
URDFDOM_FOUND
)
ENDIF
(
link_to_main_lib
)
ENDIF
()
ADD_DEPENDENCIES
(
bench
${
bench_name
}
)
ENDMACRO
(
ADD_BENCH
)
MACRO
(
ADD_TEST_CFLAGS target flag
)
...
...
@@ -46,15 +55,27 @@ ENDMACRO(ADD_TEST_CFLAGS)
# timings
#
ADD_BENCH
(
timings TRUE
)
IF
(
CPPADCG_FOUND
)
ADD_BENCH
(
timings-cg TRUE
)
SET_PROPERTY
(
TARGET timings-cg PROPERTY CXX_STANDARD 11
)
PKG_CONFIG_USE_DEPENDENCY
(
timings-cg
"cppadcg"
)
ENDIF
(
CPPADCG_FOUND
)
# timings
#
ADD_BENCH
(
timings-cholesky TRUE
)
# timings derivatives
#
ADD_BENCH
(
timings-derivatives TRUE
)
TARGET_LINK_LIBRARIES
(
timings-derivatives
${
PROJECT_NAME
}
)
IF
(
CPPAD_FOUND
)
PKG_CONFIG_USE_DEPENDENCY
(
timings-derivatives
"cppad"
)
ENDIF
(
CPPAD_FOUND
)
IF
(
CPPADCG_FOUND
)
SET_PROPERTY
(
TARGET timings-derivatives PROPERTY CXX_STANDARD 11
)
PKG_CONFIG_USE_DEPENDENCY
(
timings-derivatives
"cppadcg"
)
ENDIF
(
CPPADCG_FOUND
)
# timings-eigen
#
...
...
@@ -64,7 +85,6 @@ ADD_BENCH(timings-eigen)
#
IF
(
URDFDOM_FOUND AND HPP_FCL_FOUND
)
ADD_BENCH
(
timings-geometry TRUE
)
PKG_CONFIG_USE_DEPENDENCY
(
timings-geometry hpp-fcl
)
ENDIF
(
URDFDOM_FOUND AND HPP_FCL_FOUND
)
benchmark/timings-cg.cpp
0 → 100644
View file @
14ae4bf0
//
// Copyright (c) 2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/codegen/code-generator-algo.hpp"
#include <iostream>
#include "pinocchio/utils/timer.hpp"
int
main
(
int
argc
,
const
char
**
argv
)
{
using
namespace
Eigen
;
using
namespace
se3
;
PinocchioTicToc
timer
(
PinocchioTicToc
::
US
);
#ifdef NDEBUG
const
int
NBT
=
1000
*
100
;
#else
const
int
NBT
=
1
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
<<
std
::
endl
;
#endif
se3
::
Model
model
;
std
::
string
filename
=
PINOCCHIO_SOURCE_DIR
"/models/simple_humanoid.urdf"
;
if
(
argc
>
1
)
filename
=
argv
[
1
];
bool
with_ff
=
true
;
if
(
argc
>
2
)
{
const
std
::
string
ff_option
=
argv
[
2
];
if
(
ff_option
==
"-no-ff"
)
with_ff
=
false
;
}
if
(
filename
==
"H"
)
se3
::
buildModels
::
humanoidRandom
(
model
,
true
);
else
if
(
with_ff
)
se3
::
urdf
::
buildModel
(
filename
,
JointModelFreeFlyer
(),
model
);
else
se3
::
urdf
::
buildModel
(
filename
,
model
);
std
::
cout
<<
"nq = "
<<
model
.
nq
<<
std
::
endl
;
std
::
cout
<<
"nv = "
<<
model
.
nv
<<
std
::
endl
;
std
::
cout
<<
"--"
<<
std
::
endl
;
se3
::
Data
data
(
model
);
CodeGenRNEA
<
double
>
rnea_code_gen
(
model
);
rnea_code_gen
.
initLib
();
rnea_code_gen
.
loadLib
();
CodeGenABA
<
double
>
aba_code_gen
(
model
);
aba_code_gen
.
initLib
();
aba_code_gen
.
loadLib
();
CodeGenCRBA
<
double
>
crba_code_gen
(
model
);
crba_code_gen
.
initLib
();
crba_code_gen
.
loadLib
();
CodeGenMinv
<
double
>
minv_code_gen
(
model
);
minv_code_gen
.
initLib
();
minv_code_gen
.
loadLib
();
CodeGenRNEADerivatives
<
double
>
rnea_derivatives_code_gen
(
model
);
rnea_derivatives_code_gen
.
initLib
();
rnea_derivatives_code_gen
.
loadLib
();
CodeGenABADerivatives
<
double
>
aba_derivatives_code_gen
(
model
);
aba_derivatives_code_gen
.
initLib
();
aba_derivatives_code_gen
.
loadLib
();
se3
::
container
::
aligned_vector
<
VectorXd
>
qs
(
NBT
);
se3
::
container
::
aligned_vector
<
VectorXd
>
qdots
(
NBT
);
se3
::
container
::
aligned_vector
<
VectorXd
>
qddots
(
NBT
);
se3
::
container
::
aligned_vector
<
VectorXd
>
taus
(
NBT
);
for
(
size_t
i
=
0
;
i
<
NBT
;
++
i
)
{
qs
[
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nq
);
qs
[
i
].
segment
<
4
>
(
3
)
/=
qs
[
i
].
segment
<
4
>
(
3
).
norm
();
qdots
[
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nv
);
qddots
[
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nv
);
taus
[
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nv
);
}
timer
.
tic
();
SMOOTH
(
NBT
)
{
rnea
(
model
,
data
,
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
]);
}
std
::
cout
<<
"RNEA =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
rnea_code_gen
.
evalFunction
(
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
]);
}
std
::
cout
<<
"RNEA generated =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
rnea_code_gen
.
evalJacobian
(
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
]);
}
std
::
cout
<<
"RNEA partial derivatives auto diff + code gen =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
rnea_derivatives_code_gen
.
evalFunction
(
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
]);
}
std
::
cout
<<
"RNEA partial derivatives code gen =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
aba
(
model
,
data
,
qs
[
_smooth
],
qdots
[
_smooth
],
taus
[
_smooth
]);
}
timer
.
toc
();
timer
.
tic
();
SMOOTH
(
NBT
)
{
crba
(
model
,
data
,
qs
[
_smooth
]);
}
std
::
cout
<<
"CRBA =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
crba_code_gen
.
evalFunction
(
qs
[
_smooth
]);
}
std
::
cout
<<
"CRBA generated =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
computeMinverse
(
model
,
data
,
qs
[
_smooth
]);
}
std
::
cout
<<
"Minv =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
minv_code_gen
.
evalFunction
(
qs
[
_smooth
]);
}
std
::
cout
<<
"Minv generated =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
aba
(
model
,
data
,
qs
[
_smooth
],
qdots
[
_smooth
],
taus
[
_smooth
]);
}
std
::
cout
<<
"ABA =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
aba_code_gen
.
evalFunction
(
qs
[
_smooth
],
qdots
[
_smooth
],
taus
[
_smooth
]);
}
std
::
cout
<<
"ABA generated =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
aba_code_gen
.
evalJacobian
(
qs
[
_smooth
],
qdots
[
_smooth
],
taus
[
_smooth
]);
}
std
::
cout
<<
"ABA partial derivatives auto diff + code gen =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
aba_derivatives_code_gen
.
evalFunction
(
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
]);
}
std
::
cout
<<
"ABA partial derivatives code gen =
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
return
0
;
}
benchmark/timings-derivatives.cpp
View file @
14ae4bf0
...
...
@@ -15,11 +15,6 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
...
...
@@ -37,14 +32,19 @@
#include "pinocchio/utils/timer.hpp"
template
<
typename
Matrix1
,
typename
Matrix2
,
typename
Matrix3
>
void
rnea_fd
(
const
se3
::
Model
&
model
,
se3
::
Data
&
data_fd
,
const
Eigen
::
VectorXd
&
q
,
const
Eigen
::
VectorXd
&
v
,
const
Eigen
::
VectorXd
&
a
,
Eigen
::
Matrix
Xd
&
drnea_dq
,
Eigen
::
Matrix
Xd
&
drnea_dv
,
Eigen
::
Matrix
Xd
&
drnea_da
)
const
Eigen
::
Matrix
Base
<
Matrix1
>
&
_
drnea_dq
,
const
Eigen
::
Matrix
Base
<
Matrix2
>
&
_
drnea_dv
,
const
Eigen
::
Matrix
Base
<
Matrix3
>
&
_
drnea_da
)
{
Matrix1
&
drnea_dq
=
EIGEN_CONST_CAST
(
Matrix1
,
_drnea_dq
);
Matrix2
&
drnea_dv
=
EIGEN_CONST_CAST
(
Matrix2
,
_drnea_dv
);
Matrix3
&
drnea_da
=
EIGEN_CONST_CAST
(
Matrix3
,
_drnea_da
);
using
namespace
Eigen
;
VectorXd
v_eps
(
VectorXd
::
Zero
(
model
.
nv
));
VectorXd
q_plus
(
model
.
nq
);
...
...
@@ -77,8 +77,8 @@ void rnea_fd(const se3::Model & model, se3::Data & data_fd,
// dRNEA/da
drnea_da
=
crba
(
model
,
data_fd
,
q
);
drnea_da
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
drnea_da
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
drnea_da
.
template
triangularView
<
Eigen
::
StrictlyLower
>()
=
drnea_da
.
transpose
().
template
triangularView
<
Eigen
::
StrictlyLower
>();
}
...
...
@@ -88,7 +88,7 @@ void aba_fd(const se3::Model & model, se3::Data & data_fd,
const
Eigen
::
VectorXd
&
tau
,
Eigen
::
MatrixXd
&
daba_dq
,
Eigen
::
MatrixXd
&
daba_dv
,
se3
::
Data
::
RowMatrixX
d
&
daba_dtau
)
se3
::
Data
::
RowMatrixX
s
&
daba_dtau
)
{
using
namespace
Eigen
;
VectorXd
v_eps
(
VectorXd
::
Zero
(
model
.
nv
));
...
...
@@ -152,11 +152,10 @@ int main(int argc, const char ** argv)
if
(
filename
==
"HS"
)
buildModels
::
humanoidRandom
(
model
,
true
);
else
if
(
filename
==
"H2"
)
buildModels
::
humanoid2d
(
model
);
else
if
(
with_ff
)
se3
::
urdf
::
buildModel
(
filename
,
JointModelFreeFlyer
(),
model
);
// se3::urdf::buildModel(filename,JointModelRX(),model);
else
se3
::
urdf
::
buildModel
(
filename
,
model
);
std
::
cout
<<
"nq = "
<<
model
.
nq
<<
std
::
endl
;
...
...
@@ -180,14 +179,14 @@ int main(int argc, const char ** argv)
qddots
[
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nv
);
taus
[
i
]
=
Eigen
::
VectorXd
::
Random
(
model
.
nv
);
}
MatrixXd
drnea_dq
(
MatrixXd
::
Zero
(
model
.
nv
,
model
.
nv
));
MatrixXd
drnea_dv
(
MatrixXd
::
Zero
(
model
.
nv
,
model
.
nv
));
EIGEN_PLAIN_ROW_MAJOR_TYPE
(
MatrixXd
)
drnea_dq
(
MatrixXd
::
Zero
(
model
.
nv
,
model
.
nv
));
EIGEN_PLAIN_ROW_MAJOR_TYPE
(
MatrixXd
)
drnea_dv
(
MatrixXd
::
Zero
(
model
.
nv
,
model
.
nv
));
MatrixXd
drnea_da
(
MatrixXd
::
Zero
(
model
.
nv
,
model
.
nv
));
MatrixXd
daba_dq
(
MatrixXd
::
Zero
(
model
.
nv
,
model
.
nv
));
MatrixXd
daba_dv
(
MatrixXd
::
Zero
(
model
.
nv
,
model
.
nv
));
Data
::
RowMatrixX
d
daba_dtau
(
Data
::
RowMatrixX
d
::
Zero
(
model
.
nv
,
model
.
nv
));
Data
::
RowMatrixX
s
daba_dtau
(
Data
::
RowMatrixX
s
::
Zero
(
model
.
nv
,
model
.
nv
));
timer
.
tic
();
SMOOTH
(
NBT
)
...
...
@@ -195,14 +194,14 @@ int main(int argc, const char ** argv)
forwardKinematics
(
model
,
data
,
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
]);
}
std
::
cout
<<
"FK=
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
computeForwardKinematicsDerivatives
(
model
,
data
,
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
]);
}
std
::
cout
<<
"FK derivatives=
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
...
...
@@ -217,15 +216,15 @@ int main(int argc, const char ** argv)
drnea_dq
,
drnea_dv
,
drnea_da
);
}
std
::
cout
<<
"RNEA derivatives=
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
SMOOTH
(
NBT
/
100
)
{
rnea_fd
(
model
,
data
,
qs
[
_smooth
],
qdots
[
_smooth
],
qddots
[
_smooth
],
drnea_dq
,
drnea_dv
,
drnea_da
);
}
std
::
cout
<<
"RNEA finite differences=
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
std
::
cout
<<
"RNEA finite differences=
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
/
100
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
...
...
@@ -248,7 +247,7 @@ int main(int argc, const char ** argv)
daba_dq
,
daba_dv
,
daba_dtau
);
}
std
::
cout
<<
"ABA finite differences=
\t\t
"
;
timer
.
toc
(
std
::
cout
,
NBT
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
...
...
benchmark/timings.cpp
View file @
14ae4bf0
...
...
@@ -56,13 +56,25 @@ int main(int argc, const char ** argv)
std
::
string
filename
=
PINOCCHIO_SOURCE_DIR
"/models/simple_humanoid.urdf"
;
if
(
argc
>
1
)
filename
=
argv
[
1
];
bool
with_ff
=
true
;
if
(
argc
>
2
)
{
const
std
::
string
ff_option
=
argv
[
2
];
if
(
ff_option
==
"-no-ff"
)
with_ff
=
false
;
}
if
(
filename
==
"HS"
)
se3
::
buildModels
::
humanoidRandom
(
model
,
true
);
else
if
(
filename
==
"H2"
)
se3
::
buildModels
::
humanoid2d
(
model
);
else
se3
::
urdf
::
buildModel
(
filename
,
JointModelFreeFlyer
(),
model
);
if
(
with_ff
)
se3
::
urdf
::
buildModel
(
filename
,
JointModelFreeFlyer
(),
model
);
else
se3
::
urdf
::
buildModel
(
filename
,
model
);
std
::
cout
<<
"nq = "
<<
model
.
nq
<<
std
::
endl
;