Commit 6ac36861 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

tests compile exept body-pair-collision

TODO : build the correct robot model for each test, and not the one from hppPinocchio()
parent b8cc26b8
......@@ -121,9 +121,11 @@ ADD_REQUIRED_DEPENDENCY("hpp-util >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-statistics")
ADD_REQUIRED_DEPENDENCY("hpp-constraints >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-model >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-model-urdf >= 3")
ADD_SUBDIRECTORY(src)
#ADD_SUBDIRECTORY(tests)
#ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(tests)
# Add dependency toward hpp-core library in pkg-config file.
PKG_CONFIG_APPEND_LIBS("hpp-core")
......
......@@ -90,4 +90,5 @@ PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-statistics)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-constraints)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-model)
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
......@@ -27,8 +27,6 @@
# include <hpp/pinocchio/body.hh>
# include <hpp/pinocchio/collision-object.hh>
# include <hpp/pinocchio/joint.hh>
//# include <hpp/pinocchio/joint-configuration.hh>
# include <hpp/core/collision-validation-report.hh>
# include <hpp/core/straight-path.hh>
# include <hpp/core/projection-error.hh>
......
......@@ -15,12 +15,19 @@
# You should have received a copy of the GNU Lesser General Public License
# along with hpp-core If not, see <http://www.gnu.org/licenses/>.
SET(BOOST_COMPONENTS system timer unit_test_framework)
SEARCH_FOR_BOOST()
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
# Make Boost.Test generates the main function in test cases.
ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
# Specify the tests data files directory.
ADD_DEFINITIONS(-DTEST_DIRECTORY="${CMAKE_SOURCE_DIR}/tests/data")
# Make Boost.Test generates the main function in test cases.
#ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
......@@ -42,7 +49,8 @@ MACRO(ADD_TESTCASE NAME GENERATED)
PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-util)
PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-model)
PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-model-urdf)
PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-pinocchio)
# Link against Boost and project library.
TARGET_LINK_LIBRARIES(${NAME}
${Boost_LIBRARIES} hpp-core
......@@ -50,13 +58,13 @@ MACRO(ADD_TESTCASE NAME GENERATED)
ENDMACRO(ADD_TESTCASE)
ADD_TESTCASE (test-kdTree FALSE)
ADD_TESTCASE (roadmap-1 FALSE)
#ADD_TESTCASE (test-kdTree FALSE)
#ADD_TESTCASE (roadmap-1 FALSE)
ADD_TESTCASE (test-intervals FALSE)
ADD_TESTCASE (test-body-pair-collision FALSE)
ADD_TESTCASE (test-gradient-based FALSE)
ADD_TESTCASE (test-configprojector FALSE)
ADD_TESTCASE (path-projectors FALSE)
#ADD_TESTCASE (test-body-pair-collision FALSE)
#ADD_TESTCASE (test-gradient-based FALSE)
#ADD_TESTCASE (test-configprojector FALSE)
#ADD_TESTCASE (path-projectors FALSE) # link error
ADD_TESTCASE (containers FALSE)
ADD_TESTCASE (paths FALSE)
ADD_TESTCASE (relative-motion FALSE)
#ADD_TESTCASE (paths FALSE)
#ADD_TESTCASE (relative-motion FALSE)
......@@ -20,6 +20,7 @@
#include <boost/mpl/list.hpp>
#include <boost/shared_ptr.hpp>
// Boost version 1.54
// Cannot include boost CPU timers
// #include <boost/timer/timer.hpp>
......@@ -31,10 +32,11 @@
#define HPP_ENABLE_BENCHMARK 1
#include <hpp/util/timer.hh>
#include <hpp/model/device.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/object-factory.hh>
#include <hpp/constraints/differentiable-function.hh>
......@@ -46,6 +48,8 @@
#include <hpp/core/path-projector/global.hh>
#include <hpp/core/path-projector/progressive.hh>
#include "../tests/utils.hh"
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
......@@ -53,18 +57,16 @@ using hpp::pinocchio::JointPtr_t;
using namespace hpp::core;
hpp::pinocchio::ObjectFactory objectFactory;
/*
DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("test");
const std::string& name = robot->name ();
fcl::Transform3f mat; mat.setIdentity ();
Transform3f mat; mat.setIdentity ();
JointPtr_t joint;
std::string jointName = name + "_x";
// Translation along x
fcl::Matrix3f permutation;
joint = objectFactory.createJointTranslation2 (mat);
joint->name (jointName);
......@@ -77,7 +79,7 @@ DevicePtr_t createRobot ()
robot->rootJoint (joint);
return robot;
}
}*/
ConstraintSetPtr_t createConstraints (DevicePtr_t r)
{
......@@ -221,7 +223,7 @@ typedef boost::mpl::list <traits_global_circle,
BOOST_AUTO_TEST_CASE_TEMPLATE (projectors, traits, test_types)
{
DevicePtr_t dev = createRobot ();
DevicePtr_t dev = hppPinocchio();
BOOST_REQUIRE (dev);
Problem problem (dev);
......
......@@ -26,17 +26,20 @@
// because the original timers are already included by
// the unit test framework
// #include <boost/timer.hh>
#include <hpp/model/device.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/object-factory.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/path.hh>
#include <hpp/core/straight-path.hh>
#include <hpp/core/subchain-path.hh>
#include "../tests/utils.hh"
#define TOSTR( x ) static_cast< std::ostringstream & >( ( std::ostringstream() << x ) ).str()
using hpp::pinocchio::Device;
......@@ -45,9 +48,8 @@ using hpp::pinocchio::JointPtr_t;
using namespace hpp::core;
hpp::pinocchio::ObjectFactory objectFactory;
DevicePtr_t createRobot ()
/*DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("test");
......@@ -66,9 +68,9 @@ DevicePtr_t createRobot ()
robot->rootJoint (joint);
return robot;
}
}*/
DevicePtr_t createRobot2 ()
/*DevicePtr_t createRobot2 ()
{
DevicePtr_t robot = Device::create ("test");
......@@ -91,7 +93,7 @@ DevicePtr_t createRobot2 ()
}
return robot;
}
}*/
typedef std::pair<value_type, value_type> Pair_t;
......@@ -120,7 +122,7 @@ void checkAt(const PathPtr_t orig, value_type to,
BOOST_AUTO_TEST_CASE (extracted)
{
DevicePtr_t dev = createRobot ();
DevicePtr_t dev = hppPinocchio ();
BOOST_REQUIRE (dev);
Problem problem (dev);
......@@ -148,7 +150,7 @@ BOOST_AUTO_TEST_CASE (extracted)
BOOST_AUTO_TEST_CASE (subchain)
{
DevicePtr_t dev = createRobot2 (); // 10 translations
DevicePtr_t dev = hppPinocchio (); // 10 translations
BOOST_REQUIRE (dev);
Problem problem (dev);
......
......@@ -37,10 +37,11 @@
#include <boost/shared_ptr.hpp>
#include <hpp/model/device.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/object-factory.hh>
#include <hpp/constraints/generic-transformation.hh>
......@@ -49,6 +50,8 @@
#include <hpp/core/constraint-set.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/core/numerical-constraint.hh>
#include "../tests/utils.hh"
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
......@@ -58,8 +61,7 @@ using hpp::constraints::RelativeTransformation;
using namespace hpp::core;
hpp::pinocchio::ObjectFactory objectFactory;
/*
DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("test");
......@@ -107,7 +109,7 @@ DevicePtr_t createRobot ()
parent = joint;
}
return robot;
}
}*/
void lockJoint (ConfigProjectorPtr_t proj, DevicePtr_t dev, std::string name)
{
......@@ -130,7 +132,7 @@ struct Jidx {
BOOST_AUTO_TEST_CASE (relativeMotion)
{
DevicePtr_t dev = createRobot ();
DevicePtr_t dev = hppPinocchio();
BOOST_REQUIRE (dev);
Jidx jointid;
jointid.dev = dev;
......
......@@ -18,6 +18,8 @@
#include <boost/assign.hpp>
#include <hpp/model/device.hh>
#include <hpp/util/debug.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
......@@ -29,20 +31,21 @@
#include <hpp/core/connected-component.hh>
#include <hpp/core/node.hh>
#include <hpp/core/nearest-neighbor.hh>
#include <hpp/pinocchio/joint-configuration.hh>
#include <hpp/core/steering-method-straight.hh>
#include <hpp/core/weighed-distance.hh>
#define BOOST_TEST_MODULE roadmap-1
#include <boost/test/included/unit_test.hpp>
#include "../tests/utils.hh"
using hpp::pinocchio::Configuration_t;
using hpp::core::ConfigurationPtr_t;
using hpp::pinocchio::JointPtr_t;
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointTranslation;
using hpp::core::Problem;
using hpp::core::SteeringMethodStraight;
using hpp::core::SteeringMethodStraightPtr_t;
......@@ -64,7 +67,7 @@ void addEdge (const hpp::core::RoadmapPtr_t& r,
BOOST_AUTO_TEST_CASE (Roadmap1) {
// Build robot
DevicePtr_t robot = Device::create("robot");
/* DevicePtr_t robot = Device::create("robot");
JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f());
xJoint->isBounded(0,1);
xJoint->lowerBound(0,-3.);
......@@ -77,12 +80,13 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
robot->rootJoint (xJoint);
xJoint->addChildJoint (yJoint);
*/
DevicePtr_t robot = hppPinocchio();
// Create steering method
Problem p = Problem (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);
// create roadmap
hpp::core::DistancePtr_t distance (WeighedDistance::create
hpp::core::DistancePtr_t distance (WeighedDistance::createWithWeight
(robot, boost::assign::list_of (1)(1)));
RoadmapPtr_t r = Roadmap::create (distance, robot);
......@@ -288,8 +292,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
BOOST_AUTO_TEST_CASE (nearestNeighbor) {
// Build robot
DevicePtr_t robot = Device::create("robot");
JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f());
DevicePtr_t robot = hppPinocchio();
/*JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f());
xJoint->isBounded(0,1);
xJoint->lowerBound(0,-3.);
xJoint->upperBound(0,3.);
......@@ -300,13 +304,13 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
yJoint->upperBound(0,3.);
robot->rootJoint (xJoint);
xJoint->addChildJoint (yJoint);
xJoint->addChildJoint (yJoint);*/
// Create steering method
Problem p (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);
// create roadmap
hpp::core::DistancePtr_t distance (WeighedDistance::create
hpp::core::DistancePtr_t distance (WeighedDistance::createWithWeight
(robot, boost::assign::list_of (1)(1)));
RoadmapPtr_t r = Roadmap::create (distance, robot);
......
......@@ -22,12 +22,15 @@
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/pinocchio/object-factory.hh>
#include <hpp/model/device.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include "../src/continuous-collision-checking/dichotomy/body-pair-collision.hh"
#include <boost/test/included/unit_test.hpp>
#include "../tests/utils.hh"
using std::numeric_limits;
using hpp::pinocchio::BodyPtr_t;
......@@ -43,8 +46,7 @@ using hpp::core::continuousCollisionChecking::dichotomy::BodyPairCollisionPtr_t;
BOOST_AUTO_TEST_SUITE( test_hpp_core )
hpp::pinocchio::ObjectFactory objectFactory;
/*
JointPtr_t createFreeflyerJoint (DevicePtr_t robot)
{
const std::string& name = robot->name ();
......@@ -264,7 +266,7 @@ DevicePtr_t createRobot ()
joint->setLinkedBody (body);
return robot;
}
}*/
CollisionObjectPtr_t createObstacle ()
{
......@@ -276,7 +278,7 @@ CollisionObjectPtr_t createObstacle ()
BOOST_AUTO_TEST_CASE (body_pair_collision_1)
{
DevicePtr_t robot = createRobot ();
DevicePtr_t robot = hppPinocchio();
JointPtr_t joint_a = robot->getJointByBodyName ("LLEG_BODY5");
JointPtr_t joint_b = robot->getJointByBodyName ("RLEG_BODY5");
......
......@@ -18,14 +18,17 @@
#include <boost/test/included/unit_test.hpp>
#include <hpp/core/config-projector.hh>
#include <hpp/model/device.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/object-factory.hh>
#include <hpp/constraints/generic-transformation.hh>
#include "../tests/utils.hh"
using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
......@@ -38,8 +41,7 @@ using hpp::constraints::vector3_t;
using namespace hpp::core;
hpp::pinocchio::ObjectFactory objectFactory;
/*
JointPtr_t createFreeflyerJoint (DevicePtr_t robot)
{
const std::string& name = robot->name ();
......@@ -262,25 +264,26 @@ DevicePtr_t createRobot ()
joint->setLinkedBody (body);
return robot;
}
}*/
BOOST_AUTO_TEST_SUITE (config_projector)
BOOST_AUTO_TEST_CASE (ref_zero)
{
DevicePtr_t dev = createRobot ();
DevicePtr_t dev = hppPinocchio ();
JointPtr_t xyz = dev->getJointByName ("test_z");
matrix3_t rot; rot.setIdentity ();
vector3_t zero; zero.setZero();
ComparisonType::VectorOfTypes types (3, ComparisonType::Superior);
BOOST_REQUIRE (dev);
PositionPtr_t position =
Position::create ("Position", dev, xyz, zero);
Position::create ("Position", dev, xyz, Transform3f(rot,zero));
types[1] = ComparisonType::Inferior;
ComparisonTypesPtr_t ineq = ComparisonTypes::create (types);
ConfigProjectorPtr_t projector =
ConfigProjector::create (dev, "test", 1e-4, 20);
projector->add (NumericalConstraint::create (position, ineq));
Configuration_t cfg(dev->configSize ());
cfg.setZero ();
......@@ -303,7 +306,7 @@ BOOST_AUTO_TEST_CASE (ref_zero)
BOOST_AUTO_TEST_CASE (ref_not_zero)
{
DevicePtr_t dev = createRobot ();
DevicePtr_t dev = hppPinocchio ();
JointPtr_t xyz = dev->getJointByName ("test_z");
matrix3_t rot; rot.setIdentity ();
vector3_t zero; zero.setZero();
......@@ -311,7 +314,7 @@ BOOST_AUTO_TEST_CASE (ref_not_zero)
ComparisonType::VectorOfTypes types (3, ComparisonType::Superior);
BOOST_REQUIRE (dev);
PositionPtr_t position =
Position::create ("Position", dev, xyz, zero, vector3_t (1,1,1));
Position::create ("Position", dev, xyz, Transform3f(rot,zero), Transform3f(rot,vector3_t (1,1,1)));
ref[0] = 0; ref[1] = 0; ref[2] = 0;
types[1] = ComparisonType::Inferior;
......
......@@ -25,16 +25,20 @@
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/model/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/collision-object.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/object-factory.hh>
#include <hpp/core/steering-method-straight.hh>
#include <hpp/core/path-optimization/gradient-based.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include "../tests/utils.hh"
using hpp::pinocchio::BodyPtr_t;
using hpp::pinocchio::Body;
using hpp::pinocchio::CollisionObject;
......@@ -44,8 +48,6 @@ using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::Transform3f;
using hpp::pinocchio::JointPtr_t;
using hpp::pinocchio::JointTranslation;
using hpp::pinocchio::ObjectFactory;
using hpp::pinocchio::value_type;
using fcl::Quaternion3f;
using fcl::Box;
......@@ -60,7 +62,7 @@ using hpp::core::pathOptimization::GradientBased;
BOOST_AUTO_TEST_SUITE( test_hpp_core )
DevicePtr_t createRobot ()
/*DevicePtr_t createRobot ()
{
DevicePtr_t robot = Device::create ("planar-robot");
Transform3f position; position.setIdentity ();
......@@ -81,11 +83,11 @@ DevicePtr_t createRobot ()
body->addInnerObject (object, true, true);
return robot;
}
}*/
BOOST_AUTO_TEST_CASE (BFGS)
{
DevicePtr_t robot = createRobot ();
DevicePtr_t robot = hppPinocchio ();
Configuration_t q0 (robot->configSize ());
Configuration_t q1 (robot->configSize ());
Configuration_t q2 (robot->configSize ());
......
......@@ -24,6 +24,9 @@
//#include <Eigen/Core>
#include <hpp/util/debug.hh>
#include <hpp/model/device.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/configuration.hh>
......@@ -34,7 +37,6 @@
#include <hpp/core/connected-component.hh>
#include <hpp/core/node.hh>
#include <hpp/core/steering-method-straight.hh>
#include <hpp/pinocchio/joint-configuration.hh>
#include "../src/nearest-neighbor/basic.hh"
#include "../src/nearest-neighbor/k-d-tree.hh"
......@@ -42,6 +44,9 @@
#define BOOST_TEST_MODULE kdTree
#include <boost/test/included/unit_test.hpp>
#include "../tests/utils.hh"
using namespace hpp;
using namespace core;
using namespace pinocchio;
......@@ -51,7 +56,7 @@ BOOST_AUTO_TEST_SUITE( test_hpp_core )
BOOST_AUTO_TEST_CASE (kdTree) {
// Build Device
DevicePtr_t robot = Device::create("robot");
/* DevicePtr_t robot = Device::create("robot");
JointPtr_t transJoint = new JointTranslation <3> (Transform3f());
transJoint->isBounded (0, true);
transJoint->lowerBound(0,-3.);
......@@ -68,6 +73,8 @@ BOOST_AUTO_TEST_CASE (kdTree) {
robot->rootJoint(transJoint);
transJoint->addChildJoint (so3Joint);
so3Joint->addChildJoint (so2Joint);
*/
DevicePtr_t robot = hppPinocchio();
// Build Distance, nearestNeighbor, KDTree
Problem problem (robot);
......
#include <hpp/pinocchio/hpp-model/conversions.hh>
#include <hpp/pinocchio/hpp-model/model-loader.hh>
template<typename D>
bool isPermutation(const Eigen::MatrixBase<D> & R)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
for( int row=0;row<3;row++ )
for( int col=0;col<3;col++ )
if ( (std::abs(R(row,col))>1e-6) && (std::abs(std::abs(R(row,col))-1)>1e-6) )
return false;
return true;
}
/* Frames in hpp::model are "permuted" (i.e. combination of PI/2 Cartesian rotation)
* so check here that the two placements fits, under this permutation.
*/
bool isApproxPermutation( hpp::model::Transform3f Mm,
hpp::pinocchio::Transform3f Mp )
{
return Mm.getTranslation().isApprox( Mp.translation() )
&& isPermutation(Eigen::Matrix3d(Mp.rotation().transpose()*Mm.getRotation())) ;
}
BOOST_AUTO_TEST_CASE(convert)
{
Eigen::VectorXd q = Eigen::VectorXd::Random(20);
BOOST_CHECK( q.isApprox(m2p::q(p2m::q(q))) );
Eigen::VectorXd v = Eigen::VectorXd::Random(20);
Eigen::MatrixXd J = Eigen::MatrixXd::Random(3,20);
Eigen::MatrixXd R(3,3);
R << 0,0,1 ,1,0,0, 0,1,0;
BOOST_CHECK( v.isApprox(p2m::Xq(R)*(m2p::Xq(R)*v)) );
BOOST_CHECK( J.isApprox((J*p2m::Xq(R))*m2p::Xq(R)) );
BOOST_CHECK( ((J*m2p::Xq(R))*(p2m::Xq(R)*v)).isApprox(J*v) );
se3::SE3 Mp = se3::SE3::Random();
BOOST_CHECK( Mp.isApprox( m2p::SE3(p2m::SE3(Mp)) ) );
}
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