DART  6.10.1
SoftContactConstraint.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
34 #define DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
35 
37 
39 #include "dart/math/MathTypes.hpp"
40 
41 namespace dart {
42 
43 namespace collision {
44 class SoftCollisionInfo;
45 } // namespace collision
46 
47 namespace dynamics {
48 class BodyNode;
49 class SoftBodyNode;
50 class PointMass;
51 class Skeleton;
52 } // namespace dynamics
53 
54 namespace constraint {
55 
58 {
59 public:
61  SoftContactConstraint(collision::Contact& _contact, double _timeStep);
62 
64  virtual ~SoftContactConstraint();
65 
66  // Documentation inherited
67  const std::string& getType() const override;
68 
70  static const std::string& getStaticType();
71 
72  //----------------------------------------------------------------------------
73  // Property settings
74  //----------------------------------------------------------------------------
75 
77  static void setErrorAllowance(double _allowance);
78 
80  static double getErrorAllowance();
81 
83  static void setErrorReductionParameter(double _erp);
84 
86  static double getErrorReductionParameter();
87 
89  static void setMaxErrorReductionVelocity(double _erv);
90 
92  static double getMaxErrorReductionVelocity();
93 
95  static void setConstraintForceMixing(double _cfm);
96 
98  static double getConstraintForceMixing();
99 
101  void setFrictionDirection(const Eigen::Vector3d& _dir);
102 
104  const Eigen::Vector3d& getFrictionDirection1() const;
105 
106  //----------------------------------------------------------------------------
107  // Friendship
108  //----------------------------------------------------------------------------
109 
110  friend class ConstraintSolver;
111  friend class ConstrainedGroup;
112 
113 protected:
114  //----------------------------------------------------------------------------
115  // Constraint virtual functions
116  //----------------------------------------------------------------------------
117 
118  // Documentation inherited
119  void update() override;
120 
121  // Documentation inherited
122  void getInformation(ConstraintInfo* _info) override;
123 
124  // Documentation inherited
125  void applyUnitImpulse(std::size_t _idx) override;
126 
127  // Documentation inherited
128  void getVelocityChange(double* _vel, bool _withCfm) override;
129 
130  // Documentation inherited
131  void excite() override;
132 
133  // Documentation inherited
134  void unexcite() override;
135 
136  // Documentation inherited
137  void applyImpulse(double* _lambda) override;
138 
139  // Documentation inherited
140  dynamics::SkeletonPtr getRootSkeleton() const override;
141 
142  // Documentation inherited
143  void uniteSkeletons() override;
144 
145  // Documentation inherited
146  bool isActive() const override;
147 
148  static double computeFrictionCoefficient(
149  const dynamics::ShapeNode* shapeNode);
150  static double computeRestitutionCoefficient(
151  const dynamics::ShapeNode* shapeNode);
152 
153 private:
157  void getRelVelocity(double* _vel);
158 
161 
163  Eigen::MatrixXd getTangentBasisMatrixODE(const Eigen::Vector3d& _n);
164 
168  dynamics::SoftBodyNode* _softBodyNode,
169  const Eigen::Vector3d& _point,
170  int _faceId) const;
171 
176  const dynamics::SoftBodyNode* _softBodyNode,
177  const Eigen::Vector3d& _point,
178  int _faceId) const;
179 
180 private:
182  double mTimeStep;
183 
186 
189 
192 
195 
198 
201 
202  // TODO(JS): For now, there is only one contact per contact constraint
204  std::vector<collision::Contact*> mContacts;
205 
207  collision::SoftCollisionInfo* mSoftCollInfo;
208 
210  Eigen::Vector3d mFirstFrictionalDirection;
211 
214 
217 
220 
223 
225  Eigen::Vector3d mBodyDirection1;
226 
228  Eigen::Vector3d mBodyDirection2;
229 
232 
234  std::size_t mAppliedImpulseIndex;
235 
238 
240  bool mActive;
241 
243  static double mErrorAllowance;
244 
248 
251 
255  static double mConstraintForceMixing;
256 };
257 
258 } // namespace constraint
259 } // namespace dart
260 
261 #endif // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_
ConstrainedGroup is a group of skeletons that interact each other with constraints.
Definition: ConstrainedGroup.hpp:58
Constraint is a base class of concrete constraints classes.
Definition: ConstraintBase.hpp:75
ConstraintSolver manages constraints and computes constraint impulses.
Definition: ConstraintSolver.hpp:57
SoftContactConstraint represents a contact constraint between two bodies.
Definition: SoftContactConstraint.hpp:58
const Eigen::Vector3d & getFrictionDirection1() const
Get first frictional direction.
Definition: SoftContactConstraint.cpp:432
dynamics::PointMass * selectCollidingPointMass(dynamics::SoftBodyNode *_softBodyNode, const Eigen::Vector3d &_point, int _faceId) const
Find the nearest point mass from _point in a face, of which id is _faceId in _softBodyNode.
Definition: SoftContactConstraint.cpp:1121
static double getErrorReductionParameter()
Get global error reduction parameter.
Definition: SoftContactConstraint.cpp:378
dynamics::PointMass * mPointMass2
Second point mass.
Definition: SoftContactConstraint.hpp:200
Eigen::Vector3d mBodyDirection2
Contact normal expressed in body frame of the second body node.
Definition: SoftContactConstraint.hpp:228
bool mIsBounceOn
Definition: SoftContactConstraint.hpp:237
dynamics::SoftBodyNode * mSoftBodyNode1
First soft body node.
Definition: SoftContactConstraint.hpp:191
dynamics::SoftBodyNode * mSoftBodyNode2
Second soft body node.
Definition: SoftContactConstraint.hpp:194
dynamics::SkeletonPtr getRootSkeleton() const override
Definition: SoftContactConstraint.cpp:984
std::vector< collision::Contact * > mContacts
Contacts between mBodyNode1 and mBodyNode2.
Definition: SoftContactConstraint.hpp:204
static double getConstraintForceMixing()
Get global constraint force mixing parameter.
Definition: SoftContactConstraint.cpp:420
virtual ~SoftContactConstraint()
Destructor.
Definition: SoftContactConstraint.cpp:319
void uniteSkeletons() override
Definition: SoftContactConstraint.cpp:993
common::aligned_vector< Eigen::Vector6d > mJacobians1
Local body jacobians for mBodyNode1.
Definition: SoftContactConstraint.hpp:219
bool mIsFrictionOn
Definition: SoftContactConstraint.hpp:231
double mFrictionCoeff
Coefficient of Friction.
Definition: SoftContactConstraint.hpp:213
double mTimeStep
Time step.
Definition: SoftContactConstraint.hpp:182
void getInformation(ConstraintInfo *_info) override
Fill LCP variables.
Definition: SoftContactConstraint.cpp:447
dynamics::PointMass * mPointMass1
First point mass.
Definition: SoftContactConstraint.hpp:197
void setFrictionDirection(const Eigen::Vector3d &_dir)
Set first frictional direction.
Definition: SoftContactConstraint.cpp:426
static double mErrorReductionParameter
Global constraint error redection parameter in the range of [0, 1].
Definition: SoftContactConstraint.hpp:247
static double mMaxErrorReductionVelocity
Maximum error reduction velocity.
Definition: SoftContactConstraint.hpp:250
void applyImpulse(double *_lambda) override
Apply computed constraint impulse to constrained skeletons.
Definition: SoftContactConstraint.cpp:759
collision::SoftCollisionInfo * mSoftCollInfo
Soft collision information.
Definition: SoftContactConstraint.hpp:207
void applyUnitImpulse(std::size_t _idx) override
Apply unit impulse to constraint space.
Definition: SoftContactConstraint.cpp:607
void unexcite() override
Unexcite the constraint.
Definition: SoftContactConstraint.cpp:749
void getRelVelocity(double *_vel)
Get change in relative velocity at contact point due to external impulse.
Definition: SoftContactConstraint.cpp:915
common::aligned_vector< Eigen::Vector6d > mJacobians2
Local body jacobians for mBodyNode2.
Definition: SoftContactConstraint.hpp:222
static double getMaxErrorReductionVelocity()
Get global error reduction parameter.
Definition: SoftContactConstraint.cpp:399
static void setErrorReductionParameter(double _erp)
Set global error reduction parameter.
Definition: SoftContactConstraint.cpp:358
dynamics::BodyNode * mBodyNode1
Fircst body node.
Definition: SoftContactConstraint.hpp:185
Eigen::MatrixXd getTangentBasisMatrixODE(const Eigen::Vector3d &_n)
Definition: SoftContactConstraint.cpp:1038
void excite() override
Excite the constraint.
Definition: SoftContactConstraint.cpp:739
bool mActive
Definition: SoftContactConstraint.hpp:240
static double getErrorAllowance()
Get global error reduction parameter.
Definition: SoftContactConstraint.cpp:352
Eigen::Vector3d mBodyDirection1
Contact normal expressed in body frame of the first body node.
Definition: SoftContactConstraint.hpp:225
double mRestitutionCoeff
Coefficient of restitution.
Definition: SoftContactConstraint.hpp:216
static void setConstraintForceMixing(double _cfm)
Set global constraint force mixing parameter.
Definition: SoftContactConstraint.cpp:405
static double computeRestitutionCoefficient(const dynamics::ShapeNode *shapeNode)
Definition: SoftContactConstraint.cpp:964
void update() override
Update constraint using updated Skeleton's states.
Definition: SoftContactConstraint.cpp:438
dynamics::BodyNode * mBodyNode2
Second body node.
Definition: SoftContactConstraint.hpp:188
std::size_t mAppliedImpulseIndex
Index of applied impulse.
Definition: SoftContactConstraint.hpp:234
static void setMaxErrorReductionVelocity(double _erv)
Set global error reduction parameter.
Definition: SoftContactConstraint.cpp:384
const std::string & getType() const override
Returns a string representing the constraint type.
Definition: SoftContactConstraint.cpp:324
static double mErrorAllowance
Global constraint error allowance.
Definition: SoftContactConstraint.hpp:243
bool isActive() const override
Return true if this constraint is active.
Definition: SoftContactConstraint.cpp:938
static double mConstraintForceMixing
Global constraint force mixing parameter in the range of [1e-9, 1].
Definition: SoftContactConstraint.hpp:255
Eigen::Vector3d mFirstFrictionalDirection
First frictional direction.
Definition: SoftContactConstraint.hpp:210
SoftContactConstraint(collision::Contact &_contact, double _timeStep)
Constructor.
Definition: SoftContactConstraint.cpp:73
static void setErrorAllowance(double _allowance)
Set global error reduction parameter.
Definition: SoftContactConstraint.cpp:337
static const std::string & getStaticType()
Returns constraint type for this class.
Definition: SoftContactConstraint.cpp:330
void getVelocityChange(double *_vel, bool _withCfm) override
Get velocity change due to the uint impulse.
Definition: SoftContactConstraint.cpp:692
static double computeFrictionCoefficient(const dynamics::ShapeNode *shapeNode)
Definition: SoftContactConstraint.cpp:944
void updateFirstFrictionalDirection()
Definition: SoftContactConstraint.cpp:1024
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
Definition: PointMass.hpp:51
Definition: ShapeNode.hpp:49
SoftBodyNode represent a soft body that has one deformable skin.
Definition: SoftBodyNode.hpp:46
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: Memory.hpp:71
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
Definition: BulletCollisionDetector.cpp:65
Contact information.
Definition: Contact.hpp:45
ConstraintInfo.
Definition: ConstraintBase.hpp:50