GCC Code Coverage Report

Directory: ./
File: src/explicit/convex-shape-contact.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 64 104 61.5%
Branches: 72 262 27.5%
Line Branch Exec Source
1 // Copyright (c) 2020, Airbus SAS and CNRS
2 // Authors: Florent Lamiraux
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/constraints/affine-function.hh>
30 #include <hpp/constraints/convex-shape-contact.hh>
31 #include <hpp/constraints/explicit/convex-shape-contact.hh>
32 #include <hpp/constraints/explicit/relative-pose.hh>
33 #include <hpp/pinocchio/liegroup-space.hh>
34
35 #include "../src/explicit/input-configurations.hh"
36
37 namespace hpp {
38 namespace constraints {
39 namespace explicit_ {
40 // Check that object contact surface
41 // - belong to the same object (joint),
42 // - are on a freeflyer joint.
43 // Throw in case these assertions are not true
44 1 static void checkContactSurfaces(const JointAndShapes_t& objectSurfaces) {
45 1 JointPtr_t joint(0x0);
46
3/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2 times.
✓ Branch 9 taken 1 times.
 3 for (auto js : objectSurfaces) {
47
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
 2 if (!joint) {
48 1 joint = js.first;
49 // Check that joint is a freeflyer
50
6/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 2 if ((*joint->configurationSpace() != *pinocchio::LiegroupSpace::SE3()) &&
51
1/10
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 1 (*joint->configurationSpace() !=
52
2/8
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 1 *pinocchio::LiegroupSpace::R3xSO3())) {
53 ✗ std::ostringstream os;
54 os << "You are trying to build an explicit contact constraint but"
55 " the joint that holds at least on object contact surface is "
56 " not a freeflyer joint: "
57 ✗ << joint->name();
58 ✗ throw std::logic_error(os.str().c_str());
59 if (joint->parentJoint()) {
60 os << "You are trying to build an explicit contact constraint "
61 "but the joint that holds at least on object contact "
62 "surface: "
63 << joint->name()
64 << " is attached to another joint. This is not supported";
65 throw std::logic_error(os.str().c_str());
66 }
67 }
68
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 } else if (*(js.first) != *joint) {
69 ✗ std::ostringstream os;
70 os << "You are trying to build an explicit contact constraint "
71 "but several joints hold object contact surface: "
72 ✗ << joint->name() << " and " << js.first->name();
73 ✗ throw std::logic_error(os.str().c_str());
74 }
75 2 }
76 1 }
77
78 1 ConvexShapeContactPtr_t ConvexShapeContact::create(
79 const std::string& name, DevicePtr_t robot,
80 const JointAndShapes_t& floorSurfaces,
81 const JointAndShapes_t& objectSurfaces, const value_type& margin) {
82 1 checkContactSurfaces(objectSurfaces);
83 ConvexShapeContact* ptr(new ConvexShapeContact(name, robot, floorSurfaces,
84
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 objectSurfaces, margin));
85 1 ConvexShapeContactPtr_t shPtr(ptr);
86
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ptr->init(shPtr);
87 1 return shPtr;
88 }
89
90 ConvexShapeContact::Constraints_t
91 ✗ ConvexShapeContact::createConstraintAndComplement(
92 const std::string& name, DevicePtr_t robot,
93 const JointAndShapes_t& floorSurfaces,
94 const JointAndShapes_t& objectSurfaces, const value_type& margin) {
95 ✗ Constraints_t result;
96 std::pair<hpp::constraints::ConvexShapeContactPtr_t,
97 hpp::constraints::ConvexShapeContactComplementPtr_t>
98 functions(ConvexShapeContactComplement::createPair(
99 ✗ name, robot, floorSurfaces, objectSurfaces));
100 ✗ functions.first->setNormalMargin(margin);
101 // Contact constraint (= 0)
102 ✗ std::get<0>(result) = Implicit::create(functions.first, 5 * EqualToZero);
103 // Contact constraint complement (= rhs)
104 ✗ std::get<1>(result) = Implicit::create(
105 functions.second,
106 ✗ ComparisonTypes_t(functions.second->outputSize(), constraints::Equality));
107 ✗ std::get<2>(result) =
108 ✗ create(name + "/hold", robot, floorSurfaces, objectSurfaces, margin);
109 ✗ return result;
110 }
111
112 ✗ ConvexShapeContactPtr_t ConvexShapeContact::createCopy(
113 const ConvexShapeContactPtr_t& other) {
114 ✗ ConvexShapeContact* ptr(new ConvexShapeContact(*other));
115 ✗ ConvexShapeContactPtr_t shPtr(ptr);
116 ✗ ConvexShapeContactWkPtr_t wkPtr(shPtr);
117 ✗ ptr->init(wkPtr);
118 ✗ return shPtr;
119 }
120
121 ✗ ImplicitPtr_t ConvexShapeContact::copy() const {
122 ✗ return createCopy(weak_.lock());
123 }
124
125 400 void ConvexShapeContact::outputValue(LiegroupElementRef result, vectorIn_t qin,
126 LiegroupElementConstRef rhs) const {
127
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 400 times.
 400 assert(HPP_DYNAMIC_PTR_CAST(ConvexShapeContactHold, functionPtr()));
128 ConvexShapeContactHoldPtr_t f(
129 400 HPP_STATIC_PTR_CAST(ConvexShapeContactHold, functionPtr()));
130 std::size_t ifloor, iobject;
131
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 LiegroupElement relativePoseRhs(LiegroupSpace::R3xSO3());
132
3/6
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 400 times.
✗ Branch 11 not taken.
 400 f->complement()->computeRelativePoseRightHandSide(rhs, ifloor, iobject,
133 relativePoseRhs);
134 // Extract input configuration of relative pose from qin
135
1/2
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
 400 Eigen::RowBlockIndices inputIndices(inputConf());
136
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 vector_t q(f->inputSize());
137
1/2
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
 400 q.fill(sqrt(-1));
138
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 inputIndices.lview(q) = qin;
139 400 RelativePosePtr_t relativePose(pose_[ifloor * nObjSurf_ + iobject]);
140
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 Eigen::RowBlockIndices relPosInputIndices(relativePose->inputConf());
141
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 vector_t qinRelPose = relPosInputIndices.rview(q);
142
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 400 times.
 400 assert(!qinRelPose.hasNaN());
143
4/8
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 400 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 400 times.
✗ Branch 12 not taken.
 400 relativePose->outputValue(result, qinRelPose, relativePoseRhs);
144 400 }
145
146 ✗ void ConvexShapeContact::jacobianOutputValue(vectorIn_t qin,
147 LiegroupElementConstRef,
148 LiegroupElementConstRef rhs,
149 matrixOut_t jacobian) const {
150 ✗ assert(HPP_DYNAMIC_PTR_CAST(ConvexShapeContactHold, functionPtr()));
151 ConvexShapeContactHoldPtr_t f(
152 ✗ HPP_STATIC_PTR_CAST(ConvexShapeContactHold, functionPtr()));
153 std::size_t ifloor, iobject;
154 ✗ LiegroupElement relativePoseRhs(LiegroupSpace::R3xSO3());
155 ✗ f->complement()->computeRelativePoseRightHandSide(rhs, ifloor, iobject,
156 relativePoseRhs);
157 // Extract input configuration of relative pose from qin
158 ✗ Eigen::RowBlockIndices inputIndices(inputConf());
159 ✗ vector_t q(f->inputSize());
160 ✗ q.fill(sqrt(-1));
161 ✗ inputIndices.lview(q) = qin;
162 ✗ RelativePosePtr_t relativePose(pose_[ifloor * nObjSurf_ + iobject]);
163 ✗ Eigen::RowBlockIndices relPosInputIndices(relativePose->inputConf());
164 ✗ vector_t qinRelPose = relPosInputIndices.rview(q);
165 ✗ assert(!qinRelPose.hasNaN());
166 ✗ LiegroupElement outputRelPose(LiegroupSpace::R3xSO3());
167 ✗ relativePose->outputValue(outputRelPose, qinRelPose, relativePoseRhs);
168 ✗ relativePose->jacobianOutputValue(qinRelPose, outputRelPose, relativePoseRhs,
169 jacobian);
170 }
171
172 1 ConvexShapeContact::ConvexShapeContact(const std::string& name,
173 DevicePtr_t robot,
174 const JointAndShapes_t& floorSurfaces,
175 const JointAndShapes_t& objectSurfaces,
176 1 const value_type& margin)
177 : Explicit(
178
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ConvexShapeContactHold::create(name, robot, floorSurfaces,
179 objectSurfaces),
180
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ConstantFunction::create(
181
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 pinocchio::LiegroupSpace::SE3()->neutral(),
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 contact::inputSize(robot, floorSurfaces, objectSurfaces),
183
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 contact::inputDerivSize(robot, floorSurfaces, objectSurfaces),
184 name),
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact::inputConfVariables(robot, floorSurfaces, objectSurfaces),
186
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 relativePose::jointConfInterval(objectSurfaces.front().first),
187
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact::inputVelocityVariables(robot, floorSurfaces, objectSurfaces),
188
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 relativePose::jointVelInterval(objectSurfaces.front().first),
189
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 5 (5 * EqualToZero << 3 * Equality), std::vector<bool>(8, true)) {
190
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 assert(HPP_DYNAMIC_PTR_CAST(ConvexShapeContactHold, functionPtr()));
191 ConvexShapeContactHoldPtr_t f(
192 1 HPP_STATIC_PTR_CAST(ConvexShapeContactHold, functionPtr()));
193
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 f->contactConstraint()->setNormalMargin(margin);
194 1 const ConvexShapes_t& fs(f->contactConstraint()->floorContactSurfaces());
195 1 const ConvexShapes_t& os(f->contactConstraint()->objectContactSurfaces());
196 1 nObjSurf_ = os.size();
197 // Compute explicit relative poses
198
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
 3 for (std::size_t ifloor = 0; ifloor < fs.size(); ++ifloor) {
199 // move floor surface along x to take into account margin.
200
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 Transform3s posInJoint(fs[ifloor].positionInJoint());
201 hppDout(info, "posInJoint" << posInJoint);
202
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 posInJoint.translation() -= margin * posInJoint.rotation().col(0);
203 hppDout(info, "posInJoint" << posInJoint);
204
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
 6 for (std::size_t iobject = 0; iobject < os.size(); ++iobject) {
205 // Create explicit relative pose for each combination
206 // (floor surface, object surface)
207
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 pose_.push_back(
208
2/4
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 RelativePose::create("", robot, fs[ifloor].joint_, os[iobject].joint_,
209 4 posInJoint, os[iobject].positionInJoint(),
210
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 EqualToZero << 3 * Equality << 2 * EqualToZero,
211
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 std::vector<bool>(6, true)));
212 }
213 }
214 1 }
215 1 void ConvexShapeContact::init(ConvexShapeContactWkPtr_t weak) {
216
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 Explicit::init(weak);
217 1 weak_ = weak;
218 1 }
219 } // namespace explicit_
220 } // namespace constraints
221 } // namespace hpp
222