GCC Code Coverage Report

Directory: ./
File: src/convex-shape-contact.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 221 296 74.7%
Branches: 234 581 40.3%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
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/convex-shape-contact.hh"
30
31 #include <../src/generic-transformation/helper.hh>
32 #include <hpp/constraints/matrix-view.hh>
33 #include <hpp/pinocchio/device.hh>
34 #include <hpp/pinocchio/joint.hh>
35 #include <hpp/pinocchio/liegroup-element.hh>
36 #include <limits>
37
38 namespace hpp {
39 namespace constraints {
40
41 7 ConvexShapeContact::ConvexShapeContact(const std::string& name,
42 DevicePtr_t robot,
43 const JointAndShapes_t& floorSurfaces,
44 7 const JointAndShapes_t& objectSurfaces)
45 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
46
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 LiegroupSpace::Rn(5), name),
47 7 robot_(robot),
48
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 14 relativeTransformationModel_(robot->numberDof() -
49 7 robot->extraConfigSpace().dimension()),
50 7 normalMargin_(0),
51
3/6
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 28 M_(0) {
52 7 relativeTransformationModel_.fullPos = true;
53 7 relativeTransformationModel_.fullOri = true;
54 7 relativeTransformationModel_.rowOri = 3;
55
56
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 activeParameters_.setConstant(false);
57
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 activeDerivativeParameters_.setConstant(false);
58 // Register convex polygons into constraint
59 7 for (JointAndShapes_t::const_iterator it(floorSurfaces.begin());
60
2/2
✓ Branch 2 taken 14 times.
✓ Branch 3 taken 7 times.
 21 it != floorSurfaces.end(); ++it) {
61
2/4
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 14 addFloor(ConvexShape(it->second, it->first));
62 }
63 7 for (JointAndShapes_t::const_iterator it(objectSurfaces.begin());
64
2/2
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 7 times.
 15 it != objectSurfaces.end(); ++it) {
65
2/4
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 addObject(ConvexShape(it->second, it->first));
66 }
67
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 computeRadius();
68 7 }
69
70 ✗ bool ConvexShapeContact::isEqual(const DifferentiableFunction& other) const {
71 const ConvexShapeContact& castother =
72 ✗ dynamic_cast<const ConvexShapeContact&>(other);
73 ✗ if (!DifferentiableFunction::isEqual(other)) return false;
74 ✗ if (robot_ != castother.robot_) return false;
75 ✗ if (objectConvexShapes_.size() != castother.objectConvexShapes_.size())
76 ✗ return false;
77 ✗ for (std::size_t i = 0; i < objectConvexShapes_.size(); i++)
78 ✗ if (floorConvexShapes_[i] != castother.floorConvexShapes_[i]) return false;
79 ✗ return true;
80 }
81
82 7 ConvexShapeContactPtr_t ConvexShapeContact::create(
83 const std::string& name, DevicePtr_t robot,
84 const JointAndShapes_t& floorSurfaces,
85 const JointAndShapes_t& objectSurfaces) {
86 return ConvexShapeContactPtr_t(
87
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 new ConvexShapeContact(name, robot, floorSurfaces, objectSurfaces));
88 }
89
90 8 void ConvexShapeContact::addObject(const ConvexShape& t) {
91 8 objectConvexShapes_.push_back(t);
92
93 8 for (ConvexShapes_t::const_iterator f_it = floorConvexShapes_.begin();
94
2/2
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 8 times.
 24 f_it != floorConvexShapes_.end(); ++f_it) {
95
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 setActiveParameters(robot_, f_it->joint_, t.joint_, activeParameters_,
96 16 activeDerivativeParameters_);
97 }
98 8 }
99
100 14 void ConvexShapeContact::addFloor(const ConvexShape& t) {
101
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 ConvexShape tt(t);
102
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 tt.reverse();
103
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 floorConvexShapes_.push_back(tt);
104
105 14 for (ConvexShapes_t::const_iterator o_it = objectConvexShapes_.begin();
106
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 14 o_it != objectConvexShapes_.end(); ++o_it) {
107 ✗ setActiveParameters(robot_, tt.joint_, o_it->joint_, activeParameters_,
108 ✗ activeDerivativeParameters_);
109 }
110 14 }
111
112 7 void ConvexShapeContact::computeRadius() {
113 // Compute upper bound of distance between center of polygon and
114 // vectices for all floor polygons.
115 7 for (ConvexShapes_t::const_iterator shape(floorConvexShapes_.begin());
116
2/2
✓ Branch 3 taken 14 times.
✓ Branch 4 taken 7 times.
 21 shape != floorConvexShapes_.end(); ++shape) {
117 14 for (std::vector<vector3_t>::const_iterator itv(shape->Pts_.begin());
118
2/2
✓ Branch 4 taken 62 times.
✓ Branch 5 taken 14 times.
 76 itv != shape->Pts_.end(); ++itv) {
119
2/4
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 62 times.
✗ Branch 7 not taken.
 62 value_type r((*itv - shape->C_).norm());
120
2/2
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 49 times.
 62 if (r > M_) {
121 13 M_ = r;
122 }
123 }
124 }
125 7 M_ += 1;
126 7 }
127
128 1 void ConvexShapeContact::setNormalMargin(const value_type& margin) {
129
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 assert(margin >= 0);
130 1 normalMargin_ = margin;
131 1 }
132
133 std::vector<ConvexShapeContact::ForceData>
134 ✗ ConvexShapeContact::computeContactPoints(ConfigurationIn_t q,
135 const value_type& normalMargin) const {
136 ✗ pinocchio::DeviceSync device(robot_);
137 ✗ device.currentConfiguration(q);
138 ✗ device.computeForwardKinematics(pinocchio::JOINT_POSITION);
139
140 ✗ std::vector<ForceData> forceDatas;
141 ✗ ForceData forceData;
142 ✗ ConvexShapeData od, fd;
143 ✗ for (ConvexShapes_t::const_iterator o_it = objectConvexShapes_.begin();
144 ✗ o_it != objectConvexShapes_.end(); ++o_it) {
145 ✗ od.updateToCurrentTransform(*o_it, device.d());
146 ✗ for (ConvexShapes_t::const_iterator f_it = floorConvexShapes_.begin();
147 ✗ f_it != floorConvexShapes_.end(); ++f_it) {
148 ✗ fd.updateToCurrentTransform(*f_it, device.d());
149 ✗ if (fd.isInside(*f_it, od.center_, fd.normal_)) {
150 ✗ value_type dn = fd.normal_.dot(od.center_ - fd.center_);
151 ✗ if (dn < normalMargin) {
152 // TODO: compute which points of the object are inside the floor
153 // shape.
154 ✗ forceData.joint = o_it->joint_;
155 ✗ forceData.points = o_it->Pts_;
156 ✗ forceData.normal = f_it->N_;
157 ✗ forceData.supportJoint = f_it->joint_;
158 ✗ forceDatas.push_back(forceData);
159 }
160 }
161 }
162 }
163 ✗ return forceDatas;
164 }
165
166 20995 void ConvexShapeContact::computeInternalValue(const ConfigurationIn_t& argument,
167 bool& isInside, ContactType& type,
168 vector6_t& value,
169 std::size_t& iobject,
170 std::size_t& ifloor) const {
171
1/2
✓ Branch 1 taken 21037 times.
✗ Branch 2 not taken.
 20995 GTDataV<true, true, true, false> data(relativeTransformationModel_, robot_);
172
173
2/4
✓ Branch 1 taken 21038 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21038 times.
✗ Branch 5 not taken.
 21037 data.device.currentConfiguration(argument);
174
1/2
✓ Branch 1 taken 21038 times.
✗ Branch 2 not taken.
 21038 data.device.computeForwardKinematics(pinocchio::JOINT_POSITION);
175
176
2/4
✓ Branch 1 taken 21038 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21035 times.
✗ Branch 5 not taken.
 21038 isInside = selectConvexShapes(data.device.d(), iobject, ifloor);
177 21035 const ConvexShape &object(objectConvexShapes_[iobject]),
178
1/2
✓ Branch 2 taken 21037 times.
✗ Branch 3 not taken.
 21035 floor(floorConvexShapes_[ifloor]);
179
1/2
✓ Branch 1 taken 21036 times.
✗ Branch 2 not taken.
 21037 type = contactType(object, floor);
180
181 21036 relativeTransformationModel_.joint1 = floor.joint_;
182 21032 relativeTransformationModel_.joint2 = object.joint_;
183
1/2
✓ Branch 2 taken 21038 times.
✗ Branch 3 not taken.
 21032 relativeTransformationModel_.F1inJ1 = floor.positionInJoint();
184
1/2
✓ Branch 2 taken 21038 times.
✗ Branch 3 not taken.
 21038 relativeTransformationModel_.F2inJ2 = object.positionInJoint();
185
1/2
✓ Branch 1 taken 21036 times.
✗ Branch 2 not taken.
 21038 relativeTransformationModel_.checkIsIdentity1();
186
1/2
✓ Branch 1 taken 21038 times.
✗ Branch 2 not taken.
 21036 relativeTransformationModel_.checkIsIdentity2();
187
188
1/2
✓ Branch 1 taken 21037 times.
✗ Branch 2 not taken.
 21038 compute<true, true, true, false>::error(data);
189
1/2
✓ Branch 1 taken 21035 times.
✗ Branch 2 not taken.
 21037 value = data.value;
190 21035 }
191
192 10822 void ConvexShapeContact::impl_compute(LiegroupElementRef result,
193 ConfigurationIn_t argument) const {
194 bool isInside;
195 ContactType type;
196
1/2
✓ Branch 1 taken 10794 times.
✗ Branch 2 not taken.
 10822 vector6_t value;
197 std::size_t iobject, ifloor;
198
1/2
✓ Branch 1 taken 10834 times.
✗ Branch 2 not taken.
 10794 computeInternalValue(argument, isInside, type, value, iobject, ifloor);
199
200
2/2
✓ Branch 0 taken 5648 times.
✓ Branch 1 taken 5186 times.
 10834 if (isInside) {
201
2/4
✓ Branch 1 taken 5648 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5648 times.
✗ Branch 6 not taken.
 5648 result.vector()[0] = value[0] + normalMargin_;
202
2/4
✓ Branch 2 taken 5648 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5648 times.
✗ Branch 6 not taken.
 5648 result.vector().segment<2>(1).setZero();
203 } else {
204
3/6
✓ Branch 1 taken 5186 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5186 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5186 times.
✗ Branch 9 not taken.
 5186 result.vector().segment<3>(0) = value.head<3>();
205
1/2
✓ Branch 2 taken 5186 times.
✗ Branch 3 not taken.
 5186 result.vector()[0] += normalMargin_;
206 }
207
2/3
✓ Branch 0 taken 210 times.
✓ Branch 1 taken 10624 times.
✗ Branch 2 not taken.
 10834 switch (type) {
208 210 case POINT_ON_PLANE:
209
2/4
✓ Branch 2 taken 210 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 210 times.
✗ Branch 6 not taken.
 210 result.vector().segment<2>(3).setZero();
210 210 break;
211 10624 case LINE_ON_PLANE:
212 // FIXME: only one rotation should be constrained in that case but
213 // the relative transformation is not aligned properly. The Y-axis
214 // of the reference of "object" should be aligned with the
215 // "floor" line axis (Y-axis) projection onto the plane plane.
216 // result [3] = 0;
217 // result [4] = rt_res_lge[5];
218 case PLANE_ON_PLANE:
219
3/6
✓ Branch 1 taken 10624 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10624 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10624 times.
✗ Branch 9 not taken.
 10624 result.vector().segment<2>(3) = value.tail<2>();
220 10624 break;
221 }
222 hppDout(info, "result = " << result);
223 10834 }
224
225 17518 void ConvexShapeContact::computeInternalJacobian(
226 const ConfigurationIn_t& argument, bool& isInside, ContactType& type,
227 matrix_t& jacobian) const {
228
4/8
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 17515 times.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 17518 static std::vector<bool> mask(6, true);
229
230
1/2
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
 17518 GTDataJ<true, true, true, false> data(relativeTransformationModel_, robot_);
231
232
2/4
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17519 times.
✗ Branch 5 not taken.
 17519 data.device.currentConfiguration(argument);
233
1/2
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
 17519 data.device.computeForwardKinematics(pinocchio::JOINT_POSITION |
234 pinocchio::JACOBIAN);
235
236 std::size_t ifloor, iobject;
237
2/4
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17517 times.
✗ Branch 5 not taken.
 17519 isInside = selectConvexShapes(data.device.d(), iobject, ifloor);
238 17517 const ConvexShape &object(objectConvexShapes_[iobject]),
239
1/2
✓ Branch 2 taken 17519 times.
✗ Branch 3 not taken.
 17517 floor(floorConvexShapes_[ifloor]);
240
1/2
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
 17519 type = contactType(object, floor);
241
242 17519 relativeTransformationModel_.joint1 = floor.joint_;
243 17518 relativeTransformationModel_.joint2 = object.joint_;
244
1/2
✓ Branch 2 taken 17519 times.
✗ Branch 3 not taken.
 17518 relativeTransformationModel_.F1inJ1 = floor.positionInJoint();
245
1/2
✓ Branch 2 taken 17519 times.
✗ Branch 3 not taken.
 17519 relativeTransformationModel_.F2inJ2 = object.positionInJoint();
246
1/2
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
 17519 relativeTransformationModel_.checkIsIdentity1();
247
1/2
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
 17519 relativeTransformationModel_.checkIsIdentity2();
248
1/2
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
 17519 data.cross2.setZero();
249
250
1/2
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
 17519 compute<true, true, true, false>::error(data);
251
2/4
✓ Branch 1 taken 17519 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17517 times.
✗ Branch 5 not taken.
 17519 compute<true, true, true, false>::jacobian(data, jacobian, mask);
252 17517 }
253
254 8917 void ConvexShapeContact::impl_jacobian(matrixOut_t jacobian,
255 ConfigurationIn_t argument) const {
256 bool isInside;
257 ContactType type;
258
2/4
✓ Branch 2 taken 8917 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8917 times.
✗ Branch 6 not taken.
 8917 matrix_t tmpJac(6, robot_->numberDof());
259
1/2
✓ Branch 1 taken 8917 times.
✗ Branch 2 not taken.
 8917 computeInternalJacobian(argument, isInside, type, tmpJac);
260
261
2/2
✓ Branch 0 taken 3881 times.
✓ Branch 1 taken 5036 times.
 8917 if (isInside) {
262
3/6
✓ Branch 1 taken 3881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3881 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3881 times.
✗ Branch 8 not taken.
 3881 jacobian.row(0) = tmpJac.row(0);
263
2/4
✓ Branch 1 taken 3881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3880 times.
✗ Branch 5 not taken.
 3881 jacobian.row(1).setZero();
264
2/4
✓ Branch 1 taken 3881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3881 times.
✗ Branch 5 not taken.
 3880 jacobian.row(2).setZero();
265 } else {
266
3/6
✓ Branch 1 taken 5036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5036 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5036 times.
✗ Branch 8 not taken.
 5036 jacobian.topRows<3>() = tmpJac.topRows<3>();
267 }
268
3/4
✓ Branch 0 taken 105 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 8811 times.
✓ Branch 3 taken 1 times.
 8917 switch (type) {
269 105 case POINT_ON_PLANE:
270
2/4
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
 105 jacobian.bottomRows<2>().setZero();
271 104 break;
272 ✗ case LINE_ON_PLANE:
273 // FIXME: See FIXME of impl_compute
274 // jacobian.row (3).setZero ();
275 // jacobian.row (4) = tmpJac.row (5);
276 ✗ throw std::logic_error("Contact LINE_ON_PLANE: Unimplement feature");
277 8811 case PLANE_ON_PLANE:
278 // Row: 3 4 Row: 4 5
279
3/6
✓ Branch 1 taken 8811 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8812 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8812 times.
✗ Branch 8 not taken.
 8811 jacobian.bottomRows<2>() = tmpJac.bottomRows<2>();
280 8812 break;
281 }
282 8917 }
283
284 38550 bool ConvexShapeContact::selectConvexShapes(const pinocchio::DeviceData& data,
285 std::size_t& iobject,
286 std::size_t& ifloor) const {
287
2/4
✓ Branch 1 taken 38557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38557 times.
✗ Branch 5 not taken.
 38550 ConvexShapeData od, fd;
288 38557 bool isInside = false; // Initialized only to remove compiler warning.
289
290 38557 value_type dist, minDist = +std::numeric_limits<value_type>::infinity();
291
2/2
✓ Branch 1 taken 77107 times.
✓ Branch 2 taken 38551 times.
 115660 for (std::size_t j = 0; j < floorConvexShapes_.size(); ++j) {
292
1/2
✓ Branch 2 taken 77107 times.
✗ Branch 3 not taken.
 77107 fd.updateToCurrentTransform(floorConvexShapes_[j], data);
293
294
2/2
✓ Branch 1 taken 152329 times.
✓ Branch 2 taken 77103 times.
 229436 for (std::size_t i = 0; i < objectConvexShapes_.size(); ++i) {
295
1/2
✓ Branch 2 taken 152325 times.
✗ Branch 3 not taken.
 152329 od.updateToCurrentTransform(objectConvexShapes_[i], data);
296
1/2
✓ Branch 2 taken 152328 times.
✗ Branch 3 not taken.
 152325 value_type dp = fd.distance(floorConvexShapes_[j],
297
1/2
✓ Branch 1 taken 152325 times.
✗ Branch 2 not taken.
 152325 fd.intersection(od.center_, fd.normal_)),
298
2/4
✓ Branch 1 taken 152337 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 152329 times.
✗ Branch 5 not taken.
 152328 dn = fd.normal_.dot(od.center_ - fd.center_);
299
2/2
✓ Branch 0 taken 65935 times.
✓ Branch 1 taken 86394 times.
 152329 if (dp < 0)
300 65935 dist = dn * dn;
301 else
302 86394 dist = dp * dp + dn * dn;
303
304
2/2
✓ Branch 0 taken 81751 times.
✓ Branch 1 taken 70578 times.
 152329 if (dist < minDist) {
305 81751 minDist = dist;
306 81751 iobject = i;
307 81751 ifloor = j;
308 81751 isInside = (dp < 0);
309 }
310 }
311 }
312
313 38551 return isInside;
314 }
315
316 38554 ConvexShapeContact::ContactType ConvexShapeContact::contactType(
317 const ConvexShape& object, const ConvexShape& floor) const {
318
2/4
✓ Branch 0 taken 38554 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 38555 times.
✗ Branch 3 not taken.
 38554 assert(floor.shapeDimension_ > 0 && object.shapeDimension_);
319
1/3
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 38555 times.
 38555 switch (floor.shapeDimension_) {
320 ✗ case 1:
321 ✗ throw std::logic_error("Contact on points is currently unimplemented");
322 break;
323 ✗ case 2:
324 ✗ throw std::logic_error("Contact on lines is currently unimplemented");
325 break;
326 38555 default:
327
2/3
✓ Branch 0 taken 313 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 38242 times.
 38555 switch (object.shapeDimension_) {
328 313 case 1:
329 313 return POINT_ON_PLANE;
330 break;
331 ✗ case 2:
332 ✗ return LINE_ON_PLANE;
333 break;
334 38242 default:
335 38242 return PLANE_ON_PLANE;
336 break;
337 }
338 break;
339 }
340 }
341
342 1 ConvexShapeContactComplement::ConvexShapeContactComplement(
343 const std::string& name, DevicePtr_t robot,
344 const JointAndShapes_t& floorSurfaces,
345 1 const JointAndShapes_t& objectSurfaces)
346 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
347
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 LiegroupSpace::Rn(3),
348
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 name + std::string("/complement")),
349
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 sibling_(ConvexShapeContact::create(name, robot, floorSurfaces,
350
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 objectSurfaces)) {}
351
352 ✗ bool ConvexShapeContactComplement::isEqual(
353 const DifferentiableFunction& other) const {
354 const ConvexShapeContactComplement& castother =
355 ✗ dynamic_cast<const ConvexShapeContactComplement&>(other);
356 ✗ if (!DifferentiableFunction::isEqual(other)) return false;
357 ✗ if (sibling_ == castother.sibling_) return true;
358 ✗ return (*sibling_ == *(castother.sibling_));
359 }
360
361 std::pair<ConvexShapeContactPtr_t, ConvexShapeContactComplementPtr_t>
362 1 ConvexShapeContactComplement::createPair(
363 const std::string& name, DevicePtr_t robot,
364 const JointAndShapes_t& floorSurfaces,
365 const JointAndShapes_t& objectSurfaces) {
366 ConvexShapeContactComplement* ptr = new ConvexShapeContactComplement(
367
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 name, robot, floorSurfaces, objectSurfaces);
368
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ConvexShapeContactComplementPtr_t shPtr(ptr);
369
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return std::make_pair(ptr->sibling_, shPtr);
370 1 }
371
372 400 void ConvexShapeContactComplement::computeRelativePoseRightHandSide(
373 LiegroupElementConstRef rhs, std::size_t& ifloor, std::size_t& iobject,
374 LiegroupElementRef relativePoseRhs) const {
375
3/6
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 400 times.
 400 assert(*(rhs.space()) == *LiegroupSpace::Rn(8));
376 400 value_type M(sibling_->radius());
377
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 vector8_t logRhs(log(rhs));
378
1/2
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
 400 vector6_t logRelativePoseRhs;
379
1/2
✓ Branch 2 taken 400 times.
✗ Branch 3 not taken.
 400 logRelativePoseRhs.fill(sqrt(-1));
380 // x
381
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 logRelativePoseRhs[0] = logRhs[0];
382 // ry, rz
383
3/6
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
 400 logRelativePoseRhs.segment<2>(4) = logRhs.segment<2>(3);
384 // rx
385
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 400 times.
✗ Branch 5 not taken.
 400 logRelativePoseRhs[3] = logRhs[7];
386
1/2
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
 400 value_type Y(logRhs[5]);
387
1/2
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
 400 value_type Z(logRhs[6]);
388
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 400 times.
 400 assert(floor(Y / (2 * M) + .5) >= 0);
389 400 ifloor = (std::size_t)(floor(Y / (2 * M) + .5));
390
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 400 times.
 400 assert(floor(Z / (2 * M) + .5) >= 0);
391 400 iobject = (std::size_t)(floor(Z / (2 * M) + .5));
392
5/10
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 400 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 400 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 400 times.
✗ Branch 11 not taken.
 400 if ((logRhs[1] == 0) && (logRhs[2] == 0)) {
393 // inside
394
1/2
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
 400 logRelativePoseRhs[1] = Y - (value_type)(2 * ifloor) * M;
395
1/2
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
 400 logRelativePoseRhs[2] = Z - (value_type)(2 * iobject) * M;
396 } else {
397 // outside
398 ✗ logRelativePoseRhs.segment<2>(1) = logRhs.segment<2>(1);
399 }
400
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 400 times.
 400 assert(!logRelativePoseRhs.hasNaN());
401
4/8
✓ Branch 1 taken 400 times.
✗ Branch 2 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 relativePoseRhs = LiegroupSpace::R3xSO3()->exp(logRelativePoseRhs);
402
3/6
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 400 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 400 times.
 400 assert(*(relativePoseRhs.space()) == *LiegroupSpace::R3xSO3());
403 400 }
404
405 10204 void ConvexShapeContactComplement::impl_compute(
406 LiegroupElementRef result, ConfigurationIn_t argument) const {
407 10204 value_type M(sibling_->radius());
408 bool isInside;
409 ConvexShapeContact::ContactType type;
410
1/2
✓ Branch 1 taken 10204 times.
✗ Branch 2 not taken.
 10204 vector6_t value;
411 std::size_t iobject, ifloor;
412
1/2
✓ Branch 2 taken 10204 times.
✗ Branch 3 not taken.
 10204 sibling_->computeInternalValue(argument, isInside, type, value, iobject,
413 ifloor);
414
1/2
✓ Branch 1 taken 10204 times.
✗ Branch 2 not taken.
 10204 vector3_t offset;
415
3/6
✓ Branch 1 taken 10204 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10204 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10204 times.
✗ Branch 8 not taken.
 10204 offset << (value_type)(2 * ifloor) * M, (value_type)(2 * iobject) * M, 0;
416
2/4
✓ Branch 1 taken 10204 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10204 times.
✗ Branch 6 not taken.
 10204 result.vector()[2] = value[3];
417
2/2
✓ Branch 0 taken 5018 times.
✓ Branch 1 taken 5186 times.
 10204 if (isInside)
418
3/6
✓ Branch 1 taken 5018 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5018 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5018 times.
✗ Branch 9 not taken.
 5018 result.vector().head<2>() = value.segment<2>(1);
419 else
420
2/4
✓ Branch 2 taken 5186 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5186 times.
✗ Branch 6 not taken.
 5186 result.vector().head<2>().setZero();
421
1/2
✓ Branch 2 taken 10204 times.
✗ Branch 3 not taken.
 10204 result.vector() += offset;
422 hppDout(info, "result = " << result);
423 10204 }
424
425 8602 void ConvexShapeContactComplement::impl_jacobian(
426 matrixOut_t jacobian, ConfigurationIn_t argument) const {
427 bool isInside;
428 ConvexShapeContact::ContactType type;
429
2/4
✓ Branch 3 taken 8602 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8602 times.
✗ Branch 7 not taken.
 8602 matrix_t tmpJac(6, sibling_->robot_->numberDof());
430
1/2
✓ Branch 2 taken 8602 times.
✗ Branch 3 not taken.
 8602 sibling_->computeInternalJacobian(argument, isInside, type, tmpJac);
431
432
2/2
✓ Branch 0 taken 3566 times.
✓ Branch 1 taken 5036 times.
 8602 if (isInside)
433
3/6
✓ Branch 1 taken 3566 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3566 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3566 times.
✗ Branch 8 not taken.
 3566 jacobian.topRows<2>() = tmpJac.middleRows<2>(1);
434 else
435
2/4
✓ Branch 1 taken 5036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5036 times.
✗ Branch 5 not taken.
 5036 jacobian.topRows<2>().setZero();
436
3/6
✓ Branch 1 taken 8602 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8602 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8602 times.
✗ Branch 8 not taken.
 8602 jacobian.row(2) = tmpJac.row(3);
437 8602 }
438
439 ✗ std::ostream& ConvexShapeContact::print(std::ostream& o) const {
440 ✗ o << "ConvexShapeContact: " << name() << ", active dof "
441 ✗ << pretty_print(BlockIndex::fromLogicalExpression(activeParameters_))
442 ✗ << incindent;
443
444 ✗ o << iendl << "Object shapes:" << incindent << iendl;
445 ✗ for (ConvexShapes_t::const_iterator o_it = objectConvexShapes_.begin();
446 ✗ o_it != objectConvexShapes_.end(); ++o_it) {
447 ✗ if (o_it->joint_)
448 ✗ o << "object on " << o_it->joint_->name() << iendl;
449 else
450 ✗ o << "object on universe" << iendl;
451 ✗ o << "position in joint:" << iendl;
452 ✗ o << incindent << o_it->positionInJoint();
453 ✗ o << decindent << iendl;
454 }
455 ✗ for (ConvexShapes_t::const_iterator fl_it = floorConvexShapes_.begin();
456 ✗ fl_it != floorConvexShapes_.end(); ++fl_it) {
457 ✗ if (fl_it->joint_)
458 ✗ o << "floor on " << fl_it->joint_->name() << iendl;
459 else
460 ✗ o << "floor on universe" << iendl;
461 ✗ o << "position in joint:" << iendl;
462 ✗ o << incindent << fl_it->positionInJoint();
463 ✗ o << decindent << iendl;
464 }
465 ✗ return o << decindent;
466 }
467
468 1 ConvexShapeContactHoldPtr_t ConvexShapeContactHold::create(
469 const std::string& name, DevicePtr_t robot,
470 const JointAndShapes_t& floorSurfaces,
471 const JointAndShapes_t& objectSurfaces) {
472 ConvexShapeContactHold* ptr(
473
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 new ConvexShapeContactHold(name, robot, floorSurfaces, objectSurfaces));
474 1 return ConvexShapeContactHoldPtr_t(ptr);
475 }
476
477 1 ConvexShapeContactHold::ConvexShapeContactHold(
478 const std::string& name, DevicePtr_t robot,
479 const JointAndShapes_t& floorSurfaces,
480 1 const JointAndShapes_t& objectSurfaces)
481 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
482
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 13 taken 1 times.
✗ Branch 14 not taken.
 1 LiegroupSpace::Rn(8), name) {
483 std::pair<ConvexShapeContactPtr_t, ConvexShapeContactComplementPtr_t> pair(
484 ConvexShapeContactComplement::createPair(name, robot, floorSurfaces,
485
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 objectSurfaces));
486 1 constraint_ = pair.first;
487 1 complement_ = pair.second;
488 1 }
489
490 10204 void ConvexShapeContactHold::impl_compute(LiegroupElementRef result,
491 vectorIn_t argument) const {
492
4/8
✓ Branch 1 taken 10204 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10204 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10204 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10204 times.
✗ Branch 12 not taken.
 10204 LiegroupElementRef tmp1(result.vector().head<5>(), LiegroupSpace::Rn(5));
493
4/8
✓ Branch 1 taken 10204 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10204 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10204 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10204 times.
✗ Branch 12 not taken.
 10204 LiegroupElementRef tmp2(result.vector().tail<3>(), LiegroupSpace::Rn(3));
494
3/6
✓ Branch 2 taken 10204 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10204 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10204 times.
✗ Branch 9 not taken.
 10204 constraint_->impl_compute(tmp1, argument);
495
3/6
✓ Branch 2 taken 10204 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10204 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10204 times.
✗ Branch 9 not taken.
 10204 complement_->impl_compute(tmp2, argument);
496 10204 }
497
498 8602 void ConvexShapeContactHold::impl_jacobian(matrixOut_t jacobian,
499 vectorIn_t arg) const {
500
3/6
✓ Branch 3 taken 8602 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8602 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8602 times.
✗ Branch 10 not taken.
 8602 constraint_->impl_jacobian(jacobian.topRows<5>(), arg);
501
3/6
✓ Branch 3 taken 8602 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8602 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8602 times.
✗ Branch 10 not taken.
 8602 complement_->impl_jacobian(jacobian.bottomRows<3>(), arg);
502 8602 }
503
504 ✗ bool ConvexShapeContactHold::isEqual(
505 const DifferentiableFunction& other) const {
506 const ConvexShapeContactHold& castother =
507 ✗ dynamic_cast<const ConvexShapeContactHold&>(other);
508 ✗ if (!DifferentiableFunction::isEqual(other)) return false;
509 ✗ if (constraint_ != castother.constraint_) return false;
510 ✗ if (complement_ != castother.complement_) return false;
511 ✗ return true;
512 }
513
514 } // namespace constraints
515 } // namespace hpp
516