GCC Code Coverage Report

Directory: ./
File: tests/test-solid-solid-collision.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 42 43 97.7%
Branches: 83 166 50.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <coal/fwd.hh>
31 #define BOOST_TEST_MODULE solid_solid_collision
32 #include <coal/math/transform.h>
33 #include <coal/shape/geometric_shapes.h>
34
35 #include <boost/test/included/unit_test.hpp>
36 #include <hpp/core/continuous-validation/solid-solid-collision.hh>
37 #include <hpp/pinocchio/device.hh>
38 #include <hpp/pinocchio/joint.hh>
39 #include <hpp/pinocchio/simple-device.hh>
40 #include <limits>
41 #include <pinocchio/fwd.hpp>
42 #include <pinocchio/multibody/geometry.hpp>
43 #include <pinocchio/multibody/model.hpp>
44
45 using hpp::core::continuousValidation::CoefficientVelocities_t;
46 using hpp::core::continuousValidation::SolidSolidCollision;
47 using hpp::core::continuousValidation::SolidSolidCollisionPtr_t;
48 using hpp::pinocchio::BodyPtr_t;
49 using hpp::pinocchio::CollisionObject;
50 using hpp::pinocchio::CollisionObjectPtr_t;
51 using hpp::pinocchio::Device;
52 using hpp::pinocchio::DevicePtr_t;
53 using hpp::pinocchio::JointConstPtr_t;
54 using hpp::pinocchio::JointPtr_t;
55 using hpp::pinocchio::ObjectVector_t;
56 using std::numeric_limits;
57
58 using namespace hpp::core;
59 using namespace hpp::pinocchio;
60
61 4 void display(const Model& model, const CoefficientVelocities_t& cvs) {
62 4 CoefficientVelocities_t::const_iterator it;
63
2/2
✓ Branch 4 taken 24 times.
✓ Branch 5 taken 4 times.
 28 for (it = cvs.begin(); it != cvs.end(); ++it) {
64 24 size_type jidx = it->joint_->index();
65
1/2
✓ Branch 0 taken 24 times.
✗ Branch 1 not taken.
 24 if (jidx > 0)
66
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 24 std::cout << model.names[jidx];
67 else
68 ✗ std::cout << "None";
69
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 std::cout << ", ";
70 }
71
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 std::cout << std::endl;
72 4 }
73
74 BOOST_AUTO_TEST_SUITE(test_hpp_core)
75
76 1 DevicePtr_t createRobot() {
77
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidSimple);
78 1 return robot;
79 }
80
81
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(solid_solid_collision_1) {
82
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 DevicePtr_t robot = createRobot();
83 2 const Model& model = robot->model();
84
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t joint_a = robot->getJointByBodyName("lleg5_body");
85
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t joint_b = robot->getJointByBodyName("rleg5_body");
86
87 SolidSolidCollisionPtr_t bpc =
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SolidSolidCollision::create(joint_a, joint_b, 0.001);
89
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 display(model, bpc->coefficients());
90
91 2 ConstObjectStdVector_t obstacles;
92
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 auto box = coal::make_shared<coal::Box>(.2, .4, .6);
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Transform3s I3;
94
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 I3.setIdentity();
95
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 4 pinocchio::FrameIndex frame_id = robot->model().addFrame(
96
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 4 ::pinocchio::Frame("base_link", 0, 0, I3, ::pinocchio::BODY));
97
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 4 GeomIndex idObj = robot->geomModel().addGeometryObject(
98
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
 8 ::pinocchio::GeometryObject("obstacle", frame_id, 0, box, I3, "",
99
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 vector3_t::Ones()),
100 2 robot->model());
101 CollisionObjectPtr_t collObj(
102
4/8
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 4 new CollisionObject(robot->geomModelPtr(), robot->geomDataPtr(), idObj));
103
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 obstacles.push_back(collObj);
104
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 bpc = SolidSolidCollision::create(joint_a, obstacles, 0.001);
105
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 display(model, bpc->coefficients());
106
107
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 joint_a = robot->getJointByBodyName("lleg5_body");
108
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 joint_b = robot->getJointByBodyName("lleg1_body");
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 bpc = SolidSolidCollision::create(joint_a, joint_b, 0.001);
110
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 display(model, bpc->coefficients());
111
112
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 joint_a = robot->getJointByBodyName("lleg1_body");
113
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 joint_b = robot->getJointByBodyName("lleg5_body");
114
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 bpc = SolidSolidCollision::create(joint_a, joint_b, 0.001);
115
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 display(model, bpc->coefficients());
116 2 }
117
118 BOOST_AUTO_TEST_SUITE_END()
119