Unverified Commit 14ae4bf0 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
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
Subproject commit 00d90b0f0c3aa0579667d963cc91cae4290add3c Subproject commit 083fa2cb0fa4ad594926d9bb3d246075e62ce9ee
...@@ -3,6 +3,8 @@ python: ...@@ -3,6 +3,8 @@ python:
- "2.7" - "2.7"
sudo: required sudo: required
dist: trusty dist: trusty
git:
depth: false
compiler: compiler:
- gcc - gcc
# - clang # - clang
......
...@@ -26,6 +26,7 @@ INCLUDE(cmake/test.cmake) ...@@ -26,6 +26,7 @@ INCLUDE(cmake/test.cmake)
SET(PROJECT_NAME pinocchio) SET(PROJECT_NAME pinocchio)
SET(PROJECT_DESCRIPTION "Rigid multi body dynamics algorithms") SET(PROJECT_DESCRIPTION "Rigid multi body dynamics algorithms")
SET(PROJECT_URL "http://github.com/stack-of-tasks/pinocchio") 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) OPTION(INSTALL_DOCUMENTATION "Generate and install the documentation" ON)
SET(DOXYGEN_USE_MATHJAX YES) SET(DOXYGEN_USE_MATHJAX YES)
...@@ -83,9 +84,24 @@ ENDMACRO(TAG_LIBRARY_VERSION) ...@@ -83,9 +84,24 @@ ENDMACRO(TAG_LIBRARY_VERSION)
# --- DEPENDANCIES ----------------------------------- # --- DEPENDANCIES -----------------------------------
# ---------------------------------------------------- # ----------------------------------------------------
ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5") ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
ADD_OPTIONAL_DEPENDENCY("metapod >= 1.0.7")
ADD_OPTIONAL_DEPENDENCY("urdfdom >= 0.2.0") ADD_OPTIONAL_DEPENDENCY("urdfdom >= 0.2.0")
ADD_OPTIONAL_DEPENDENCY("hpp-fcl >= 0.5.1") 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 # Special care of urdfdom version
IF(URDFDOM_FOUND) IF(URDFDOM_FOUND)
...@@ -155,19 +171,29 @@ SET(${PROJECT_NAME}_MATH_HEADERS ...@@ -155,19 +171,29 @@ SET(${PROJECT_NAME}_MATH_HEADERS
math/sincos.hpp math/sincos.hpp
math/quaternion.hpp math/quaternion.hpp
math/matrix.hpp math/matrix.hpp
math/cppad.hpp
math/cppadcg.hpp
) )
SET(${PROJECT_NAME}_UTILS_HEADERS SET(${PROJECT_NAME}_UTILS_HEADERS
utils/axis-label.hpp utils/axis-label.hpp
utils/eigen-fix.hpp
utils/timer.hpp utils/timer.hpp
utils/string-generator.hpp utils/string-generator.hpp
utils/file-explorer.hpp utils/file-explorer.hpp
utils/version.hpp utils/version.hpp
) )
SET(${PROJECT_NAME}_CODEGEN_HEADERS
codegen/code-generator-base.hpp
codegen/code-generator-algo.hpp
)
SET(${PROJECT_NAME}_SPATIAL_HEADERS SET(${PROJECT_NAME}_SPATIAL_HEADERS
spatial/symmetric3.hpp spatial/symmetric3.hpp
spatial/se3.hpp spatial/se3.hpp
spatial/se3-base.hpp
spatial/se3-tpl.hpp
spatial/motion.hpp spatial/motion.hpp
spatial/motion-base.hpp spatial/motion-base.hpp
spatial/motion-dense.hpp spatial/motion-dense.hpp
...@@ -185,6 +211,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS ...@@ -185,6 +211,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
spatial/act-on-set.hpp spatial/act-on-set.hpp
spatial/act-on-set.hxx spatial/act-on-set.hxx
spatial/explog.hpp spatial/explog.hpp
spatial/explog-quaternion.hpp
spatial/cartesian-axis.hpp spatial/cartesian-axis.hpp
spatial/spatial-axis.hpp spatial/spatial-axis.hpp
) )
...@@ -192,6 +219,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS ...@@ -192,6 +219,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/fwd.hpp multibody/joint/fwd.hpp
multibody/joint/joint-base.hpp multibody/joint/joint-base.hpp
multibody/joint/joint-collection.hpp
multibody/joint/joint-revolute.hpp multibody/joint/joint-revolute.hpp
multibody/joint/joint-revolute-unaligned.hpp multibody/joint/joint-revolute-unaligned.hpp
multibody/joint/joint-revolute-unbounded.hpp multibody/joint/joint-revolute-unbounded.hpp
...@@ -202,8 +230,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS ...@@ -202,8 +230,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-planar.hpp multibody/joint/joint-planar.hpp
multibody/joint/joint-translation.hpp multibody/joint/joint-translation.hpp
multibody/joint/joint-free-flyer.hpp multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp multibody/joint/joint-generic.hpp
multibody/joint/joint.hpp
multibody/joint/joint-basic-visitors.hpp multibody/joint/joint-basic-visitors.hpp
multibody/joint/joint-basic-visitors.hxx multibody/joint/joint-basic-visitors.hxx
multibody/joint/joint-composite.hpp multibody/joint/joint-composite.hpp
...@@ -211,14 +238,17 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS ...@@ -211,14 +238,17 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
) )
SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
multibody/liegroup/fwd.hpp
multibody/liegroup/liegroup.hpp multibody/liegroup/liegroup.hpp
multibody/liegroup/liegroup-algo.hpp multibody/liegroup/liegroup-algo.hpp
multibody/liegroup/liegroup-algo.hxx multibody/liegroup/liegroup-algo.hxx
multibody/liegroup/liegroup-collection.hpp
multibody/liegroup/liegroup-generic.hpp
multibody/liegroup/liegroup-variant.hpp multibody/liegroup/liegroup-variant.hpp
multibody/liegroup/liegroup-variant-visitors.hpp multibody/liegroup/liegroup-variant-visitors.hpp
multibody/liegroup/liegroup-variant-visitors.hxx multibody/liegroup/liegroup-variant-visitors.hxx
multibody/liegroup/operation-base.hpp multibody/liegroup/liegroup-base.hpp
multibody/liegroup/operation-base.hxx multibody/liegroup/liegroup-base.hxx
multibody/liegroup/vector-space.hpp multibody/liegroup/vector-space.hpp
multibody/liegroup/cartesian-product.hpp multibody/liegroup/cartesian-product.hpp
multibody/liegroup/special-orthogonal.hpp multibody/liegroup/special-orthogonal.hpp
...@@ -244,6 +274,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS ...@@ -244,6 +274,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
algorithm/aba-derivatives.hxx algorithm/aba-derivatives.hxx
algorithm/centroidal.hpp algorithm/centroidal.hpp
algorithm/centroidal.hxx algorithm/centroidal.hxx
algorithm/centroidal-derivatives.hpp
algorithm/centroidal-derivatives.hxx
algorithm/rnea.hpp algorithm/rnea.hpp
algorithm/rnea.hxx algorithm/rnea.hxx
algorithm/rnea-derivatives.hpp algorithm/rnea-derivatives.hpp
...@@ -285,11 +317,14 @@ SET(${PROJECT_NAME}_PARSERS_HEADERS ...@@ -285,11 +317,14 @@ SET(${PROJECT_NAME}_PARSERS_HEADERS
parsers/sample-models.hpp parsers/sample-models.hpp
parsers/utils.hpp parsers/utils.hpp
parsers/srdf.hpp parsers/srdf.hpp
parsers/srdf.hxx
) )
IF(URDFDOM_FOUND) IF(URDFDOM_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
parsers/urdf.hpp parsers/urdf.hpp
parsers/urdf/model.hxx
parsers/urdf/geometry.hxx
parsers/urdf/utils.hpp parsers/urdf/utils.hpp
parsers/urdf/types.hpp parsers/urdf/types.hpp
) )
...@@ -337,9 +372,12 @@ SET(HEADERS ...@@ -337,9 +372,12 @@ SET(HEADERS
${${PROJECT_NAME}_PARSERS_HEADERS} ${${PROJECT_NAME}_PARSERS_HEADERS}
${${PROJECT_NAME}_ALGORITHM_HEADERS} ${${PROJECT_NAME}_ALGORITHM_HEADERS}
${${PROJECT_NAME}_CONTAINER_HEADERS} ${${PROJECT_NAME}_CONTAINER_HEADERS}
${${PROJECT_NAME}_CODEGEN_HEADERS}
exception.hpp exception.hpp
assert.hpp assert.hpp
macros.hpp macros.hpp
eigen-macros.hpp
fwd.hpp
) )
LIST(REMOVE_DUPLICATES HEADERS) LIST(REMOVE_DUPLICATES HEADERS)
...@@ -355,6 +393,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf") ...@@ -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/utils")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm") 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/container")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/codegen")
FOREACH(header ${HEADERS}) FOREACH(header ${HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME) GET_FILENAME_COMPONENT(headerName ${header} NAME)
......
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
...@@ -20,23 +20,32 @@ ...@@ -20,23 +20,32 @@
ADD_CUSTOM_TARGET(bench) ADD_CUSTOM_TARGET(bench)
MACRO(ADD_BENCH bench_name) MACRO(ADD_BENCH bench_name)
IF(BUILD_BENCHMARK) IF(BUILD_BENCHMARK)
ADD_EXECUTABLE(${bench_name} ${bench_name}.cpp) ADD_EXECUTABLE(${bench_name} ${bench_name}.cpp)
ELSE(BUILD_BENCHMARK) ELSE(BUILD_BENCHMARK)
ADD_EXECUTABLE(${bench_name} EXCLUDE_FROM_ALL ${bench_name}.cpp) ADD_EXECUTABLE(${bench_name} EXCLUDE_FROM_ALL ${bench_name}.cpp)
ENDIF(BUILD_BENCHMARK) ENDIF(BUILD_BENCHMARK)
SET(ExtraMacroArgs ${ARGN}) SET(ExtraMacroArgs ${ARGN})
LIST(LENGTH ExtraMacroArgs NumExtraMacroArgs) LIST(LENGTH ExtraMacroArgs NumExtraMacroArgs)
IF(NumExtraMacroArgs GREATER 0) IF(NumExtraMacroArgs GREATER 0)
SET(link_to_main_lib ${ARGV1}) SET(link_to_main_lib ${ARGV1})
IF(link_to_main_lib) IF(link_to_main_lib)
SET_TARGET_PROPERTIES(${bench_name} PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}") SET_TARGET_PROPERTIES(${bench_name} PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")
TARGET_LINK_LIBRARIES(${bench_name} ${PROJECT_NAME}) TARGET_LINK_LIBRARIES(${bench_name} ${PROJECT_NAME})
PKG_CONFIG_USE_DEPENDENCY(${bench_name} eigen3) PKG_CONFIG_USE_DEPENDENCY(${bench_name} eigen3)
ENDIF(link_to_main_lib)
ENDIF()
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) ENDMACRO(ADD_BENCH)
MACRO(ADD_TEST_CFLAGS target flag) MACRO(ADD_TEST_CFLAGS target flag)
...@@ -46,15 +55,27 @@ ENDMACRO(ADD_TEST_CFLAGS) ...@@ -46,15 +55,27 @@ ENDMACRO(ADD_TEST_CFLAGS)
# timings # timings
# #
ADD_BENCH(timings TRUE) 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 # timings
# #
ADD_BENCH(timings-cholesky TRUE) ADD_BENCH(timings-cholesky TRUE)
# timings derivatives # timings derivatives
#
ADD_BENCH(timings-derivatives TRUE) ADD_BENCH(timings-derivatives TRUE)
TARGET_LINK_LIBRARIES(timings-derivatives ${PROJECT_NAME}) 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 # timings-eigen
# #
...@@ -64,7 +85,6 @@ ADD_BENCH(timings-eigen) ...@@ -64,7 +85,6 @@ ADD_BENCH(timings-eigen)
# #
IF(URDFDOM_FOUND AND HPP_FCL_FOUND) IF(URDFDOM_FOUND AND HPP_FCL_FOUND)
ADD_BENCH(timings-geometry TRUE) ADD_BENCH(timings-geometry TRUE)
PKG_CONFIG_USE_DEPENDENCY(timings-geometry hpp-fcl)
ENDIF(URDFDOM_FOUND AND HPP_FCL_FOUND) ENDIF(URDFDOM_FOUND AND HPP_FCL_FOUND)
//
// 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;
}
...@@ -15,11 +15,6 @@ ...@@ -15,11 +15,6 @@
// Pinocchio If not, see // Pinocchio If not, see
// <http://www.gnu.org/licenses/>. // <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/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp" #include "pinocchio/algorithm/kinematics-derivatives.hpp"
...@@ -37,14 +32,19 @@ ...@@ -37,14 +32,19 @@
#include "pinocchio/utils/timer.hpp" #include "pinocchio/utils/timer.hpp"
template<typename Matrix1, typename Matrix2, typename Matrix3>
void rnea_fd(const se3::Model & model, se3::Data & data_fd, void rnea_fd(const se3::Model & model, se3::Data & data_fd,
const Eigen::VectorXd & q, const Eigen::VectorXd & q,
const Eigen::VectorXd & v, const Eigen::VectorXd & v,
const Eigen::VectorXd & a, const Eigen::VectorXd & a,
Eigen::MatrixXd & drnea_dq, const Eigen::MatrixBase<Matrix1> & _drnea_dq,
Eigen::MatrixXd & drnea_dv, const Eigen::MatrixBase<Matrix2> & _drnea_dv,
Eigen::MatrixXd & drnea_da) const Eigen::MatrixBase<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; using namespace Eigen;
VectorXd v_eps(VectorXd::Zero(model.nv)); VectorXd v_eps(VectorXd::Zero(model.nv));
VectorXd q_plus(model.nq); VectorXd q_plus(model.nq);
...@@ -77,8 +77,8 @@ void rnea_fd(const se3::Model & model, se3::Data & data_fd, ...@@ -77,8 +77,8 @@ void rnea_fd(const se3::Model & model, se3::Data & data_fd,
// dRNEA/da // dRNEA/da
drnea_da = crba(model,data_fd,q); drnea_da = crba(model,data_fd,q);
drnea_da.triangularView<Eigen::StrictlyLower>() drnea_da.template triangularView<Eigen::StrictlyLower>()
= drnea_da.transpose().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, ...@@ -88,7 +88,7 @@ void aba_fd(const se3::Model & model, se3::Data & data_fd,
const Eigen::VectorXd & tau, const Eigen::VectorXd & tau,
Eigen::MatrixXd & daba_dq, Eigen::MatrixXd & daba_dq,