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:
- "2.7"
sudo: required
dist: trusty
git:
depth: false
compiler:
- gcc
# - clang
......
......@@ -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)
......
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 @@
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)
//
// 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 @@
// 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::MatrixXd & drnea_dq,
Eigen::MatrixXd & drnea_dv,
Eigen::MatrixXd & drnea_da)
const Eigen::MatrixBase<Matrix1> & _drnea_dq,
const Eigen::MatrixBase<Matrix2> & _drnea_dv,
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;
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::RowMatrixXd & daba_dtau)
se3::Data::RowMatrixXs & 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::RowMatrixXd daba_dtau(Data::RowMatrixXd::Zero(model.nv,model.nv));
Data::RowMatrixXs daba_dtau(Data::RowMatrixXs::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)
{
......
......@@ -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;
se3::Data data(model);