GCC Code Coverage Report

Directory: ./
File: src/path-optimization/spline-gradient-based/collision-constraint.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 86 103 83.5%
Branches: 109 274 39.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015 CNRS
3 // Author: Mylene Campana, Joseph Mirabel
4 //
5 //
6
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30
31 #ifndef HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_COLLISION_CONSTRAINTS_HH
32 #define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_COLLISION_CONSTRAINTS_HH
33
34 #include <coal/collision.h>
35 #include <coal/distance.h>
36
37 #include <hpp/constraints/generic-transformation.hh>
38 #include <hpp/pinocchio/configuration.hh>
39 #include <hpp/pinocchio/util.hh>
40 #include <hpp/util/exception-factory.hh>
41 #include <pinocchio/multibody/liegroup/liegroup.hpp>
42
43 namespace hpp {
44 namespace core {
45 namespace pathOptimization {
46
47 template <typename SplinePtr_t>
48 class CollisionFunction : public DifferentiableFunction {
49 public:
50 typedef shared_ptr<CollisionFunction> Ptr_t;
51 12 virtual ~CollisionFunction() {}
52 3 static Ptr_t create(const DevicePtr_t& robot, const SplinePtr_t& freeSpline,
53 const SplinePtr_t& collSpline,
54 const PathValidationReportPtr_t& report) {
55 6 CollisionFunction* ptr =
56
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 new CollisionFunction(robot, freeSpline, collSpline, report);
57 6 return Ptr_t(ptr);
58 }
59
60 12 void updateConstraint(const Configuration_t& q) {
61
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 coal::CollisionResult result = checkCollision(q, true);
62
63
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
 12 if (result.numContacts() == 1) { // Update qColl_
64
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 qColl_ = q;
65
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 contactPoint_ = result.getContact(0).pos;
66 hppDout(info, "contact point = " << contactPoint_.transpose());
67 } else { // Update qFree_
68
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 qFree_ = q;
69 }
70
71
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 computeJacobian();
72 }
73
74 Configuration_t qFree_, qColl_;
75
76 protected:
77 6 CollisionFunction(const DevicePtr_t& robot, const SplinePtr_t& freeSpline,
78 const SplinePtr_t& collSpline,
79 const PathValidationReportPtr_t& report)
80 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
81 LiegroupSpace::R1(), ""),
82 6 qFree_(),
83
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 qColl_(),
84 6 robot_(robot),
85 6 object1_(),
86 6 object2_(),
87
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 J_(),
88
9/18
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 29 taken 3 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 3 times.
✗ Branch 33 not taken.
 12 difference_(robot->numberDof()) {
89 6 const value_type& tColl = report->parameter;
90 bool success;
91
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 qColl_ = collSpline->eval(tColl, success);
92
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 6 assert(success);
93
94 6 CollisionValidationReportPtr_t collisionReport = (HPP_DYNAMIC_PTR_CAST(
95 CollisionValidationReport, report->configurationReport));
96
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 6 if (!collisionReport) {
97 ✗ std::stringstream ss;
98 ✗ ss << "Expected a CollisionValidationReport. Got a "
99 ✗ << *report->configurationReport;
100 ✗ throw std::logic_error(ss.str());
101 }
102 6 object1_ = collisionReport->object1;
103 6 object2_ = collisionReport->object2;
104
105 hppDout(info, "obj1 = " << object1_->name()
106 << " and obj2 = " << object2_->name());
107 hppDout(info, "qColl = " << pinocchio::displayConfig(qColl_));
108
109 // Backtrack collision in previous path (x0) to create constraint
110
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 6 value_type tFree = tColl * freeSpline->length() / collSpline->length();
111 12 tFree = std::min(tFree,
112
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 freeSpline->length()); // can be slightly above freeSpline
113 // length due to round-offs
114
115
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 qFree_ = freeSpline->eval(tFree, success);
116
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 6 assert(success);
117 hppDout(info, "qFree = " << pinocchio::displayConfig(qFree_));
118 // Compute contact point in configuration qColl
119 6 const coal::CollisionResult& result(collisionReport->result);
120
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 6 if (result.numContacts() < 1) {
121 ✗ abort();
122 }
123
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 6 assert(result.numContacts() >= 1);
124
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 contactPoint_ = result.getContact(0).pos;
125 hppDout(info, "contact point = " << contactPoint_.transpose());
126
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 computeJacobian();
127 }
128
129 12 coal::CollisionResult checkCollision(const Configuration_t& q,
130 bool enableContact) {
131
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 pinocchio::DeviceSync device(robot_);
132
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 device.currentConfiguration(q);
133
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 device.computeForwardKinematics(pinocchio::JOINT_POSITION);
134
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 device.updateGeometryPlacements();
135
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 coal::CollisionResult result;
136 12 coal::CollisionRequestFlag flag =
137
1/2
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
 12 enableContact ? coal::CONTACT : coal::NO_REQUEST;
138
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 coal::CollisionRequest collisionRequest(flag, 1);
139 using ::pinocchio::toFclTransform3f;
140
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
 12 coal::collide(object1_->geometry().get(),
141
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 12 toFclTransform3f(object1_->getTransform(device.d())),
142
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 24 object2_->geometry().get(),
143
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 12 toFclTransform3f(object2_->getTransform(device.d())),
144 collisionRequest, result);
145 24 return result;
146 }
147
148 18 void computeJacobian() {
149
5/10
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 6 times.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 18 static const matrix3_t I3(matrix3_t::Identity());
150
151 18 DifferentiableFunctionPtr_t f;
152
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 vector3_t u;
153
154
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 pinocchio::DeviceSync device(robot_);
155
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 18 device.currentConfiguration(qColl_);
156
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 device.computeForwardKinematics(pinocchio::JOINT_POSITION |
157 pinocchio::JACOBIAN);
158
159
10/18
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 3 times.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 6 times.
✓ Branch 15 taken 6 times.
✓ Branch 16 taken 3 times.
✓ Branch 18 taken 9 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✓ Branch 22 taken 6 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
 18 if (!object2_->joint() || object2_->joint()->index() == 0) {
160 6 object1_.swap(object2_);
161 }
162
163
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 18 JointConstPtr_t joint1 = object1_->joint();
164
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 18 JointConstPtr_t joint2 = object2_->joint();
165
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 18 assert(joint2 && joint2->index() > 0);
166
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 18 Transform3s M2(joint2->currentTransformation(device.d()));
167
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 vector3_t x2_J2(M2.actInv(contactPoint_));
168
169
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 9 times.
 18 if (joint1 && joint1->index() > 0) { // object1 = body part
170 ✗ Transform3s M1(joint1->currentTransformation(device.d()));
171 // Position of contact point in each object local frame
172 ✗ vector3_t x1_J1 = M1.actInv(contactPoint_);
173 // Compute contact points in configuration qFree
174 ✗ device.currentConfiguration(qFree_);
175 ✗ device.computeForwardKinematics(pinocchio::JOINT_POSITION);
176 ✗ M2 = joint2->currentTransformation(device.d());
177 ✗ M1 = joint1->currentTransformation(device.d());
178 ✗ device.unlock();
179 // Position of x1 in local frame of joint2
180 ✗ vector3_t x1_J2(M2.actInv(M1.act(x1_J1)));
181 hppDout(info, "x2 in J2 = " << x2_J2.transpose());
182 hppDout(info, "x1 in J2 = " << x1_J2.transpose());
183
184 ✗ u = (x1_J2 - x2_J2).normalized();
185 ✗ f = constraints::RelativePosition::create("", robot_, joint1, joint2,
186 ✗ Transform3s(I3, x1_J1),
187 ✗ Transform3s(I3, x2_J2));
188 } else { // object1 = fixed obstacle and has no joint
189
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 vector3_t x1_J1(contactPoint_);
190 // Compute contact points in configuration qFree
191
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 18 device.currentConfiguration(qFree_);
192
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 device.computeForwardKinematics(pinocchio::JOINT_POSITION);
193
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 18 Transform3s M2(joint2->currentTransformation(device.d()));
194
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 device.unlock();
195 // position of x2 in global frame
196
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 vector3_t x2_J1(M2.act(x2_J2));
197 hppDout(info, "x2 in J1 = " << x2_J1.transpose());
198
199
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 18 u = (x2_J1 - x2_J2).normalized();
200
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 54 f = constraints::Position::create(
201
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 36 "", robot_, joint2, Transform3s(I3, x2_J2), Transform3s(I3, x1_J1));
202 }
203
1/2
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 18 matrix_t Jpos(f->outputSize(), f->inputDerivativeSize());
204
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 18 f->jacobian(Jpos, qFree_);
205
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 18 J_ = u.transpose() * Jpos;
206
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
 18 assert(J_.rows() == 1);
207 }
208
209 18 virtual void impl_compute(LiegroupElementRef result,
210 vectorIn_t argument) const {
211
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 36 pinocchio::difference<pinocchio::DefaultLieGroupMap>(robot_, argument,
212
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 qFree_, difference_);
213
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 18 result.vector() = J_ * difference_;
214 18 result.check();
215 }
216 9 virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t) const {
217 18 jacobian = J_;
218 }
219
220 private:
221 DevicePtr_t robot_;
222 CollisionObjectConstPtr_t object1_, object2_;
223 matrix_t J_;
224 vector3_t contactPoint_;
225 mutable vector_t difference_;
226 }; // class CollisionFunction
227 } // namespace pathOptimization
228 } // namespace core
229 } // namespace hpp
230 #endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_COLLISION_CONSTRAINTS_HH
231