DART  6.10.1
ContactConstraint.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_CONTACTCONSTRAINT_HPP_
34 #define DART_CONSTRAINT_CONTACTCONSTRAINT_HPP_
35 
38 #include "dart/math/MathTypes.hpp"
39 
40 namespace dart {
41 
42 namespace dynamics {
43 class BodyNode;
44 class Skeleton;
45 } // namespace dynamics
46 
47 namespace constraint {
48 
51 {
52 public:
54  ContactConstraint(collision::Contact& contact, double timeStep);
55 
57  ~ContactConstraint() override = default;
58 
59  // Documentation inherited
60  const std::string& getType() const override;
61 
63  static const std::string& getStaticType();
64 
65  //----------------------------------------------------------------------------
66  // Property settings
67  //----------------------------------------------------------------------------
68 
70  static void setErrorAllowance(double allowance);
71 
73  static double getErrorAllowance();
74 
76  static void setErrorReductionParameter(double erp);
77 
79  static double getErrorReductionParameter();
80 
82  static void setMaxErrorReductionVelocity(double erv);
83 
85  static double getMaxErrorReductionVelocity();
86 
88  static void setConstraintForceMixing(double cfm);
89 
91  static double getConstraintForceMixing();
92 
94  void setFrictionDirection(const Eigen::Vector3d& dir);
95 
97  const Eigen::Vector3d& getFrictionDirection1() const;
98 
99  //----------------------------------------------------------------------------
100  // Friendship
101  //----------------------------------------------------------------------------
102 
103  friend class ConstraintSolver;
104  friend class ConstrainedGroup;
105 
106 protected:
107  //----------------------------------------------------------------------------
108  // Constraint virtual functions
109  //----------------------------------------------------------------------------
110 
111  // Documentation inherited
112  void update() override;
113 
114  // Documentation inherited
115  void getInformation(ConstraintInfo* info) override;
116 
117  // Documentation inherited
118  void applyUnitImpulse(std::size_t index) override;
119 
120  // Documentation inherited
121  void getVelocityChange(double* vel, bool withCfm) override;
122 
123  // Documentation inherited
124  void excite() override;
125 
126  // Documentation inherited
127  void unexcite() override;
128 
129  // Documentation inherited
130  void applyImpulse(double* lambda) override;
131 
132  // Documentation inherited
133  dynamics::SkeletonPtr getRootSkeleton() const override;
134 
135  // Documentation inherited
136  void uniteSkeletons() override;
137 
138  // Documentation inherited
139  bool isActive() const override;
140 
141  static double computeFrictionCoefficient(
142  const dynamics::ShapeNode* shapeNode);
143  static double computePrimaryFrictionCoefficient(
144  const dynamics::ShapeNode* shapeNode);
146  const dynamics::ShapeNode* shapeNode);
147  static double computePrimarySlipCompliance(
148  const dynamics::ShapeNode* shapeNode);
149  static double computeSecondarySlipCompliance(
150  const dynamics::ShapeNode* shapeNode);
151  static Eigen::Vector3d computeWorldFirstFrictionDir(
152  const dynamics::ShapeNode* shapenode);
153  static double computeRestitutionCoefficient(
154  const dynamics::ShapeNode* shapeNode);
155 
156 private:
157  using TangentBasisMatrix = Eigen::Matrix<double, 3, 2>;
158 
162  void getRelVelocity(double* relVel);
163 
166 
168  TangentBasisMatrix getTangentBasisMatrixODE(const Eigen::Vector3d& n);
169 
170  // The following functions for getting and setting slip compliance and
171  // accessing the contact object are meant to be used by ConstraintSolver to
172  // update the slip compliances based on the number of contacts between the
173  // collision objects.
174  //
176  double getPrimarySlipCompliance() const;
177 
179  void setPrimarySlipCompliance(double slip);
180 
182  double getSecondarySlipCompliance() const;
183 
185  void setSecondarySlipCompliance(double slip);
186 
188  const collision::Contact& getContact() const;
189 
190 private:
192  double mTimeStep;
193 
196 
199 
202 
204  Eigen::Vector3d mFirstFrictionalDirection;
205 
208 
211 
214 
217 
220 
223 
225  Eigen::Matrix<double, 6, Eigen::Dynamic> mSpatialNormalA;
226 
228  Eigen::Matrix<double, 6, Eigen::Dynamic> mSpatialNormalB;
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 // TODO(JS): Create SelfContactConstraint.
258 
259 } // namespace constraint
260 } // namespace dart
261 
262 #endif // DART_CONSTRAINT_CONTACTCONSTRAINT_HPP_
std::size_t index
Definition: SkelParser.cpp:1672
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
ContactConstraint represents a contact constraint between two bodies.
Definition: ContactConstraint.hpp:51
static const std::string & getStaticType()
Returns constraint type for this class.
Definition: ContactConstraint.cpp:311
double mTimeStep
Time step.
Definition: ContactConstraint.hpp:192
const Eigen::Vector3d & getFrictionDirection1() const
Get first frictional direction.
Definition: ContactConstraint.cpp:413
void setFrictionDirection(const Eigen::Vector3d &dir)
Set first frictional direction.
Definition: ContactConstraint.cpp:407
bool mIsFrictionOn
Definition: ContactConstraint.hpp:231
void update() override
Update constraint using updated Skeleton's states.
Definition: ContactConstraint.cpp:419
static void setErrorAllowance(double allowance)
Set global error reduction parameter.
Definition: ContactConstraint.cpp:318
void excite() override
Excite the constraint.
Definition: ContactConstraint.cpp:662
static double mMaxErrorReductionVelocity
Maximum error reduction velocity.
Definition: ContactConstraint.hpp:250
double getPrimarySlipCompliance() const
Get primary slip compliance.
Definition: ContactConstraint.cpp:1033
bool mActive
Definition: ContactConstraint.hpp:240
bool isActive() const override
Return true if this constraint is active.
Definition: ContactConstraint.cpp:750
static void setErrorReductionParameter(double erp)
Set global error reduction parameter.
Definition: ContactConstraint.cpp:339
void applyImpulse(double *lambda) override
Apply computed constraint impulse to constrained skeletons.
Definition: ContactConstraint.cpp:682
double mPrimarySlipCompliance
Primary Coefficient of Slip Compliance.
Definition: ContactConstraint.hpp:213
static void setMaxErrorReductionVelocity(double erv)
Set global error reduction parameter.
Definition: ContactConstraint.cpp:365
static double getConstraintForceMixing()
Get global constraint force mixing parameter.
Definition: ContactConstraint.cpp:401
static double computeRestitutionCoefficient(const dynamics::ShapeNode *shapeNode)
Definition: ContactConstraint.cpp:899
static double computePrimarySlipCompliance(const dynamics::ShapeNode *shapeNode)
Definition: ContactConstraint.cpp:818
Eigen::Matrix< double, 6, Eigen::Dynamic > mSpatialNormalB
Local body jacobians for mBodyNode2.
Definition: ContactConstraint.hpp:228
const collision::Contact & getContact() const
Get contact object associated witht this constraint.
Definition: ContactConstraint.cpp:1057
double mSecondarySlipCompliance
Secondary Coefficient of Slip Compliance.
Definition: ContactConstraint.hpp:216
void updateFirstFrictionalDirection()
Definition: ContactConstraint.cpp:930
void setPrimarySlipCompliance(double slip)
Set primary slip compliance.
Definition: ContactConstraint.cpp:1039
void getInformation(ConstraintInfo *info) override
Fill LCP variables.
Definition: ContactConstraint.cpp:428
static double computeSecondarySlipCompliance(const dynamics::ShapeNode *shapeNode)
Definition: ContactConstraint.cpp:843
static double mErrorReductionParameter
Global constraint error redection parameter in the range of [0, 1].
Definition: ContactConstraint.hpp:247
static double getErrorAllowance()
Get global error reduction parameter.
Definition: ContactConstraint.cpp:333
static double computeFrictionCoefficient(const dynamics::ShapeNode *shapeNode)
Definition: ContactConstraint.cpp:756
static double computeSecondaryFrictionCoefficient(const dynamics::ShapeNode *shapeNode)
Definition: ContactConstraint.cpp:797
collision::Contact & mContact
Contact between mBodyNode1 and mBodyNode2.
Definition: ContactConstraint.hpp:201
void getRelVelocity(double *relVel)
Get change in relative velocity at contact point due to external impulse.
Definition: ContactConstraint.cpp:739
double mPrimaryFrictionCoeff
Primary Coefficient of Friction.
Definition: ContactConstraint.hpp:207
TangentBasisMatrix getTangentBasisMatrixODE(const Eigen::Vector3d &n)
Definition: ContactConstraint.cpp:944
const std::string & getType() const override
Returns a string representing the constraint type.
Definition: ContactConstraint.cpp:305
static double getMaxErrorReductionVelocity()
Get global error reduction parameter.
Definition: ContactConstraint.cpp:380
std::size_t mAppliedImpulseIndex
Index of applied impulse.
Definition: ContactConstraint.hpp:234
dynamics::BodyNode * mBodyNodeA
Fircst body node.
Definition: ContactConstraint.hpp:195
void applyUnitImpulse(std::size_t index) override
Apply unit impulse to constraint space.
Definition: ContactConstraint.cpp:558
void unexcite() override
Unexcite the constraint.
Definition: ContactConstraint.cpp:672
~ContactConstraint() override=default
Destructor.
void getVelocityChange(double *vel, bool withCfm) override
Get velocity change due to the uint impulse.
Definition: ContactConstraint.cpp:628
static double getErrorReductionParameter()
Get global error reduction parameter.
Definition: ContactConstraint.cpp:359
static double computePrimaryFrictionCoefficient(const dynamics::ShapeNode *shapeNode)
Definition: ContactConstraint.cpp:776
static double mErrorAllowance
Global constraint error allowance.
Definition: ContactConstraint.hpp:243
double getSecondarySlipCompliance() const
Get secondary slip compliance.
Definition: ContactConstraint.cpp:1045
bool mIsBounceOn
Definition: ContactConstraint.hpp:237
Eigen::Matrix< double, 6, Eigen::Dynamic > mSpatialNormalA
Local body jacobians for mBodyNode1.
Definition: ContactConstraint.hpp:225
double mRestitutionCoeff
Coefficient of restitution.
Definition: ContactConstraint.hpp:219
void uniteSkeletons() override
Definition: ContactConstraint.cpp:1002
static Eigen::Vector3d computeWorldFirstFrictionDir(const dynamics::ShapeNode *shapenode)
Definition: ContactConstraint.cpp:869
Eigen::Matrix< double, 3, 2 > TangentBasisMatrix
Definition: ContactConstraint.hpp:157
static double mConstraintForceMixing
Global constraint force mixing parameter in the range of [1e-9, 1].
Definition: ContactConstraint.hpp:255
dynamics::BodyNode * mBodyNodeB
Second body node.
Definition: ContactConstraint.hpp:198
dynamics::SkeletonPtr getRootSkeleton() const override
Definition: ContactConstraint.cpp:919
bool mIsSelfCollision
Whether this contact is self-collision.
Definition: ContactConstraint.hpp:222
ContactConstraint(collision::Contact &contact, double timeStep)
Constructor.
Definition: ContactConstraint.cpp:72
double mSecondaryFrictionCoeff
Primary Coefficient of Friction.
Definition: ContactConstraint.hpp:210
Eigen::Vector3d mFirstFrictionalDirection
First frictional direction.
Definition: ContactConstraint.hpp:204
static void setConstraintForceMixing(double cfm)
Set global constraint force mixing parameter.
Definition: ContactConstraint.cpp:386
void setSecondarySlipCompliance(double slip)
Set secondary slip compliance.
Definition: ContactConstraint.cpp:1051
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
Definition: ShapeNode.hpp:49
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
Definition: BulletCollisionDetector.cpp:65
Contact information.
Definition: Contact.hpp:45
ConstraintInfo.
Definition: ConstraintBase.hpp:50