hpp-constraints  4.14.0
Definition of basic geometric constraints for motion planning
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 
36 #include <hpp/constraints/fwd.hh>
38 #include <vector>
39 
40 namespace hpp {
41 namespace constraints {
42 
45 
94  : public DifferentiableFunction {
95  public:
97  friend class ConvexShapeContactHold;
98 
100  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
102 
104  enum ContactType {
111  PLANE_ON_PLANE
112  };
113 
117  struct ForceData {
120  std::vector<vector3_t> points;
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 
138  return floorConvexShapes_;
139  }
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 
280  : public DifferentiableFunction {
281  public:
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 
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_;
398 }; // class ConvexShapeContactHold
400 } // namespace constraints
401 } // namespace hpp
402 
403 #endif // HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
Definition: convex-shape-contact.hh:279
value_type radius() const
Definition: convex-shape-contact.hh:146
Definition: active-set-differentiable-function.hh:36
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:109
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:67
std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition: convex-shape-contact.hh:312
ConvexShapeContactPtr_t contactConstraint() const
Definition: convex-shape-contact.hh:361
ContactType
The type of contact between each pair (object shape, floor shape).
Definition: convex-shape-contact.hh:104
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
JointPtr_t joint
Definition: convex-shape-contact.hh:118
pinocchio::vector3_t vector3_t
Definition: fwd.hh:52
shared_ptr< ConvexShapeContactComplement > ConvexShapeContactComplementPtr_t
Definition: fwd.hh:131
The object shape degenerates to a line,.
Definition: convex-shape-contact.hh:108
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
Definition: convex-shape-contact.hh:346
The object shape is a single point,.
Definition: convex-shape-contact.hh:106
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:49
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:50
shared_ptr< ConvexShapeContact > ConvexShapeContactPtr_t
Definition: fwd.hh:129
Definition: differentiable-function.hh:63
Definition: convex-shape-contact.hh:93
shared_ptr< ConvexShapeContactHold > ConvexShapeContactHoldPtr_t
Definition: fwd.hh:132
std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition: convex-shape-contact.hh:372
Eigen::Matrix< value_type, 6, 1 > vector6_t
Definition: fwd.hh:76
std::vector< JointAndShape_t > JointAndShapes_t
Definition: fwd.hh:128
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:66
pinocchio::size_type size_type
Definition: fwd.hh:47
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:58
std::vector< vector3_t > points
Definition: convex-shape-contact.hh:120
const ConvexShapes_t & floorContactSurfaces() const
Get vector of floor contact surfaces.
Definition: convex-shape-contact.hh:137
ConvexShapeContactComplementPtr_t complement() const
Definition: convex-shape-contact.hh:362
std::vector< ConvexShape > ConvexShapes_t
Definition: fwd.hh:92
pinocchio::DeviceConstPtr_t DeviceConstPtr_t
Definition: fwd.hh:110
Definition: convex-shape.hh:73
pinocchio::value_type value_type
Definition: fwd.hh:48
JointPtr_t supportJoint
Definition: convex-shape-contact.hh:119
vector3_t normal
Definition: convex-shape-contact.hh:121
std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition: convex-shape-contact.hh:167
Definition: convex-shape-contact.hh:117
const ConvexShapes_t & objectContactSurfaces() const
Get vector of object contact surfaces.
Definition: convex-shape-contact.hh:141