| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2022 INRIA |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
#include <iostream> |
| 6 |
|
|
#include "pinocchio/algorithm/constraints/coulomb-friction-cone.hpp" |
| 7 |
|
|
|
| 8 |
|
|
#include <boost/test/unit_test.hpp> |
| 9 |
|
|
#include <boost/utility/binary.hpp> |
| 10 |
|
|
|
| 11 |
|
|
using namespace pinocchio; |
| 12 |
|
|
|
| 13 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
| 14 |
|
|
|
| 15 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_proj) |
| 16 |
|
|
{ |
| 17 |
|
✗ |
const int num_tests = int(1e5); |
| 18 |
|
✗ |
const double mu = .4; |
| 19 |
|
|
|
| 20 |
|
✗ |
const CoulombFrictionCone cone(mu); |
| 21 |
|
✗ |
const DualCoulombFrictionCone dual_cone = cone.dual(); |
| 22 |
|
|
|
| 23 |
|
✗ |
BOOST_CHECK(cone.isInside(Eigen::Vector3d::Zero())); |
| 24 |
|
✗ |
BOOST_CHECK(cone.project(Eigen::Vector3d::Zero()) == Eigen::Vector3d::Zero()); |
| 25 |
|
|
|
| 26 |
|
✗ |
BOOST_CHECK(dual_cone.isInside(Eigen::Vector3d::Zero())); |
| 27 |
|
✗ |
BOOST_CHECK(dual_cone.project(Eigen::Vector3d::Zero()) == Eigen::Vector3d::Zero()); |
| 28 |
|
|
|
| 29 |
|
✗ |
for (int k = 0; k < num_tests; ++k) |
| 30 |
|
|
{ |
| 31 |
|
✗ |
const Eigen::Vector3d x = Eigen::Vector3d::Random(); |
| 32 |
|
|
|
| 33 |
|
|
// Cone |
| 34 |
|
✗ |
const Eigen::Vector3d proj_x = cone.project(x); |
| 35 |
|
✗ |
const Eigen::Vector3d proj_proj_x = cone.project(proj_x); |
| 36 |
|
|
|
| 37 |
|
✗ |
BOOST_CHECK(cone.isInside(proj_x, 1e-12)); |
| 38 |
|
✗ |
BOOST_CHECK(cone.isInside(proj_proj_x, 1e-12)); |
| 39 |
|
✗ |
BOOST_CHECK(proj_x.isApprox(proj_proj_x)); |
| 40 |
|
✗ |
if (cone.isInside(x)) |
| 41 |
|
✗ |
BOOST_CHECK(x == proj_x); |
| 42 |
|
|
|
| 43 |
|
✗ |
BOOST_CHECK(fabs((x - proj_x).dot(proj_x)) <= 1e-12); // orthogonal projection |
| 44 |
|
|
|
| 45 |
|
|
// Dual cone |
| 46 |
|
✗ |
const Eigen::Vector3d dual_proj_x = dual_cone.project(x); |
| 47 |
|
✗ |
const Eigen::Vector3d dual_proj_proj_x = dual_cone.project(dual_proj_x); |
| 48 |
|
|
|
| 49 |
|
✗ |
BOOST_CHECK(dual_cone.isInside(dual_proj_x, 1e-12)); |
| 50 |
|
✗ |
BOOST_CHECK(dual_cone.isInside(dual_proj_proj_x, 1e-12)); |
| 51 |
|
✗ |
BOOST_CHECK(dual_proj_x.isApprox(dual_proj_proj_x)); |
| 52 |
|
|
|
| 53 |
|
✗ |
if (dual_cone.isInside(x)) |
| 54 |
|
✗ |
BOOST_CHECK(x == dual_proj_x); |
| 55 |
|
|
|
| 56 |
|
✗ |
BOOST_CHECK(fabs((x - dual_proj_x).dot(dual_proj_x)) <= 1e-12); // orthogonal projection |
| 57 |
|
|
|
| 58 |
|
|
// Radial projection |
| 59 |
|
|
{ |
| 60 |
|
✗ |
const Eigen::Vector3d radial_proj_x = cone.computeRadialProjection(x); |
| 61 |
|
✗ |
const Eigen::Vector3d radial_proj_radial_proj_x = cone.computeRadialProjection(radial_proj_x); |
| 62 |
|
✗ |
BOOST_CHECK(cone.isInside(radial_proj_x, 1e-12)); |
| 63 |
|
✗ |
BOOST_CHECK(radial_proj_x[2] == x[2] || radial_proj_x[2] == 0.); |
| 64 |
|
✗ |
if (radial_proj_x[2] == x[2]) |
| 65 |
|
✗ |
BOOST_CHECK( |
| 66 |
|
|
std::fabs(radial_proj_x.head<2>().normalized().dot(x.head<2>().normalized()) - 1.) |
| 67 |
|
|
<= 1e-6); |
| 68 |
|
|
else |
| 69 |
|
✗ |
BOOST_CHECK(radial_proj_x.head<2>().isZero()); |
| 70 |
|
✗ |
BOOST_CHECK(radial_proj_radial_proj_x.isApprox(radial_proj_radial_proj_x)); |
| 71 |
|
|
} |
| 72 |
|
|
} |
| 73 |
|
|
} |
| 74 |
|
|
|
| 75 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
| 76 |
|
|
|