hpp-constraints: include/hpp/constraints/convex-shape.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
convex-shape.hh
Go to the documentation of this file.
1// Copyright (c) 2015, 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#ifndef HPP_CONSTRAINTS_CONVEX_SHAPE_HH
30#define HPP_CONSTRAINTS_CONVEX_SHAPE_HH
31
32// clang-format off
33#include <pinocchio/multibody/model.hpp>
34// clang-format on
35
36#include <coal/shape/geometric_shapes.h>
37
38#include <vector>
39
40// Only for specialization of vector3_t. This is a bad design of Pinocchio.
41#include <hpp/constraints/config.hh>
42#include <hpp/constraints/fwd.hh>
43#include <hpp/pinocchio/joint.hh>
44
45 namespace hpp {
46 namespace constraints {
51 inline void closestPointToSegment(const vector3_t& P, const vector3_t& A,
52 const vector3_t& v, vector3_t& B) {
53 vector3_t w = A - P;
54 value_type c1, c2;
55 c1 = v.dot(w);
56 c2 = v.dot(v);
57 if (c1 <= 0)
58 B = A;
59 else if (c2 <= c1)
60 B = A + v;
61 else
62 B = A + c1 / c2 * v;
63}
64
71 inline vector3_t linePlaneIntersection(const vector3_t& A, const vector3_t& u,
72 const vector3_t& P, const vector3_t& n) {
73 assert(std::abs(n.dot(u)) > 1e-8);
74 return A + u * (n.dot(P - A)) / n.dot(u);
75}
76
77 class HPP_CONSTRAINTS_DLLAPI ConvexShape {
78 public:
87 ConvexShape(const std::vector<vector3_t>& pts,
88 JointPtr_t joint = JointPtr_t())
89 : Pts_(pts), joint_(joint) {
90 init();
91 }
92
93 ConvexShape(const coal::TriangleP& t, const JointPtr_t& joint = JointPtr_t())
94 : Pts_(triangleToPoints(t)), joint_(joint) {
95 init();
96 }
97
100 ConvexShape(const vector3_t& p0, const vector3_t& p1, const vector3_t& p2,
101 const JointPtr_t& joint = JointPtr_t())
102 : Pts_(points(p0, p1, p2)), joint_(joint) {
103 init();
104 }
105
106 // Copy constructor
107 ConvexShape(const ConvexShape& t) : Pts_(t.Pts_), joint_(t.joint_) { init(); }
108
109 void reverse() {
110 std::reverse(Pts_.begin(), Pts_.end());
111 init();
112 }
113
116 inline vector3_t intersectionLocal(const vector3_t& A,
117 const vector3_t& u) const {
118 return linePlaneIntersection(A, u, C_, N_);
119 }
120
122 inline bool isInsideLocal(const vector3_t& Ap) const {
123 assert(shapeDimension_ > 2);
124 for (std::size_t i = 0; i < shapeDimension_; ++i) {
125 if (Ns_[i].dot(Ap - Pts_[i]) > 0) return false;
126 }
127 return true;
128 }
129
134 inline value_type distanceLocal(const vector3_t& a) const {
135 assert(shapeDimension_ > 1);
136 const value_type inf = std::numeric_limits<value_type>::infinity();
137 value_type minPosDist = inf, maxNegDist = -inf;
138 bool outside = false;
139 for (std::size_t i = 0; i < shapeDimension_; ++i) {
140 value_type d = dist(a - Pts_[i], Ls_[i], Us_[i], Ns_[i]);
141 if (d > 0) {
142 outside = true;
143 if (d < minPosDist) minPosDist = d;
144 }
145 if (d <= 0 && d > maxNegDist) maxNegDist = d;
146 }
147 if (outside) return minPosDist;
148 return maxNegDist;
149 }
150
152 inline const vector3_t& planeXaxis() const {
153 assert(shapeDimension_ > 2);
154 return Ns_[0];
155 }
158 inline const vector3_t& planeYaxis() const {
159 assert(shapeDimension_ > 2);
160 return Us_[0];
161 }
162
164 inline const Transform3s& positionInJoint() const { return MinJoint_; }
165
166 bool operator==(ConvexShape const& other) const {
167 if (Pts_ != other.Pts_) return false;
168 if (shapeDimension_ != other.shapeDimension_) return false;
169 if (C_ != other.C_) return false;
170 if (N_ != other.N_) return false;
171 if (Ns_ != other.Ns_) return false;
172 if (Us_ != other.Us_) return false;
173 if (Ls_ != other.Ls_) return false;
174 if (MinJoint_ != other.MinJoint_) return false;
175 if (joint_ != other.joint_) return false;
176 return true;
177 }
178 bool operator!=(ConvexShape const& other) const {
179 return !(this->operator==(other));
180 }
181
183 std::vector<vector3_t> Pts_;
184 size_t shapeDimension_;
186 vector3_t C_;
188 vector3_t N_;
193 std::vector<vector3_t> Ns_, Us_;
194 vector_t Ls_;
195 Transform3s MinJoint_;
196 JointPtr_t joint_;
197
198 private:
202 inline value_type dist(const vector3_t& w, const value_type& c2,
203 const vector3_t& v, const vector3_t& u) const {
204 value_type c1;
205 c1 = v.dot(w);
206 if (c1 <= 0) return (u.dot(w) > 0) ? (w.norm()) : (-w.norm());
207 if (c2 <= c1)
208 // TODO: (w - c2 * v).norm() == sqrt((u.dot(w)**2 + (c1 - c2)**2)
209 // second should be cheaper.
210 return (u.dot(w) > 0) ? ((w - c2 * v).norm()) : (-(w - c2 * v).norm());
211 return u.dot(w);
212 }
213
214 static std::vector<vector3_t> triangleToPoints(const coal::TriangleP& t) {
215 // TODO
216 // return points (t.a, t.b, t.c);
217 std::vector<vector3_t> ret(3);
218 ret[0] = t.a;
219 ret[1] = t.b;
220 ret[2] = t.c;
221 return ret;
222 }
223 static std::vector<vector3_t> points(const vector3_t& p0, const vector3_t& p1,
224 const vector3_t& p2) {
225 std::vector<vector3_t> ret(3);
226 ret[0] = p0;
227 ret[1] = p1;
228 ret[2] = p2;
229 return ret;
230 }
231
232 void init() {
233 shapeDimension_ = Pts_.size();
234
235 switch (shapeDimension_) {
236 case 0:
237 throw std::logic_error("Cannot represent an empty shape.");
238 break;
239 case 1:
240 C_ = Pts_[0];
241 // The transformation will be (N_, Ns_[0], Us_[0])
242 // Fill vectors so as to be consistent
243 N_ = vector3_t(1, 0, 0);
244 Ns_.push_back(vector3_t(0, 1, 0));
245 Us_.push_back(vector3_t(0, 0, 1));
246 break;
247 case 2:
248 Ls_ = vector_t(1);
249 C_ = (Pts_[0] + Pts_[1]) / 2;
250 // The transformation will be (N_, Ns_[0], Us_[0])
251 // Fill vectors so as to be consistent
252 Us_.push_back(Pts_[1] - Pts_[0]);
253 Ls_[0] = Us_[0].norm();
254 Us_[0].normalize();
255 if (Us_[0][0] != 0)
256 N_ = vector3_t(-Us_[0][1], Us_[0][0], 0);
257 else
258 N_ = vector3_t(0, -Us_[0][2], Us_[0][1]);
259 N_.normalize();
260 Ns_.push_back(Us_[0].cross(N_));
261 Ns_[0].normalize(); // Should be unnecessary
262 break;
263 default:
264 Ls_ = vector_t(shapeDimension_);
265 C_.setZero();
266 for (std::size_t i = 0; i < shapeDimension_; ++i) C_ += Pts_[i];
267 // TODO This is very ugly. Why Eigen does not have the operator/=(int)
268 // ...
269 C_ /= (value_type)Pts_.size();
270 N_ = (Pts_[1] - Pts_[0]).cross(Pts_[2] - Pts_[1]);
271 assert(!N_.isZero());
272 N_.normalize();
273
274 Us_.resize(Pts_.size());
275 Ns_.resize(Pts_.size());
276 for (std::size_t i = 0; i < shapeDimension_; ++i) {
277 Us_[i] = Pts_[(i + 1) % shapeDimension_] - Pts_[i];
278 Ls_[i] = Us_[i].norm();
279 Us_[i].normalize();
280 Ns_[i] = Us_[i].cross(N_);
281 Ns_[i].normalize();
282 }
283 for (std::size_t i = 0; i < shapeDimension_; ++i) {
284 assert(Us_[(i + 1) % shapeDimension_].dot(Ns_[i]) < 0 &&
285 "The sequence does not define a convex surface");
286 }
287 break;
288 }
289
290 MinJoint_.translation() = C_;
291 MinJoint_.rotation().col(0) = N_;
292 MinJoint_.rotation().col(1) = Ns_[0];
293 MinJoint_.rotation().col(2) = Us_[0];
294 }
295};
296
297 struct HPP_CONSTRAINTS_DLLAPI ConvexShapeData {
298 // normal in the world frame
299 vector3_t normal_;
300 // center in the world frame
301 vector3_t center_;
302 // Current joint position
303 Transform3s oMj_;
304
306 inline void updateToCurrentTransform(const ConvexShape& cs) {
307 if (cs.joint_ == NULL) {
308 oMj_.setIdentity();
309 _recompute<true>(cs);
310 } else {
311 oMj_ = cs.joint_->currentTransformation();
312 _recompute<false>(cs);
313 }
314 }
315
318 inline void updateToCurrentTransform(const ConvexShape& cs,
319 const pinocchio::DeviceData& d) {
320 if (cs.joint_ == NULL) {
321 oMj_.setIdentity();
322 _recompute<true>(cs);
323 } else {
324 oMj_ = cs.joint_->currentTransformation(d);
325 _recompute<false>(cs);
326 }
327 }
328
329 template <bool WorldFrame>
330 inline void _recompute(const ConvexShape& cs) {
331 if (WorldFrame) {
332 center_ = cs.C_;
333 normal_ = cs.N_;
334 } else {
335 center_ = oMj_.act(cs.C_);
336 normal_ = oMj_.rotation() * cs.N_;
337 }
338 }
339
342 inline vector3_t intersection(const vector3_t& A, const vector3_t& u) const {
343 return linePlaneIntersection(A, u, center_, normal_);
344 }
345
350 inline bool isInside(const ConvexShape& cs, const vector3_t& A,
351 const vector3_t& u) const {
352 return isInside(cs, intersection(A, u));
353 }
355 inline bool isInside(const ConvexShape& cs, const vector3_t& Ap) const {
356 if (cs.joint_ == NULL) return cs.isInsideLocal(Ap);
357 vector3_t Ap_loc = oMj_.actInv(Ap);
358 return cs.isInsideLocal(Ap_loc);
359 }
360
362 inline Transform3s alignedPositionInJoint(const ConvexShape& cs,
363 vector3_t yaxis) const {
364 assert(cs.shapeDimension_ > 2);
365 // Project vector onto the plane
366 yaxis = oMj_.actInv(yaxis);
367 vector3_t yproj = yaxis - yaxis.dot(cs.N_) * cs.N_;
368 if (yproj.isZero())
369 return cs.MinJoint_;
370 else {
371 Transform3s M;
372 M.translation() = cs.C_;
373 M.rotation().col(0) = cs.N_;
374 M.rotation().col(1) = yaxis;
375 M.rotation().col(2) = cs.N_.cross(yaxis);
376 return M;
377 }
378 }
379
383 inline value_type distance(const ConvexShape& cs, vector3_t a) const {
384 if (cs.joint_ != NULL) a = oMj_.actInv(a);
385 return cs.distanceLocal(a);
386 }
387};
388} // namespace constraints
389} // namespace hpp
390
391#endif // HPP_CONSTRAINTS_CONVEX_SHAPE_HH
hpp::constraints::ConvexShape
Definition convex-shape.hh:77
hpp::constraints::ConvexShape::ConvexShape
ConvexShape(const std::vector< vector3_t > &pts, JointPtr_t joint=JointPtr_t())
Definition convex-shape.hh:87
hpp::constraints::ConvexShape::MinJoint_
Transform3s MinJoint_
Definition convex-shape.hh:195
hpp::constraints::ConvexShape::ConvexShape
ConvexShape(const vector3_t &p0, const vector3_t &p1, const vector3_t &p2, const JointPtr_t &joint=JointPtr_t())
Definition convex-shape.hh:100
hpp::constraints::ConvexShape::planeYaxis
const vector3_t & planeYaxis() const
Definition convex-shape.hh:158
hpp::constraints::ConvexShape::operator!=
bool operator!=(ConvexShape const &other) const
Definition convex-shape.hh:178
hpp::constraints::ConvexShape::C_
vector3_t C_
the center in the joint frame. It is constant.
Definition convex-shape.hh:186
hpp::constraints::ConvexShape::Us_
std::vector< vector3_t > Us_
Definition convex-shape.hh:193
hpp::constraints::ConvexShape::Pts_
std::vector< vector3_t > Pts_
The points in the joint frame. It is constant.
Definition convex-shape.hh:183
hpp::constraints::ConvexShape::operator==
bool operator==(ConvexShape const &other) const
Definition convex-shape.hh:166
hpp::constraints::ConvexShape::ConvexShape
ConvexShape(const ConvexShape &t)
Definition convex-shape.hh:107
hpp::constraints::ConvexShape::isInsideLocal
bool isInsideLocal(const vector3_t &Ap) const
As isInside but consider A as expressed in joint frame.
Definition convex-shape.hh:122
hpp::constraints::ConvexShape::reverse
void reverse()
Definition convex-shape.hh:109
hpp::constraints::ConvexShape::N_
vector3_t N_
the normal to the shape in the joint frame. It is constant.
Definition convex-shape.hh:188
hpp::constraints::ConvexShape::distanceLocal
value_type distanceLocal(const vector3_t &a) const
Definition convex-shape.hh:134
hpp::constraints::ConvexShape::Ls_
vector_t Ls_
Definition convex-shape.hh:194
hpp::constraints::ConvexShape::Ns_
std::vector< vector3_t > Ns_
Definition convex-shape.hh:193
hpp::constraints::ConvexShape::ConvexShape
ConvexShape(const coal::TriangleP &t, const JointPtr_t &joint=JointPtr_t())
Definition convex-shape.hh:93
hpp::constraints::ConvexShape::intersectionLocal
vector3_t intersectionLocal(const vector3_t &A, const vector3_t &u) const
Definition convex-shape.hh:116
hpp::constraints::ConvexShape::positionInJoint
const Transform3s & positionInJoint() const
Transform of the shape in the joint frame.
Definition convex-shape.hh:164
hpp::constraints::ConvexShape::planeXaxis
const vector3_t & planeXaxis() const
Return the X axis of the plane in the joint frame.
Definition convex-shape.hh:152
hpp::constraints::ConvexShape::joint_
JointPtr_t joint_
Definition convex-shape.hh:196
hpp::constraints::ConvexShape::shapeDimension_
size_t shapeDimension_
Definition convex-shape.hh:184
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
fwd.hh
joint.hh
Eigen::assert
assert(d.lhs()._blocks()==d.rhs()._blocks())
hpp::constraints::vector3_t
pinocchio::vector3_t vector3_t
Definition fwd.hh:53
hpp::constraints::closestPointToSegment
void closestPointToSegment(const vector3_t &P, const vector3_t &A, const vector3_t &v, vector3_t &B)
Definition convex-shape.hh:51
hpp::constraints::Transform3s
pinocchio::Transform3s Transform3s
Definition fwd.hh:65
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::operator==
bool operator==(const ComparisonTypes_t &v, const internal::ReplicateCompType &r)
Definition comparison-types.hh:117
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:60
hpp::constraints::linePlaneIntersection
vector3_t linePlaneIntersection(const vector3_t &A, const vector3_t &u, const vector3_t &P, const vector3_t &n)
Definition convex-shape.hh:71
hpp::constraints::JointPtr_t
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:50
hpp
hpp::constraints::ConvexShapeData
Definition convex-shape.hh:297
hpp::constraints::ConvexShapeData::alignedPositionInJoint
Transform3s alignedPositionInJoint(const ConvexShape &cs, vector3_t yaxis) const
Definition convex-shape.hh:362
hpp::constraints::ConvexShapeData::normal_
vector3_t normal_
Definition convex-shape.hh:299
hpp::constraints::ConvexShapeData::updateToCurrentTransform
void updateToCurrentTransform(const ConvexShape &cs, const pinocchio::DeviceData &d)
Definition convex-shape.hh:318
hpp::constraints::ConvexShapeData::oMj_
Transform3s oMj_
Definition convex-shape.hh:303
hpp::constraints::ConvexShapeData::_recompute
void _recompute(const ConvexShape &cs)
Definition convex-shape.hh:330
hpp::constraints::ConvexShapeData::isInside
bool isInside(const ConvexShape &cs, const vector3_t &A, const vector3_t &u) const
Definition convex-shape.hh:350
hpp::constraints::ConvexShapeData::intersection
vector3_t intersection(const vector3_t &A, const vector3_t &u) const
Definition convex-shape.hh:342
hpp::constraints::ConvexShapeData::updateToCurrentTransform
void updateToCurrentTransform(const ConvexShape &cs)
Compute center and normal in world frame.
Definition convex-shape.hh:306
hpp::constraints::ConvexShapeData::isInside
bool isInside(const ConvexShape &cs, const vector3_t &Ap) const
Check whether the point As in world frame is inside the triangle.
Definition convex-shape.hh:355
hpp::constraints::ConvexShapeData::distance
value_type distance(const ConvexShape &cs, vector3_t a) const
Definition convex-shape.hh:383
hpp::constraints::ConvexShapeData::center_
vector3_t center_
Definition convex-shape.hh:301
hpp::pinocchio::DeviceData