Verified Commit 6429ea1b authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test: add test for ABA, RNEA and Collisions in //

parent 097ae1f3
#
# Copyright (c) 2015-2020 CNRS INRIA
# Copyright (c) 2015-2021 CNRS INRIA
# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
#
......@@ -45,6 +45,15 @@ MACRO(ADD_OPTIONAL_COMPILE_DEPENDENCY PKG_CONFIG_STRING)
ADD_DEPENDENCY(0 1 ${PKG_CONFIG_STRING} "${PKG_CONFIG_DEBUG_STRING}")
ENDMACRO()
MACRO(ADD_PINOCCHIO_PARALLEL_UNIT_TEST NAME)
IF(NOT BUILD_WITH_OPENMP_SUPPORT)
RETURN()
ENDIF()
ADD_PINOCCHIO_UNIT_TEST(${ARGV})
SET(TEST_NAME "test-cpp-${NAME}")
TARGET_LINK_LIBRARIES(${TEST_NAME} PRIVATE OpenMP::OpenMP)
ENDMACRO()
# Find Boost.UnitTestFramework
FIND_PACKAGE(Boost COMPONENTS unit_test_framework)
......@@ -64,7 +73,9 @@ ADD_PINOCCHIO_UNIT_TEST(vector)
ADD_PINOCCHIO_UNIT_TEST(spatial)
ADD_PINOCCHIO_UNIT_TEST(symmetric)
ADD_PINOCCHIO_UNIT_TEST(aba)
ADD_PINOCCHIO_PARALLEL_UNIT_TEST(parallel-aba)
ADD_PINOCCHIO_UNIT_TEST(rnea)
ADD_PINOCCHIO_PARALLEL_UNIT_TEST(parallel-rnea)
ADD_PINOCCHIO_UNIT_TEST(crba)
ADD_PINOCCHIO_UNIT_TEST(centroidal)
ADD_PINOCCHIO_UNIT_TEST(com)
......@@ -80,6 +91,7 @@ IF(urdfdom_FOUND)
ADD_PINOCCHIO_UNIT_TEST(value)
IF(hpp-fcl_FOUND)
ADD_PINOCCHIO_UNIT_TEST(geom)
ADD_PINOCCHIO_PARALLEL_UNIT_TEST(parallel-geometry)
ADD_PINOCCHIO_UNIT_TEST(srdf)
ENDIF(hpp-fcl_FOUND)
ENDIF(urdfdom_FOUND)
......
//
// Copyright (c) 2021 INRIA
//
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/algorithm/parallel/aba.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"
#include <iostream>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
using namespace pinocchio;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(test_parallel_aba)
{
pinocchio::Model model; buildModels::humanoidRandom(model);
Data data_ref(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill( 1.);
const Eigen::DenseIndex batch_size = 128;
const int num_threads = omp_get_max_threads();
Eigen::MatrixXd q(model.nq,batch_size);
Eigen::MatrixXd v(model.nv,batch_size);
Eigen::MatrixXd tau(model.nv,batch_size);
Eigen::MatrixXd a(model.nv,batch_size);
Eigen::MatrixXd a_ref(model.nv,batch_size);
for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
{
q.col(i) = randomConfiguration(model);
v.col(i) = Eigen::VectorXd::Random(model.nv);
tau.col(i) = Eigen::VectorXd::Random(model.nv);
}
ModelPool pool(model);
aba(num_threads,pool,q,v,tau,a);
for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
{
a_ref.col(i) = aba(model,data_ref,q.col(i),v.col(i),tau.col(i));
}
BOOST_CHECK(a == a_ref);
}
BOOST_AUTO_TEST_SUITE_END()
//
// Copyright (c) 2021 INRIA
//
#include <iostream>
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/algorithm/parallel/geometry.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
#include <vector>
#include <boost/test/unit_test.hpp>
using namespace pinocchio;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(test_talos)
{
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");
pinocchio::Model model;
pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
Data data(model), data_ref(model);
const std::string package_path = PINOCCHIO_MODEL_DIR;
hpp::fcl::MeshLoaderPtr mesh_loader = boost::make_shared<hpp::fcl::CachedMeshLoader>();
const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
std::vector<std::string> package_paths(1,package_path);
pinocchio::GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,COLLISION,geometry_model,package_paths,mesh_loader);
geometry_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model,geometry_model,srdf_filename,false);
GeometryData geometry_data(geometry_model), geometry_data_ref(geometry_model);
const Eigen::VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
Eigen::VectorXd q = randomConfiguration(model,-qmax,qmax);
const bool res_ref = computeCollisions(model,data_ref,geometry_model,geometry_data_ref,q);
const bool res = computeCollisions(model,data,geometry_model,geometry_data,q);
BOOST_CHECK(res_ref == res);
BOOST_CHECK(geometry_data_ref.collisionPairIndex == geometry_data.collisionPairIndex);
for(size_t k = 0; k < geometry_model.collisionPairs.size(); ++k)
{
const CollisionPair & cp = geometry_model.collisionPairs[k];
BOOST_CHECK(geometry_data_ref.oMg[cp.first] == geometry_data.oMg[cp.first]);
BOOST_CHECK(geometry_data_ref.oMg[cp.second] == geometry_data.oMg[cp.second]);
BOOST_CHECK( geometry_data_ref.collisionResults[k].getContacts()
== geometry_data.collisionResults[k].getContacts());
}
}
BOOST_AUTO_TEST_CASE(test_pool_talos)
{
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");
pinocchio::Model model;
pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
Data data_ref(model);
const std::string package_path = PINOCCHIO_MODEL_DIR;
hpp::fcl::MeshLoaderPtr mesh_loader = boost::make_shared<hpp::fcl::CachedMeshLoader>();
const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
std::vector<std::string> package_paths(1,package_path);
pinocchio::GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,COLLISION,geometry_model,package_paths,mesh_loader);
geometry_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model,geometry_model,srdf_filename,false);
GeometryData geometry_data_ref(geometry_model);
const Eigen::VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
const Eigen::DenseIndex batch_size = 128;
const int num_thread = omp_get_max_threads();
Eigen::MatrixXd q(model.nq,batch_size);
for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
{
q.col(i) = randomConfiguration(model,-qmax,qmax);
}
typedef Eigen::Matrix<bool,Eigen::Dynamic,1> VectorXb;
VectorXb res(batch_size); res.fill(false);
VectorXb res_ref(batch_size); res_ref.fill(false);
for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
{
res_ref[i] = computeCollisions(model,data_ref,geometry_model,geometry_data_ref,q.col(i));
}
GeometryPool pool(model,geometry_model,num_thread);
computeCollisions(num_thread,pool,q,res);
BOOST_CHECK(res == res_ref);
}
BOOST_AUTO_TEST_SUITE_END()
//
// Copyright (c) 2021 INRIA
//
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/algorithm/parallel/rnea.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"
#include <iostream>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
using namespace pinocchio;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(test_parallel_rnea)
{
pinocchio::Model model; buildModels::humanoidRandom(model);
Data data_ref(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill( 1.);
const Eigen::DenseIndex batch_size = 128;
const int num_thread = omp_get_max_threads();
Eigen::MatrixXd q(model.nq,batch_size);
Eigen::MatrixXd v(model.nv,batch_size);
Eigen::MatrixXd a(model.nv,batch_size);
Eigen::MatrixXd tau(model.nv,batch_size);
Eigen::MatrixXd tau_ref(model.nv,batch_size);
for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
{
q.col(i) = randomConfiguration(model);
v.col(i) = Eigen::VectorXd::Random(model.nv);
a.col(i) = Eigen::VectorXd::Random(model.nv);
}
ModelPool pool(model,num_thread);
rnea(num_thread,pool,q,v,a,tau);
for(Eigen::DenseIndex i = 0; i < batch_size; ++i)
{
tau_ref.col(i) = rnea(model,data_ref,q.col(i),v.col(i),a.col(i));
}
BOOST_CHECK(tau == tau_ref);
}
BOOST_AUTO_TEST_SUITE_END()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment