hpp-core  4.13.0
Implement basic classes for canonical path planning for kinematic chains.
solid-solid-collision.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014,2015,2016,2018 CNRS
3 // Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
4 //
5 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29 
30 #ifndef HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH
31 #define HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH
32 
34 
35 namespace hpp {
36 namespace core {
37 namespace continuousValidation {
43 }; // struct CoefficientVelocity
44 typedef std::vector<CoefficientVelocity> CoefficientVelocities_t;
45 
56  public:
63  static SolidSolidCollisionPtr_t create(
64  const JointPtr_t& joint_a, const ConstObjectStdVector_t& objects_b,
65  value_type tolerance);
66 
71  static SolidSolidCollisionPtr_t create(const JointPtr_t& joint_a,
72  const JointPtr_t& joint_b,
73  value_type tolerance);
74 
76  static SolidSolidCollisionPtr_t createCopy(
77  const SolidSolidCollisionPtr_t& other);
78 
79  value_type computeMaximalVelocity(vector_t& Vb) const;
80 
81  bool removeObjectTo_b(const CollisionObjectConstPtr_t& object);
82 
83  std::string name() const;
84 
85  std::ostream& print(std::ostream& os) const;
86 
90  void addCollisionPair(const CollisionObjectConstPtr_t& left,
91  const CollisionObjectConstPtr_t& right);
92 
93  // Get coefficients and joints
95  return m_->coefficients;
96  }
97 
99  const JointPtr_t& joint_a() const { return m_->joint_a; }
101  const JointPtr_t& joint_b() const { return m_->joint_b; }
102 
105  return (m_->joint_a ? m_->joint_a->index() : 0);
106  }
109  return (m_->joint_b ? m_->joint_b->index() : 0);
110  }
111 
112  IntervalValidationPtr_t copy() const;
113 
114  protected:
120  SolidSolidCollision(const JointPtr_t& joint_a, const JointPtr_t& joint_b,
121  value_type tolerance);
122 
129  SolidSolidCollision(const JointPtr_t& joint_a,
130  const ConstObjectStdVector_t& objects_b,
131  value_type tolerance);
132 
135  : BodyPairCollision(other), m_(other.m_) {}
136 
137  void init(const SolidSolidCollisionWkPtr_t& weak);
138 
139  private:
140  typedef pinocchio::JointIndex JointIndex;
141  typedef std::vector<JointIndex> JointIndices_t;
142 
143  struct Model {
144  JointPtr_t joint_a;
145  JointPtr_t joint_b;
146  CoefficientVelocities_t coefficients;
147  CoefficientVelocities_t coefficients_reverse;
148  JointIndices_t computeSequenceOfJoints() const;
149  CoefficientVelocities_t computeCoefficients(
150  const JointIndices_t& joints) const;
151  void setCoefficients(const JointIndices_t& joints);
152  };
153  shared_ptr<Model> m_;
154  SolidSolidCollisionWkPtr_t weak_;
155 }; // class SolidSolidCollision
156 } // namespace continuousValidation
157 } // namespace core
158 } // namespace hpp
159 #endif // HPP_CORE_CONTINUOUS_VALIDATION_SOLID_SOLID_COLLISION_HH
JointPtr_t joint_
Joint the degrees of freedom of which the bounds correspond to.
Definition: solid-solid-collision.hh:41
Definition: bi-rrt-planner.hh:35
std::vector< CollisionObjectConstPtr_t > ConstObjectStdVector_t
Definition: fwd.hh:174
pinocchio::size_type size_type
Definition: fwd.hh:162
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:140
Definition: solid-solid-collision.hh:38
size_type indexJointA() const
Returns joint A index or -1 if no such joint exists.
Definition: solid-solid-collision.hh:104
const CoefficientVelocities_t & coefficients() const
Definition: solid-solid-collision.hh:94
const JointPtr_t & joint_b() const
Get joint b.
Definition: solid-solid-collision.hh:101
value_type value_
Definition: solid-solid-collision.hh:42
shared_ptr< SolidSolidCollision > SolidSolidCollisionPtr_t
Definition: fwd.hh:253
pinocchio::vector_t vector_t
Definition: fwd.hh:209
pinocchio::value_type value_type
Definition: fwd.hh:163
std::vector< CoefficientVelocity > CoefficientVelocities_t
Definition: solid-solid-collision.hh:44
SolidSolidCollision(const SolidSolidCollision &other)
Copy constructor.
Definition: solid-solid-collision.hh:134
const JointPtr_t & joint_a() const
Get joint a.
Definition: solid-solid-collision.hh:99
Definition: body-pair-collision.hh:62
size_type indexJointB() const
Returns joint B index or -1 if no such joint exists.
Definition: solid-solid-collision.hh:108
CoefficientVelocity()
Definition: solid-solid-collision.hh:39
Definition: solid-solid-collision.hh:55
shared_ptr< IntervalValidation > IntervalValidationPtr_t
Definition: fwd.hh:250
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:99