Skip to content
Snippets Groups Projects
Commit 78d18d5d authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

add nao & poppy models, rework submodules, try kdl

sorry for the big commit -_-
parent 3cb2245f
No related branches found
No related tags found
1 merge request!3Update the repository to the version used for the first submission of the article
[submodule "libs/kdl"]
path = libs/kdl
url = https://github.com/orocos/orocos_kinematics_dynamics.git
[submodule "libs/pinocchio"]
[submodule "pinocchio"]
path = libs/pinocchio
url = https://github.com/stack-of-tasks/pinocchio.git
url = ../../stack-of-tasks/pinocchio.git
shallow = true
fetchRecurseSubmodules = true
[submodule "benchmark"]
path = benchmark
url = https://github.com/google/benchmark.git
[submodule "googletest"]
path = googletest
url = https://github.com/google/googletest.git
path = libs/benchmark
url = ../../google/benchmark.git
shallow = true
fetchRecurseSubmodules = false
[submodule "nao"]
path = libs/nao_description
url = ../../iaslab-unipd/nao_description.git
shallow = true
fetchRecurseSubmodules = false
[submodule "poppy"]
path = libs/poppy-humanoid
url = ../../poppy-project/poppy-humanoid.git
shallow = true
fetchRecurseSubmodules = false
......@@ -15,9 +15,11 @@ ADD_REQUIRED_DEPENDENCY("eigen3")
ADD_REQUIRED_DEPENDENCY("pinocchio")
ADD_REQUIRED_DEPENDENCY("rbdl")
ADD_REQUIRED_DEPENDENCY("orocos-kdl")
ADD_REQUIRED_DEPENDENCY("kdl_parser")
ADD_REQUIRED_DEPENDENCY("urdfdom")
ADD_REQUIRED_DEPENDENCY("benchmark")
ADD_SUBDIRECTORY("libs")
ADD_SUBDIRECTORY("src")
SETUP_PROJECT_FINALIZE()
# Benchmarking Pinocchio
## Setup your environment
## Install Pinocchio & KDL binaries
You need both eigen2 (for KDL) and eigen3 (for RBDL & Pinocchio):
```
sudo apt install -qqy libeigen{2,3}-dev
sudo apt install -qqy robotpkg-pinocchio liborocos-kdl-dev libkdl-parser-dev
```
You have to choose the `PREFIX` in which you want to install Pinocchio, RBDL & KDL:
## Configure your prefix
```
export PREFIX=$PWD/prefix # with bash / zsh
......@@ -18,53 +16,13 @@ set -x PREFIX $PWD/prefix # with fish
set -x LD_LIBRARY_PATH $PREFIX/lib:$LD_LIBRARY_PATH
```
## Pinocchio
### Downloading
If you did not clone this repository with the `--recursive` options, you will have to get the submodules with:
```
git submodule update --init --recursive
```
### Installing
```
mkdir -p build/pinocchio
pushd build/pinocchio
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_UNIT_TESTS=OFF ../../libs/pinocchio
make -j8 install
popd
```
## KDL
### Installing
```
mkdir -p build/kdl
pushd build/kdl
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib ../../libs/kdl/orocos_kdl
make -j8 install
popd
```
## RBDL
### Downloading
RBDL uses mercurial, and git-hg can't clone it, so we can't provide a git submodule, and you have to download it:
## Install RBDL
```
wget https://bitbucket.org/rbdl/rbdl/get/default.zip
unzip default.zip
rm default.zip
mv rbdl-rbdl-* libs/rbdl
```
### Installing
```
mkdir -p build/rbdl
pushd build/rbdl
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DRBDL_BUILD_ADDON_URDFREADER=ON ../../libs/rbdl
......@@ -72,21 +30,17 @@ make -j8 install
popd
```
## Google Benchmark
### Installing
## Install Google Benchmark
```
mkdir -p build/google-benchmark
pushd build/google-benchmark
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DBENCHMARK_ENABLE_GTEST_TESTS=OFF ../../benchmark
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib -DBENCHMARK_ENABLE_GTEST_TESTS=OFF ../../libs/benchmark
make -j8 install
popd
```
## Benchmarks
### Installing
## Install
```
mkdir -p build/pinocchio-benchmarks
......@@ -99,12 +53,10 @@ popd
### Running
```
./prefix/bin/rbdl-bench models/simple_humanoid.urdf
./prefix/bin/rbdl-bench models/romeo/romeo_description/urdf/romeo.urdf
./prefix/bin/pinocchio-bench models/simple_humanoid.urdf
./prefix/bin/pinocchio-bench models/romeo/romeo_description/urdf/romeo.urdf
sudo cpupower frequency-set --governor performance
./prefix/bin/pinocchio-benchmark
./prefix/bin/rbdl-benchmark
$PREFIX/bin/rbdl-test
$PREFIX/bin/pinocchio-test
$PREFIX/bin/pinocchio-benchmark
$PREFIX/bin/rbdl-benchmark
sudo cpupower frequency-set --governor powersave
```
Subproject commit 1f35fa4aa71bffb5e5672f7ca876561d6adef4fd
INSTALL(FILES
"pinocchio/models/simple_humanoid.urdf"
"pinocchio/models/romeo/romeo_description/urdf/romeo.urdf"
"nao_description/urdf/nao_robot_v4.urdf"
"poppy-humanoid/hardware/URDF/robots/Poppy_Humanoid.URDF"
DESTINATION "share/pinocchio-benchmarks/models"
)
Subproject commit 63e183b38945a0d67671f94bd3f74d7ffb6f5a82
Subproject commit ce11d1c8158e57f255a85700373c0d0b105cad82
Subproject commit 9e1d7b0e1ed83a8f16dc9daf2fef0bb9a792a73d
Subproject commit 5b6c54e55cc8218757581041d6118a28c89515ed
libs/pinocchio/models
\ No newline at end of file
run.sh 0 → 100755
#!/bin/bash
set -e
#set -x
#rm -rf build prefix
#sudo apt install -qqy robotpkg-pinocchio liborocos-kdl-dev libkdl-parser-dev
export PREFIX=${1:-$PWD/prefix} # with bash / zsh
export LD_LIBRARY_PATH=$PREFIX/lib:$LD_LIBRARY_PATH
export CXX="ccache clang++-3.8"
export CC="ccache clang-3.8"
if [[ ! -d libs/rbdl ]]
then
wget https://bitbucket.org/rbdl/rbdl/get/default.zip
unzip default.zip
rm default.zip
mv rbdl-rbdl-* libs/rbdl
fi
mkdir -p build/rbdl
pushd build/rbdl
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib \
-DRBDL_BUILD_ADDON_URDFREADER=ON \
../../libs/rbdl
make -j8 install
popd
mkdir -p build/google-benchmark
pushd build/google-benchmark
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib \
-DBENCHMARK_ENABLE_GTEST_TESTS=OFF -DBENCHMARK_ENABLE_LTO=true \
-DLLVMAR_EXECUTABLE="/usr/bin/llvm-ar-3.8" \
-DLLVMNM_EXECUTABLE="/usr/bin/llvm-nm-3.8" \
-DLLVMRANLIB_EXECUTABLE="/usr/bin/llvm-ranlib-3.8" \
../../libs/benchmark
make -j8 install
popd
mkdir -p build/pinocchio-benchmarks
pushd build/pinocchio-benchmarks
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$PREFIX -DCMAKE_INSTALL_LIBDIR=lib \
-DCMAKE_PREFIX_PATH=$PREFIX \
../..
make -j8 install
popd
$PREFIX/bin/rbdl-test
$PREFIX/bin/pinocchio-test
$PREFIX/bin/kdl-test
echo "don't forget to run 'sudo cpupower frequency-set --governor performance'"
$PREFIX/bin/pinocchio-benchmark
$PREFIX/bin/rbdl-benchmark
echo "don't forget to run 'sudo cpupower frequency-set --governor powersave'"
# RBDL
ADD_EXECUTABLE(rbdl-bench rbdl-bench)
PKG_CONFIG_USE_DEPENDENCY(rbdl-bench rbdl)
TARGET_LINK_LIBRARIES(rbdl-bench rbdl_urdfreader)
INSTALL(TARGETS rbdl-bench RUNTIME DESTINATION bin)
CONFIG_FILES(models.h)
INCLUDE_DIRECTORIES("${CMAKE_CURRENT_BINARY_DIR}")
# Pinocchio
ADD_EXECUTABLE(pinocchio-bench pinocchio-bench)
PKG_CONFIG_USE_DEPENDENCY(pinocchio-bench pinocchio)
INSTALL(TARGETS pinocchio-bench RUNTIME DESTINATION bin)
# RBDL test
ADD_EXECUTABLE(rbdl-test rbdl-test)
PKG_CONFIG_USE_DEPENDENCY(rbdl-test rbdl)
TARGET_LINK_LIBRARIES(rbdl-test rbdl_urdfreader)
INSTALL(TARGETS rbdl-test RUNTIME DESTINATION bin)
# RBDL & benchmark
# Pinocchio test
ADD_EXECUTABLE(pinocchio-test pinocchio-test)
PKG_CONFIG_USE_DEPENDENCY(pinocchio-test pinocchio)
INSTALL(TARGETS pinocchio-test RUNTIME DESTINATION bin)
# KDL test
ADD_EXECUTABLE(kdl-test kdl-test)
PKG_CONFIG_USE_DEPENDENCY(kdl-test kdl_parser)
INSTALL(TARGETS kdl-test RUNTIME DESTINATION bin)
# RBDL benchmark
ADD_EXECUTABLE(rbdl-benchmark rbdl-benchmark)
PKG_CONFIG_USE_DEPENDENCY(rbdl-benchmark rbdl)
PKG_CONFIG_USE_DEPENDENCY(rbdl-benchmark benchmark)
TARGET_LINK_LIBRARIES(rbdl-benchmark rbdl_urdfreader pthread)
INSTALL(TARGETS rbdl-benchmark RUNTIME DESTINATION bin)
# Pinocchio & benchmark
# Pinocchio benchmark
ADD_EXECUTABLE(pinocchio-benchmark pinocchio-benchmark)
PKG_CONFIG_USE_DEPENDENCY(pinocchio-benchmark pinocchio)
PKG_CONFIG_USE_DEPENDENCY(pinocchio-benchmark benchmark)
......
#include <iostream>
#include <fstream>
#include <kdl_parser/kdl_parser.hpp>
#include "models.h"
void kdl_test(std::string model_file)
{
KDL::Tree tree;
kdl_parser::treeFromFile(pinocchio_benchmarks::path + model_file, tree);
std::cout << "KDL Test" << std::endl;
std::cout << " model: " << model_file << std::endl;
std::cout << " nq: " << tree.getNrOfJoints() << std::endl;
}
int main()
{
for (auto& model : pinocchio_benchmarks::models)
{
kdl_test(model);
}
return 0;
}
#ifndef __pinocchio_benchmarks_models__
#define __pinocchio_benchmarks_models__
#include <boost/array.hpp>
namespace pinocchio_benchmarks
{
static const std::string path = "@CMAKE_INSTALL_PREFIX@/share/pinocchio-benchmarks/models/";
static const boost::array<std::string, 4> models =
{
"simple_humanoid.urdf",
"romeo.urdf",
"nao_robot_v4.urdf",
"Poppy_Humanoid.URDF"
};
}
#endif
......@@ -8,10 +8,14 @@
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include "models.h"
static void BM_Pinocchio_RNEA(benchmark::State& state)
{
se3::Model model;
se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
se3::urdf::buildModel(pinocchio_benchmarks::path +
pinocchio_benchmarks::models[state.range(0)],
se3::JointModelFreeFlyer(), model);
se3::Data data(model);
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
......@@ -31,11 +35,15 @@ static void BM_Pinocchio_RNEA(benchmark::State& state)
}
}
static void BM_Pinocchio_ABA(benchmark::State& state)
{
std::string model_file = pinocchio_benchmarks::path +
pinocchio_benchmarks::models[state.range(0)];
se3::Model model;
se3::urdf::buildModel("models/simple_humanoid.urdf", se3::JointModelFreeFlyer(), model);
se3::urdf::buildModel(pinocchio_benchmarks::path +
pinocchio_benchmarks::models[state.range(0)],
se3::JointModelFreeFlyer(), model);
se3::Data data(model);
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
......@@ -55,7 +63,7 @@ static void BM_Pinocchio_ABA(benchmark::State& state)
}
}
BENCHMARK(BM_Pinocchio_RNEA);
BENCHMARK(BM_Pinocchio_ABA);
BENCHMARK(BM_Pinocchio_RNEA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
BENCHMARK(BM_Pinocchio_ABA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
BENCHMARK_MAIN();
......@@ -6,43 +6,39 @@
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/aba.hpp>
int main(int argc, char** argv)
{
if (argc != 2)
{
std::cerr << "You have to specify a path to an urdf file" << std::endl;
return 1;
}
std::ifstream path(argv[1]);
if (!path)
{
std::cerr << "This path is not a valid file: " << argv[1] << std::endl;
return 2;
}
#include "models.h"
void pinocchio_test(std::string model_file)
{
se3::Model model;
se3::urdf::buildModel(pinocchio_benchmarks::path + model_file,
se3::JointModelFreeFlyer(), model);
// Load an urdf file provided by the user
se3::urdf::buildModel(argv[1], se3::JointModelFreeFlyer(), model);
std::cout << "Pinocchio Benchmark" << std::endl;
std::cout << " model: " << argv[1] << std::endl;
std::cout << "Pinocchio Test" << std::endl;
std::cout << " model: " << model_file << std::endl;
std::cout << " nq: " << model.nq << std::endl;
std::cout << " nv: " << model.nv << std::endl;
se3::Data data(model);
Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq);
Eigen::VectorXd qdot = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd qddot = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
Eigen::VectorXd qdot = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd qddot = Eigen::VectorXd::Zero(model.nv);
q[0] = 1;
se3::aba(model, data, q, qdot, tau);
std::cout << "qddot after ABA: " << data.ddq.transpose() << std::endl;
se3::rnea(model, data, q, qdot, qddot);
std::cout << "tau aftern RNEA: " << data.tau.transpose() << std::endl;
}
int main()
{
for (auto& model : pinocchio_benchmarks::models)
{
pinocchio_test(model);
}
return 0;
}
......@@ -6,10 +6,13 @@
#include <rbdl/rbdl.h>
#include <rbdl/addons/urdfreader/urdfreader.h>
#include "models.h"
static void BM_RBDL_RNEA(benchmark::State& state)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
pinocchio_benchmarks::models[state.range(0)]).c_str(), model, true);
RigidBodyDynamics::Math::VectorNd q =
RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
......@@ -30,12 +33,15 @@ static void BM_RBDL_RNEA(benchmark::State& state)
RigidBodyDynamics::InverseDynamics(*model, q, qdot, tau, qddot);
}
delete model;
}
static void BM_RBDL_ABA(benchmark::State& state)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile("models/simple_humanoid.urdf", model, true);
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
pinocchio_benchmarks::models[state.range(0)]).c_str(), model, true);
RigidBodyDynamics::Math::VectorNd q =
RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
......@@ -56,10 +62,12 @@ static void BM_RBDL_ABA(benchmark::State& state)
RigidBodyDynamics::ForwardDynamics(*model, q, qdot, tau, qddot);
}
delete model;
}
BENCHMARK(BM_RBDL_RNEA);
BENCHMARK(BM_RBDL_ABA);
BENCHMARK(BM_RBDL_RNEA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
BENCHMARK(BM_RBDL_ABA)->Arg(0)->Arg(1)->Arg(2)->Arg(3);
BENCHMARK_MAIN();
......@@ -4,45 +4,45 @@
#include <rbdl/rbdl.h>
#include <rbdl/addons/urdfreader/urdfreader.h>
int main(int argc, char** argv)
{
if (argc != 2)
{
std::cerr << "You have to specify a path to an urdf file" << std::endl;
return 1;
}
std::ifstream path(argv[1]);
if (!path)
{
std::cerr << "This path is not a valid file: " << argv[1] << std::endl;
return 2;
}
#include "models.h"
void rbdl_test(std::string model_file)
{
RigidBodyDynamics::Model* model = new RigidBodyDynamics::Model();
RigidBodyDynamics::Addons::URDFReadFromFile((pinocchio_benchmarks::path +
model_file).c_str(), model, true);
// Load an urdf file provided by the user
RigidBodyDynamics::Addons::URDFReadFromFile(argv[1], model, true);
std::cout << "RBDL Benchmark" << std::endl;
std::cout << " model: " << argv[1] << std::endl;
std::cout << "RBDL Test" << std::endl;
std::cout << " model: " << model_file << std::endl;
std::cout << " nq: " << model->q_size << std::endl;
std::cout << " nv: " << model->qdot_size << std::endl;
RigidBodyDynamics::Math::VectorNd q =
RigidBodyDynamics::Math::VectorNd::Random(model->q_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->q_size);
RigidBodyDynamics::Math::VectorNd qdot =
RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
RigidBodyDynamics::Math::VectorNd tau =
RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
RigidBodyDynamics::Math::VectorNd qddot =
RigidBodyDynamics::Math::VectorNd::Random(model->qdot_size);
RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
q[0] = 1;
RigidBodyDynamics::ForwardDynamics(*model, q, qdot, tau, qddot);
std::cout << "qddot after ABA: " << qddot.transpose() << std::endl;
qddot = RigidBodyDynamics::Math::VectorNd::Zero(model->qdot_size);
RigidBodyDynamics::InverseDynamics(*model, q, qdot, qddot, tau);
std::cout << "tau aftern RNEA: " << tau.transpose() << std::endl;
delete model;
}
int main()
{
for (auto& model : pinocchio_benchmarks::models)
{
rbdl_test(model);
}
return 0;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment