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

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

Loading...
Searching...
No Matches
convex-shape-contact.hh
Go to the documentation of this file.
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#ifndef HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
30#define HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
31
32#include <hpp/constraints/config.hh>
33#include <hpp/constraints/convex-shape.hh>
34#include <hpp/constraints/deprecated.hh>
35#include <hpp/constraints/differentiable-function.hh>
36#include <hpp/constraints/fwd.hh>
37#include <hpp/constraints/generic-transformation.hh>
38#include <vector>
39
40 namespace hpp {
41 namespace constraints {
42
45
93 class HPP_CONSTRAINTS_DLLAPI ConvexShapeContact
94 : public DifferentiableFunction {
95 public:
96 friend class ConvexShapeContactComplement;
97 friend class ConvexShapeContactHold;
98
100 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
102
104 enum ContactType {
106 POINT_ON_PLANE,
108 LINE_ON_PLANE,
111 PLANE_ON_PLANE
112 };
113
117 struct ForceData {
118 JointPtr_t joint;
119 JointPtr_t supportJoint;
120 std::vector<vector3_t> points;
121 vector3_t normal;
122 };
123
129 static ConvexShapeContactPtr_t create(const std::string& name,
130 DevicePtr_t robot,
131 const JointAndShapes_t& floorSurfaces,
132 const JointAndShapes_t& objectSurfaces);
133
134 static ConvexShapeContactPtr_t create(const DevicePtr_t& robot);
135
137 const ConvexShapes_t& floorContactSurfaces() const {
138 return floorConvexShapes_;
139 }
141 const ConvexShapes_t& objectContactSurfaces() const {
142 return objectConvexShapes_;
143 }
146 value_type radius() const { return M_; }
150 void setNormalMargin(const value_type& margin);
151
153 std::vector<ForceData> computeContactPoints(
154 ConfigurationIn_t q, const value_type& normalMargin) const;
155
157 std::ostream& print(std::ostream& o) const;
158
167 std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
168 DeviceConstPtr_t /*robot*/) const {
169 if ((floorConvexShapes_.size() == 0) || (objectConvexShapes_.size() == 0)) {
170 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
171 }
172 JointConstPtr_t floor0_joint = floorConvexShapes_[0].joint_;
173 JointConstPtr_t object0_joint = objectConvexShapes_[0].joint_;
174
175 size_type index1 = Joint::index(floor0_joint);
176 size_type index2 = Joint::index(object0_joint);
177 // check that all the joints involved are the same
178 for (ConvexShapes_t::const_iterator it(floorConvexShapes_.begin());
179 it != floorConvexShapes_.end(); ++it) {
180 size_type jointIndex = Joint::index(it->joint_);
181 if (jointIndex != index1) {
182 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
183 }
184 }
185
186 for (ConvexShapes_t::const_iterator it(objectConvexShapes_.begin());
187 it != objectConvexShapes_.end(); ++it) {
188 size_type jointIndex = Joint::index(it->joint_);
189 if (jointIndex != index2) {
190 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
191 }
192 }
193
194 if (index1 <= index2) {
195 return std::pair<JointConstPtr_t, JointConstPtr_t>(floor0_joint,
196 object0_joint);
197 } else {
198 return std::pair<JointConstPtr_t, JointConstPtr_t>(object0_joint,
199 floor0_joint);
200 }
201 };
202
203 protected:
209 ConvexShapeContact(const std::string& name, DevicePtr_t robot,
210 const JointAndShapes_t& floorSurfaces,
211 const JointAndShapes_t& objectSurfaces);
212
213 bool isEqual(const DifferentiableFunction& other) const;
214
215 private:
217 void addObject(const ConvexShape& t);
218
223 void addFloor(const ConvexShape& t);
224 void computeRadius();
225
226 void impl_compute(LiegroupElementRef result,
227 ConfigurationIn_t argument) const;
228 void computeInternalValue(const ConfigurationIn_t& argument, bool& isInside,
229 ContactType& type, vector6_t& value,
230 std::size_t& iobject, std::size_t& ifloor) const;
231
232 void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
233 void computeInternalJacobian(const ConfigurationIn_t& argument,
234 bool& isInside, ContactType& type,
235 matrix_t& jacobian) const;
236
241 bool selectConvexShapes(const pinocchio::DeviceData& data,
242 std::size_t& iobject, std::size_t& ifloor) const;
243 ContactType contactType(const ConvexShape& object,
244 const ConvexShape& floor) const;
245
246 DevicePtr_t robot_;
247 mutable GenericTransformationModel<true> relativeTransformationModel_;
248
249 ConvexShapes_t objectConvexShapes_;
250 ConvexShapes_t floorConvexShapes_;
251
252 value_type normalMargin_;
253 // upper bound of distance between center of polygon and vectices for
254 // all floor polygons.
255 value_type M_;
256};
257
279 class HPP_CONSTRAINTS_DLLAPI ConvexShapeContactComplement
280 : public DifferentiableFunction {
281 public:
282 friend class ConvexShapeContactHold;
289 static std::pair<ConvexShapeContactPtr_t, ConvexShapeContactComplementPtr_t>
290 createPair(const std::string& name, DevicePtr_t robot,
291 const JointAndShapes_t& floorSurfaces,
292 const JointAndShapes_t& objectSurfaces);
293
300 void computeRelativePoseRightHandSide(
301 LiegroupElementConstRef rhs, std::size_t& ifloor, std::size_t& iobject,
302 LiegroupElementRef relativePoseRhs) const;
303
312 std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
313 DeviceConstPtr_t /*robot*/) const {
314 return sibling_->dependsOnRelPoseBetween(nullptr);
315 };
316
317 protected:
324 ConvexShapeContactComplement(const std::string& name, DevicePtr_t robot,
325 const JointAndShapes_t& floorSurfaces,
326 const JointAndShapes_t& objectSurfaces);
327
328 bool isEqual(const DifferentiableFunction& other) const;
329
330 private:
331 void impl_compute(LiegroupElementRef result,
332 ConfigurationIn_t argument) const;
333
334 void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
335
336 ConvexShapeContactPtr_t sibling_;
337}; // class ConvexShapeContactComplement
338
346 class HPP_CONSTRAINTS_DLLAPI ConvexShapeContactHold
347 : public DifferentiableFunction {
348 public:
356 static ConvexShapeContactHoldPtr_t create(
357 const std::string& name, DevicePtr_t robot,
358 const JointAndShapes_t& floorSurfaces,
359 const JointAndShapes_t& objectSurfaces);
360
361 ConvexShapeContactPtr_t contactConstraint() const { return constraint_; }
362 ConvexShapeContactComplementPtr_t complement() const { return complement_; }
363
372 std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
373 DeviceConstPtr_t /*robot*/) const {
374 return constraint_->dependsOnRelPoseBetween(nullptr);
375 }
376
377 protected:
385 ConvexShapeContactHold(const std::string& name, DevicePtr_t robot,
386 const JointAndShapes_t& floorSurfaces,
387 const JointAndShapes_t& objectSurfaces);
388
389 virtual void impl_compute(LiegroupElementRef result,
390 vectorIn_t argument) const;
391 virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const;
392
393 bool isEqual(const DifferentiableFunction& other) const;
394
395 private:
396 ConvexShapeContactPtr_t constraint_;
397 ConvexShapeContactComplementPtr_t complement_;
398}; // class ConvexShapeContactHold
400} // namespace constraints
401} // namespace hpp
402
403#endif // HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
hpp::constraints::ConvexShapeContactComplement
Definition convex-shape-contact.hh:280
hpp::constraints::ConvexShapeContactComplement::createPair
static std::pair< ConvexShapeContactPtr_t, ConvexShapeContactComplementPtr_t > createPair(const std::string &name, DevicePtr_t robot, const JointAndShapes_t &floorSurfaces, const JointAndShapes_t &objectSurfaces)
hpp::constraints::ConvexShapeContactComplement::ConvexShapeContactComplement
ConvexShapeContactComplement(const std::string &name, DevicePtr_t robot, const JointAndShapes_t &floorSurfaces, const JointAndShapes_t &objectSurfaces)
hpp::constraints::ConvexShapeContactComplement::dependsOnRelPoseBetween
std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition convex-shape-contact.hh:312
hpp::constraints::ConvexShapeContactComplement::computeRelativePoseRightHandSide
void computeRelativePoseRightHandSide(LiegroupElementConstRef rhs, std::size_t &ifloor, std::size_t &iobject, LiegroupElementRef relativePoseRhs) const
hpp::constraints::ConvexShapeContactComplement::isEqual
bool isEqual(const DifferentiableFunction &other) const
hpp::constraints::ConvexShapeContactHold
Definition convex-shape-contact.hh:347
hpp::constraints::ConvexShapeContactHold::isEqual
bool isEqual(const DifferentiableFunction &other) const
hpp::constraints::ConvexShapeContactHold::impl_compute
virtual void impl_compute(LiegroupElementRef result, vectorIn_t argument) const
User implementation of function evaluation.
hpp::constraints::ConvexShapeContactHold::impl_jacobian
virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const
hpp::constraints::ConvexShapeContactHold::create
static ConvexShapeContactHoldPtr_t create(const std::string &name, DevicePtr_t robot, const JointAndShapes_t &floorSurfaces, const JointAndShapes_t &objectSurfaces)
hpp::constraints::ConvexShapeContactHold::contactConstraint
ConvexShapeContactPtr_t contactConstraint() const
Definition convex-shape-contact.hh:361
hpp::constraints::ConvexShapeContactHold::dependsOnRelPoseBetween
std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition convex-shape-contact.hh:372
hpp::constraints::ConvexShapeContactHold::complement
ConvexShapeContactComplementPtr_t complement() const
Definition convex-shape-contact.hh:362
hpp::constraints::ConvexShapeContactHold::ConvexShapeContactHold
ConvexShapeContactHold(const std::string &name, DevicePtr_t robot, const JointAndShapes_t &floorSurfaces, const JointAndShapes_t &objectSurfaces)
hpp::constraints::ConvexShapeContact
Definition convex-shape-contact.hh:94
hpp::constraints::ConvexShapeContact::computeContactPoints
std::vector< ForceData > computeContactPoints(ConfigurationIn_t q, const value_type &normalMargin) const
Compute the contact points.
hpp::constraints::ConvexShapeContact::radius
value_type radius() const
Definition convex-shape-contact.hh:146
hpp::constraints::ConvexShapeContact::create
static ConvexShapeContactPtr_t create(const std::string &name, DevicePtr_t robot, const JointAndShapes_t &floorSurfaces, const JointAndShapes_t &objectSurfaces)
hpp::constraints::ConvexShapeContact::floorContactSurfaces
const ConvexShapes_t & floorContactSurfaces() const
Get vector of floor contact surfaces.
Definition convex-shape-contact.hh:137
hpp::constraints::ConvexShapeContact::dependsOnRelPoseBetween
std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition convex-shape-contact.hh:167
hpp::constraints::ConvexShapeContact::isEqual
bool isEqual(const DifferentiableFunction &other) const
hpp::constraints::ConvexShapeContact::create
static ConvexShapeContactPtr_t create(const DevicePtr_t &robot)
hpp::constraints::ConvexShapeContact::print
std::ostream & print(std::ostream &o) const
Display object in a stream.
hpp::constraints::ConvexShapeContact::objectContactSurfaces
const ConvexShapes_t & objectContactSurfaces() const
Get vector of object contact surfaces.
Definition convex-shape-contact.hh:141
hpp::constraints::ConvexShapeContact::ConvexShapeContact
ConvexShapeContact(const std::string &name, DevicePtr_t robot, const JointAndShapes_t &floorSurfaces, const JointAndShapes_t &objectSurfaces)
hpp::constraints::ConvexShapeContact::ContactType
ContactType
The type of contact between each pair (object shape, floor shape).
Definition convex-shape-contact.hh:104
hpp::constraints::ConvexShapeContact::LINE_ON_PLANE
@ LINE_ON_PLANE
The object shape degenerates to a line,.
Definition convex-shape-contact.hh:108
hpp::constraints::ConvexShapeContact::POINT_ON_PLANE
@ POINT_ON_PLANE
The object shape is a single point,.
Definition convex-shape-contact.hh:106
hpp::constraints::ConvexShapeContact::setNormalMargin
void setNormalMargin(const value_type &margin)
hpp::constraints::ConvexShape
Definition convex-shape.hh:77
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::pinocchio::LiegroupElementBase
hpp::pinocchio::LiegroupElementConstBase
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
convex-shape.hh
deprecated.hh
differentiable-function.hh
fwd.hh
generic-transformation.hh
hpp::constraints::JointAndShapes_t
std::vector< JointAndShape_t > JointAndShapes_t
Definition fwd.hh:128
hpp::constraints::vector3_t
pinocchio::vector3_t vector3_t
Definition fwd.hh:53
hpp::constraints::vector6_t
Eigen::Matrix< value_type, 6, 1 > vector6_t
Definition fwd.hh:77
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:111
hpp::constraints::DeviceConstPtr_t
pinocchio::DeviceConstPtr_t DeviceConstPtr_t
Definition fwd.hh:112
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::ConvexShapeContactPtr_t
shared_ptr< ConvexShapeContact > ConvexShapeContactPtr_t
Definition fwd.hh:129
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::ConvexShapeContactComplementPtr_t
shared_ptr< ConvexShapeContactComplement > ConvexShapeContactComplementPtr_t
Definition fwd.hh:131
hpp::constraints::ConvexShapeContactHoldPtr_t
shared_ptr< ConvexShapeContactHold > ConvexShapeContactHoldPtr_t
Definition fwd.hh:132
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:61
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:57
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:59
hpp::constraints::ConvexShapes_t
std::vector< ConvexShape > ConvexShapes_t
Definition fwd.hh:95
hpp::constraints::JointPtr_t
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:50
hpp::constraints::JointConstPtr_t
pinocchio::JointConstPtr_t JointConstPtr_t
Definition fwd.hh:51
hpp
hpp::constraints::ConvexShapeContact::ForceData
Definition convex-shape-contact.hh:117
hpp::constraints::ConvexShapeContact::ForceData::normal
vector3_t normal
Definition convex-shape-contact.hh:121
hpp::constraints::ConvexShapeContact::ForceData::joint
JointPtr_t joint
Definition convex-shape-contact.hh:118
hpp::constraints::ConvexShapeContact::ForceData::supportJoint
JointPtr_t supportJoint
Definition convex-shape-contact.hh:119
hpp::constraints::ConvexShapeContact::ForceData::points
std::vector< vector3_t > points
Definition convex-shape-contact.hh:120
hpp::pinocchio::DeviceData