hpp-constraints  4.13.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 
159  protected:
165  ConvexShapeContact(const std::string& name, DevicePtr_t robot,
166  const JointAndShapes_t& floorSurfaces,
167  const JointAndShapes_t& objectSurfaces);
168 
169  bool isEqual(const DifferentiableFunction& other) const;
170 
171  private:
173  void addObject(const ConvexShape& t);
174 
179  void addFloor(const ConvexShape& t);
180  void computeRadius();
181 
182  void impl_compute(LiegroupElementRef result,
183  ConfigurationIn_t argument) const;
184  void computeInternalValue(const ConfigurationIn_t& argument, bool& isInside,
185  ContactType& type, vector6_t& value,
186  std::size_t& iobject, std::size_t& ifloor) const;
187 
188  void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
189  void computeInternalJacobian(const ConfigurationIn_t& argument,
190  bool& isInside, ContactType& type,
191  matrix_t& jacobian) const;
192 
197  bool selectConvexShapes(const pinocchio::DeviceData& data,
198  std::size_t& iobject, std::size_t& ifloor) const;
199  ContactType contactType(const ConvexShape& object,
200  const ConvexShape& floor) const;
201 
202  DevicePtr_t robot_;
203  mutable GenericTransformationModel<true> relativeTransformationModel_;
204 
205  ConvexShapes_t objectConvexShapes_;
206  ConvexShapes_t floorConvexShapes_;
207 
208  value_type normalMargin_;
209  // upper bound of distance between center of polygon and vectices for
210  // all floor polygons.
211  value_type M_;
212 };
213 
236  : public DifferentiableFunction {
237  public:
245  static std::pair<ConvexShapeContactPtr_t, ConvexShapeContactComplementPtr_t>
246  createPair(const std::string& name, DevicePtr_t robot,
247  const JointAndShapes_t& floorSurfaces,
248  const JointAndShapes_t& objectSurfaces);
249 
256  void computeRelativePoseRightHandSide(
257  LiegroupElementConstRef rhs, std::size_t& ifloor, std::size_t& iobject,
258  LiegroupElementRef relativePoseRhs) const;
259 
260  protected:
267  ConvexShapeContactComplement(const std::string& name, DevicePtr_t robot,
268  const JointAndShapes_t& floorSurfaces,
269  const JointAndShapes_t& objectSurfaces);
270 
271  bool isEqual(const DifferentiableFunction& other) const;
272 
273  private:
274  void impl_compute(LiegroupElementRef result,
275  ConfigurationIn_t argument) const;
276 
277  void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
278 
279  ConvexShapeContactPtr_t sibling_;
280 }; // class ConvexShapeContactComplement
281 
290  : public DifferentiableFunction {
291  public:
299  static ConvexShapeContactHoldPtr_t create(
300  const std::string& name, DevicePtr_t robot,
301  const JointAndShapes_t& floorSurfaces,
302  const JointAndShapes_t& objectSurfaces);
303 
304  ConvexShapeContactPtr_t contactConstraint() const { return constraint_; }
305  ConvexShapeContactComplementPtr_t complement() const { return complement_; }
306 
307  protected:
315  ConvexShapeContactHold(const std::string& name, DevicePtr_t robot,
316  const JointAndShapes_t& floorSurfaces,
317  const JointAndShapes_t& objectSurfaces);
318 
319  virtual void impl_compute(LiegroupElementRef result,
320  vectorIn_t argument) const;
321  virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const;
322 
323  bool isEqual(const DifferentiableFunction& other) const;
324 
325  private:
326  ConvexShapeContactPtr_t constraint_;
328 }; // class ConvexShapeContactHold
330 } // namespace constraints
331 } // namespace hpp
332 
333 #endif // HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:59
Definition: convex-shape-contact.hh:235
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:108
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:66
ConvexShapeContactPtr_t contactConstraint() const
Definition: convex-shape-contact.hh:304
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:55
JointPtr_t joint
Definition: convex-shape-contact.hh:118
pinocchio::vector3_t vector3_t
Definition: fwd.hh:51
shared_ptr< ConvexShapeContactComplement > ConvexShapeContactComplementPtr_t
Definition: fwd.hh:129
The object shape degenerates to a line,.
Definition: convex-shape-contact.hh:108
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:105
Definition: convex-shape-contact.hh:289
The object shape is a single point,.
Definition: convex-shape-contact.hh:106
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:49
shared_ptr< ConvexShapeContact > ConvexShapeContactPtr_t
Definition: fwd.hh:127
Definition: differentiable-function.hh:63
Definition: convex-shape-contact.hh:93
shared_ptr< ConvexShapeContactHold > ConvexShapeContactHoldPtr_t
Definition: fwd.hh:130
Eigen::Matrix< value_type, 6, 1 > vector6_t
Definition: fwd.hh:75
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:65
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:57
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:305
std::vector< ConvexShape > ConvexShapes_t
Definition: fwd.hh:91
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
Definition: convex-shape-contact.hh:117
const ConvexShapes_t & objectContactSurfaces() const
Get vector of object contact surfaces.
Definition: convex-shape-contact.hh:141
std::list< JointAndShape_t > JointAndShapes_t
Definition: fwd.hh:126