hpp-constraints  4.11.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 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
18 # define HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
19 
20 # include <vector>
21 
22 # include <hpp/constraints/fwd.hh>
23 # include <hpp/constraints/config.hh>
28 
29 namespace hpp {
30  namespace constraints {
31 
34 
78  public DifferentiableFunction {
79  public:
81  friend class ConvexShapeContactHold;
82 
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86 
88  enum ContactType {
94  PLANE_ON_PLANE
95  };
96 
100  struct ForceData {
103  std::vector<vector3_t> points;
105  };
106 
112  static ConvexShapeContactPtr_t create
113  (const std::string& name, DevicePtr_t robot,
114  const JointAndShapes_t& floorSurfaces,
115  const JointAndShapes_t& objectSurfaces);
116 
117  static ConvexShapeContactPtr_t create (
118  const DevicePtr_t& robot);
119 
122  {
123  return floorConvexShapes_;
124  }
127  {
128  return objectConvexShapes_;
129  }
133  {
134  return M_;
135  }
139  void setNormalMargin (const value_type& margin);
140 
142  std::vector <ForceData> computeContactPoints (ConfigurationIn_t q,
143  const value_type& normalMargin) const;
144 
146  std::ostream& print (std::ostream& o) const;
147  protected:
153  ConvexShapeContact (const std::string& name, DevicePtr_t robot,
154  const JointAndShapes_t& floorSurfaces,
155  const JointAndShapes_t& objectSurfaces);
156 
157 
158  private:
160  void addObject (const ConvexShape& t);
161 
166  void addFloor (const ConvexShape& t);
167  void computeRadius();
168 
169  void impl_compute (LiegroupElementRef result, ConfigurationIn_t argument)
170  const;
171  void computeInternalValue (const ConfigurationIn_t& argument,
172  bool& isInside, ContactType& type, vector6_t& value,
173  std::size_t& iobject, std::size_t& ifloor) const;
174 
175  void impl_jacobian (matrixOut_t jacobian, ConfigurationIn_t argument) const;
176  void computeInternalJacobian (const ConfigurationIn_t& argument,
177  bool& isInside, ContactType& type, matrix_t& jacobian) const;
178 
183  bool selectConvexShapes (const pinocchio::DeviceData& data,
184  std::size_t& iobject, std::size_t& ifloor)
185  const;
186  ContactType contactType (const ConvexShape& object,
187  const ConvexShape& floor) const;
188 
189  DevicePtr_t robot_;
190  mutable GenericTransformationModel<true> relativeTransformationModel_;
191 
192  ConvexShapes_t objectConvexShapes_;
193  ConvexShapes_t floorConvexShapes_;
194 
195  value_type normalMargin_;
196  // upper bound of distance between center of polygon and vectices for
197  // all floor polygons.
198  value_type M_;
199  };
200 
223  {
224  public:
232  static std::pair <ConvexShapeContactPtr_t,
234  createPair(const std::string& name, DevicePtr_t robot,
235  const JointAndShapes_t& floorSurfaces,
236  const JointAndShapes_t& objectSurfaces);
237 
244  void computeRelativePoseRightHandSide
245  (LiegroupElementConstRef rhs, std::size_t& ifloor, std::size_t& iobject,
246  LiegroupElementRef relativePoseRhs) const;
247  protected:
254  ConvexShapeContactComplement (const std::string& name, DevicePtr_t robot,
255  const JointAndShapes_t& floorSurfaces,
256  const JointAndShapes_t& objectSurfaces);
257 
258 
259  private:
260  void impl_compute (LiegroupElementRef result, ConfigurationIn_t argument) const;
261 
262  void impl_jacobian (matrixOut_t jacobian, ConfigurationIn_t argument)
263  const;
264 
265  ConvexShapeContactPtr_t sibling_;
266  }; // class ConvexShapeContactComplement
267 
277  {
278  public:
286  static ConvexShapeContactHoldPtr_t create
287  (const std::string& name, DevicePtr_t robot,
288  const JointAndShapes_t& floorSurfaces,
289  const JointAndShapes_t& objectSurfaces);
290 
292  {
293  return constraint_;
294  }
296  {
297  return complement_;
298  }
299 
300  protected:
309  (const std::string& name, DevicePtr_t robot,
310  const JointAndShapes_t& floorSurfaces,
311  const JointAndShapes_t& objectSurfaces);
312 
313  virtual void impl_compute(LiegroupElementRef result, vectorIn_t argument)
314  const;
315  virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg)
316  const;
317  private:
318  ConvexShapeContactPtr_t constraint_;
320  }; // class ConvexShapeContactHold
322  } // namespace constraints
323 } // namespace hpp
324 
325 #endif // HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
Definition: convex-shape-contact.hh:221
value_type radius() const
Definition: convex-shape-contact.hh:132
Definition: active-set-differentiable-function.hh:24
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:55
ConvexShapeContactPtr_t contactConstraint() const
Definition: convex-shape-contact.hh:291
ContactType
The type of contact between each pair (object shape, floor shape).
Definition: convex-shape-contact.hh:88
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
JointPtr_t joint
Definition: convex-shape-contact.hh:101
pinocchio::vector3_t vector3_t
Definition: fwd.hh:40
shared_ptr< ConvexShapeContactComplement > ConvexShapeContactComplementPtr_t
Definition: fwd.hh:122
The object shape degenerates to a line,.
Definition: convex-shape-contact.hh:92
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
Definition: convex-shape-contact.hh:275
The object shape is a single point,.
Definition: convex-shape-contact.hh:90
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:38
shared_ptr< ConvexShapeContact > ConvexShapeContactPtr_t
Definition: fwd.hh:120
Definition: differentiable-function.hh:52
Definition: convex-shape-contact.hh:77
shared_ptr< ConvexShapeContactHold > ConvexShapeContactHoldPtr_t
Definition: fwd.hh:124
Eigen::Matrix< value_type, 6, 1 > vector6_t
Definition: fwd.hh:64
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
std::vector< vector3_t > points
Definition: convex-shape-contact.hh:103
const ConvexShapes_t & floorContactSurfaces() const
Get vector of floor contact surfaces.
Definition: convex-shape-contact.hh:121
ConvexShapeContactComplementPtr_t complement() const
Definition: convex-shape-contact.hh:295
std::vector< ConvexShape > ConvexShapes_t
Definition: fwd.hh:80
Definition: convex-shape.hh:64
pinocchio::value_type value_type
Definition: fwd.hh:37
JointPtr_t supportJoint
Definition: convex-shape-contact.hh:102
vector3_t normal
Definition: convex-shape-contact.hh:104
Definition: convex-shape-contact.hh:100
const ConvexShapes_t & objectContactSurfaces() const
Get vector of object contact surfaces.
Definition: convex-shape-contact.hh:126
std::list< JointAndShape_t > JointAndShapes_t
Definition: fwd.hh:118