Directory: | ./ |
---|---|
File: | src/path-optimization/spline-gradient-based/collision-constraint.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 65 | 103 | 63.1% |
Branches: | 78 | 274 | 28.5% |
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 <hpp/fcl/collision.h> | ||
35 | #include <hpp/fcl/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 | ✗ | void updateConstraint(const Configuration_t& q) { | |
61 | ✗ | fcl::CollisionResult result = checkCollision(q, true); | |
62 | |||
63 | ✗ | if (result.numContacts() == 1) { // Update qColl_ | |
64 | ✗ | qColl_ = q; | |
65 | ✗ | contactPoint_ = result.getContact(0).pos; | |
66 | hppDout(info, "contact point = " << contactPoint_.transpose()); | ||
67 | } else { // Update qFree_ | ||
68 | ✗ | qFree_ = q; | |
69 | } | ||
70 | |||
71 | ✗ | 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 fcl::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 | ✗ | fcl::CollisionResult checkCollision(const Configuration_t& q, | |
130 | bool enableContact) { | ||
131 | ✗ | pinocchio::DeviceSync device(robot_); | |
132 | ✗ | device.currentConfiguration(q); | |
133 | ✗ | device.computeForwardKinematics(pinocchio::JOINT_POSITION); | |
134 | ✗ | device.updateGeometryPlacements(); | |
135 | ✗ | fcl::CollisionResult result; | |
136 | ✗ | fcl::CollisionRequestFlag flag = | |
137 | ✗ | enableContact ? fcl::CONTACT : fcl::NO_REQUEST; | |
138 | ✗ | fcl::CollisionRequest collisionRequest(flag, 1); | |
139 | using ::pinocchio::toFclTransform3f; | ||
140 | ✗ | fcl::collide(object1_->geometry().get(), | |
141 | ✗ | toFclTransform3f(object1_->getTransform(device.d())), | |
142 | ✗ | object2_->geometry().get(), | |
143 | ✗ | toFclTransform3f(object2_->getTransform(device.d())), | |
144 | collisionRequest, result); | ||
145 | ✗ | return result; | |
146 | } | ||
147 | |||
148 | 6 | void computeJacobian() { | |
149 |
4/10✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ 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.
|
6 | static const matrix3_t I3(matrix3_t::Identity()); |
150 | |||
151 | 6 | DifferentiableFunctionPtr_t f; | |
152 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | vector3_t u; |
153 | |||
154 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | pinocchio::DeviceSync device(robot_); |
155 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | device.currentConfiguration(qColl_); |
156 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | device.computeForwardKinematics(pinocchio::JOINT_POSITION | |
157 | pinocchio::JACOBIAN); | ||
158 | |||
159 |
5/18✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 3 times.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
|
6 | if (!object2_->joint() || object2_->joint()->index() == 0) { |
160 | 6 | object1_.swap(object2_); | |
161 | } | ||
162 | |||
163 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | JointConstPtr_t joint1 = object1_->joint(); |
164 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | JointConstPtr_t joint2 = object2_->joint(); |
165 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
6 | assert(joint2 && joint2->index() > 0); |
166 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
6 | Transform3f M2(joint2->currentTransformation(device.d())); |
167 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | vector3_t x2_J2(M2.actInv(contactPoint_)); |
168 | |||
169 |
2/6✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 3 times.
|
6 | if (joint1 && joint1->index() > 0) { // object1 = body part |
170 | ✗ | Transform3f 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 | ✗ | Transform3f(I3, x1_J1), | |
187 | ✗ | Transform3f(I3, x2_J2)); | |
188 | } else { // object1 = fixed obstacle and has no joint | ||
189 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | vector3_t x1_J1(contactPoint_); |
190 | // Compute contact points in configuration qFree | ||
191 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | device.currentConfiguration(qFree_); |
192 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | device.computeForwardKinematics(pinocchio::JOINT_POSITION); |
193 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
6 | Transform3f M2(joint2->currentTransformation(device.d())); |
194 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | device.unlock(); |
195 | // position of x2 in global frame | ||
196 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | vector3_t x2_J1(M2.act(x2_J2)); |
197 | hppDout(info, "x2 in J1 = " << x2_J1.transpose()); | ||
198 | |||
199 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | u = (x2_J1 - x2_J2).normalized(); |
200 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
18 | f = constraints::Position::create( |
201 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
12 | "", robot_, joint2, Transform3f(I3, x2_J2), Transform3f(I3, x1_J1)); |
202 | } | ||
203 |
1/2✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
6 | matrix_t Jpos(f->outputSize(), f->inputDerivativeSize()); |
204 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
6 | f->jacobian(Jpos, qFree_); |
205 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | J_ = u.transpose() * Jpos; |
206 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
|
6 | assert(J_.rows() == 1); |
207 | } | ||
208 | |||
209 | 6 | virtual void impl_compute(LiegroupElementRef result, | |
210 | vectorIn_t argument) const { | ||
211 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
12 | pinocchio::difference<pinocchio::DefaultLieGroupMap>(robot_, argument, |
212 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | qFree_, difference_); |
213 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | result.vector() = J_ * difference_; |
214 | 6 | result.check(); | |
215 | } | ||
216 | 3 | virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t) const { | |
217 | 6 | 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 |