DART  6.7.3
JointLimitConstraint.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, 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_JOINTLIMITCONSTRAINT_HPP_
34 #define DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_
35 
37 
38 namespace dart {
39 
40 namespace dynamics {
41 class BodyNode;
42 class Joint;
43 } // namespace dynamics
44 
45 namespace constraint {
46 
48 // TOOD: better naming
50 {
51 public:
53  explicit JointLimitConstraint(dynamics::Joint* _joint);
54 
56  virtual ~JointLimitConstraint();
57 
58  //----------------------------------------------------------------------------
59  // Property settings
60  //----------------------------------------------------------------------------
61 
63  static void setErrorAllowance(double _allowance);
64 
66  static double getErrorAllowance();
67 
69  static void setErrorReductionParameter(double _erp);
70 
72  static double getErrorReductionParameter();
73 
75  static void setMaxErrorReductionVelocity(double _erv);
76 
78  static double getMaxErrorReductionVelocity();
79 
81  static void setConstraintForceMixing(double _cfm);
82 
84  static double getConstraintForceMixing();
85 
86  //----------------------------------------------------------------------------
87  // Friendship
88  //----------------------------------------------------------------------------
89 
90  friend class ConstraintSolver;
91  friend class ConstrainedGroup;
92 
93 protected:
94  //----------------------------------------------------------------------------
95  // Constraint virtual functions
96  //----------------------------------------------------------------------------
97 
98  // Documentation inherited
99  void update() override;
100 
101  // Documentation inherited
102  void getInformation(ConstraintInfo* _lcp) override;
103 
104  // Documentation inherited
105  void applyUnitImpulse(std::size_t _index) override;
106 
107  // Documentation inherited
108  void getVelocityChange(double* _delVel, bool _withCfm) override;
109 
110  // Documentation inherited
111  void excite() override;
112 
113  // Documentation inherited
114  void unexcite() override;
115 
116  // Documentation inherited
117  void applyImpulse(double* _lambda) override;
118 
119  // Documentation inherited
120  dynamics::SkeletonPtr getRootSkeleton() const override;
121 
122  // Documentation inherited
123  bool isActive() const override;
124 
125 private:
128 
131 
133  std::size_t mAppliedImpulseIndex;
134 
136  std::size_t mLifeTime[6];
137 
139  bool mActive[6];
140 
142  double mViolation[6];
143 
145  double mNegativeVel[6];
146 
148  double mOldX[6];
149 
151  double mUpperBound[6];
152 
154  double mLowerBound[6];
155 
157  static double mErrorAllowance;
158 
162 
165 
169  static double mConstraintForceMixing;
170 };
171 
172 } // namespace constraint
173 } // namespace dart
174 
175 #endif // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_
176 
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:56
JointLimitConstraint handles joint position or velocity limits.
Definition: JointLimitConstraint.hpp:50
static void setMaxErrorReductionVelocity(double _erv)
Set global error reduction parameter.
Definition: JointLimitConstraint.cpp:135
double mUpperBound[6]
Definition: JointLimitConstraint.hpp:151
static double getErrorReductionParameter()
Get global error reduction parameter.
Definition: JointLimitConstraint.cpp:129
virtual ~JointLimitConstraint()
Destructor.
Definition: JointLimitConstraint.cpp:83
static double mMaxErrorReductionVelocity
Maximum error reduction velocity.
Definition: JointLimitConstraint.hpp:164
void unexcite() override
Unexcite the constraint.
Definition: JointLimitConstraint.cpp:356
static double mErrorAllowance
Global constraint error allowance.
Definition: JointLimitConstraint.hpp:157
double mLowerBound[6]
Definition: JointLimitConstraint.hpp:154
double mOldX[6]
Definition: JointLimitConstraint.hpp:148
std::size_t mLifeTime[6]
Definition: JointLimitConstraint.hpp:136
void getVelocityChange(double *_delVel, bool _withCfm) override
Get velocity change due to the uint impulse.
Definition: JointLimitConstraint.cpp:319
dynamics::SkeletonPtr getRootSkeleton() const override
Definition: JointLimitConstraint.cpp:381
void applyUnitImpulse(std::size_t _index) override
Apply unit impulse to constraint space.
Definition: JointLimitConstraint.cpp:290
double mViolation[6]
Definition: JointLimitConstraint.hpp:142
static void setErrorReductionParameter(double _erp)
Set global error reduction parameter.
Definition: JointLimitConstraint.cpp:109
double mNegativeVel[6]
Definition: JointLimitConstraint.hpp:145
static void setErrorAllowance(double _allowance)
Set global error reduction parameter.
Definition: JointLimitConstraint.cpp:88
static double getConstraintForceMixing()
Get global constraint force mixing parameter.
Definition: JointLimitConstraint.cpp:176
static double getMaxErrorReductionVelocity()
Get global error reduction parameter.
Definition: JointLimitConstraint.cpp:150
void update() override
Update constraint using updated Skeleton's states.
Definition: JointLimitConstraint.cpp:182
void getInformation(ConstraintInfo *_lcp) override
Fill LCP variables.
Definition: JointLimitConstraint.cpp:241
bool isActive() const override
Return true if this constraint is active.
Definition: JointLimitConstraint.cpp:387
static void setConstraintForceMixing(double _cfm)
Set global constraint force mixing parameter.
Definition: JointLimitConstraint.cpp:156
std::size_t mAppliedImpulseIndex
Index of applied impulse.
Definition: JointLimitConstraint.hpp:133
dynamics::BodyNode * mBodyNode
Definition: JointLimitConstraint.hpp:130
static double getErrorAllowance()
Get global error reduction parameter.
Definition: JointLimitConstraint.cpp:103
static double mConstraintForceMixing
Global constraint force mixing parameter in the range of [1e-9, 1].
Definition: JointLimitConstraint.hpp:169
dynamics::Joint * mJoint
Definition: JointLimitConstraint.hpp:127
static double mErrorReductionParameter
Global constraint error redection parameter in the range of [0, 1].
Definition: JointLimitConstraint.hpp:161
void applyImpulse(double *_lambda) override
Apply computed constraint impulse to constrained skeletons.
Definition: JointLimitConstraint.cpp:362
void excite() override
Excite the constraint.
Definition: JointLimitConstraint.cpp:350
bool mActive[6]
Definition: JointLimitConstraint.hpp:139
JointLimitConstraint(dynamics::Joint *_joint)
Constructor.
Definition: JointLimitConstraint.cpp:58
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:78
class Joint
Definition: Joint.hpp:59
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
Definition: BulletCollisionDetector.cpp:63
ConstraintInfo.
Definition: ConstraintBase.hpp:50