DART  6.10.1
JointLimitConstraint.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_JOINTLIMITCONSTRAINT_HPP_
34 #define DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_
35 
36 #include <Eigen/Dense>
37 
39 
40 namespace dart {
41 
42 namespace dynamics {
43 class BodyNode;
44 class Joint;
45 } // namespace dynamics
46 
47 namespace constraint {
48 
51 {
52 public:
54  explicit JointLimitConstraint(dynamics::Joint* joint);
55 
57  ~JointLimitConstraint() 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 
93  //----------------------------------------------------------------------------
94  // Friendship
95  //----------------------------------------------------------------------------
96 
97  friend class ConstraintSolver;
98  friend class ConstrainedGroup;
99 
100 protected:
101  //----------------------------------------------------------------------------
102  // Constraint virtual functions
103  //----------------------------------------------------------------------------
104 
105  // Documentation inherited
106  void update() override;
107 
108  // Documentation inherited
109  void getInformation(ConstraintInfo* lcp) override;
110 
111  // Documentation inherited
112  void applyUnitImpulse(std::size_t index) override;
113 
114  // Documentation inherited
115  void getVelocityChange(double* delVel, bool withCfm) override;
116 
117  // Documentation inherited
118  void excite() override;
119 
120  // Documentation inherited
121  void unexcite() override;
122 
123  // Documentation inherited
124  void applyImpulse(double* lambda) override;
125 
126  // Documentation inherited
127  dynamics::SkeletonPtr getRootSkeleton() const override;
128 
129  // Documentation inherited
130  bool isActive() const override;
131 
132 private:
135 
138 
140  std::size_t mAppliedImpulseIndex;
141 
143  Eigen::Matrix<std::size_t, 6, 1> mLifeTime;
144 
146  Eigen::Matrix<bool, 6, 1> mIsPositionLimitViolated;
147 
149  Eigen::Matrix<bool, 6, 1> mIsVelocityLimitViolated;
150 
152  Eigen::Matrix<double, 6, 1> mViolation;
153 
155  Eigen::Matrix<double, 6, 1> mNegativeVel;
156 
158  Eigen::Matrix<double, 6, 1> mOldX;
159 
161  Eigen::Matrix<double, 6, 1> mUpperBound;
162 
164  Eigen::Matrix<double, 6, 1> mLowerBound;
165 
167  static double mErrorAllowance;
168 
172 
175 
179  static double mConstraintForceMixing;
180 };
181 
182 } // namespace constraint
183 } // namespace dart
184 
185 #endif // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_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
JointLimitConstraint handles joint position and velocity limits.
Definition: JointLimitConstraint.hpp:51
static double getErrorReductionParameter()
Get global error reduction parameter.
Definition: JointLimitConstraint.cpp:128
static double mMaxErrorReductionVelocity
Maximum error reduction velocity.
Definition: JointLimitConstraint.hpp:174
void unexcite() override
Unexcite the constraint.
Definition: JointLimitConstraint.cpp:430
static double mErrorAllowance
Global constraint error allowance.
Definition: JointLimitConstraint.hpp:167
const std::string & getType() const override
Returns a string representing the constraint type.
Definition: JointLimitConstraint.cpp:74
Eigen::Matrix< bool, 6, 1 > mIsVelocityLimitViolated
Whether joint value exceeds the velocity limit.
Definition: JointLimitConstraint.hpp:149
void getVelocityChange(double *delVel, bool withCfm) override
Get velocity change due to the uint impulse.
Definition: JointLimitConstraint.cpp:390
Eigen::Matrix< double, 6, 1 > mViolation
How much current joint values are exceeded from the limits.
Definition: JointLimitConstraint.hpp:152
Eigen::Matrix< bool, 6, 1 > mIsPositionLimitViolated
Whether joint value exceeds the position limit.
Definition: JointLimitConstraint.hpp:146
Eigen::Matrix< std::size_t, 6, 1 > mLifeTime
Life time of constraint of each DOF.
Definition: JointLimitConstraint.hpp:143
JointLimitConstraint(dynamics::Joint *joint)
Constructor.
Definition: JointLimitConstraint.cpp:58
void getInformation(ConstraintInfo *lcp) override
Fill LCP variables.
Definition: JointLimitConstraint.cpp:289
dynamics::SkeletonPtr getRootSkeleton() const override
Definition: JointLimitConstraint.cpp:458
Eigen::Matrix< double, 6, 1 > mUpperBound
Upper limit of the constraint impulse.
Definition: JointLimitConstraint.hpp:161
static double getConstraintForceMixing()
Get global constraint force mixing parameter.
Definition: JointLimitConstraint.cpp:170
static double getMaxErrorReductionVelocity()
Get global error reduction parameter.
Definition: JointLimitConstraint.cpp:149
Eigen::Matrix< double, 6, 1 > mOldX
Constraint impulse of the previous step.
Definition: JointLimitConstraint.hpp:158
static void setErrorAllowance(double allowance)
Set global error reduction parameter.
Definition: JointLimitConstraint.cpp:87
void update() override
Update constraint using updated Skeleton's states.
Definition: JointLimitConstraint.cpp:176
bool isActive() const override
Return true if this constraint is active.
Definition: JointLimitConstraint.cpp:464
static const std::string & getStaticType()
Returns constraint type for this class.
Definition: JointLimitConstraint.cpp:80
void applyUnitImpulse(std::size_t index) override
Apply unit impulse to constraint space.
Definition: JointLimitConstraint.cpp:358
static void setConstraintForceMixing(double cfm)
Set global constraint force mixing parameter.
Definition: JointLimitConstraint.cpp:155
Eigen::Matrix< double, 6, 1 > mLowerBound
Lower limit of the constraint impulse.
Definition: JointLimitConstraint.hpp:164
std::size_t mAppliedImpulseIndex
Index of applied impulse.
Definition: JointLimitConstraint.hpp:140
dynamics::BodyNode * mBodyNode
The child BodyNode of the Joint that this constraint is associated with.
Definition: JointLimitConstraint.hpp:137
static double getErrorAllowance()
Get global error reduction parameter.
Definition: JointLimitConstraint.cpp:102
static double mConstraintForceMixing
Global constraint force mixing parameter in the range of [1e-9, 1].
Definition: JointLimitConstraint.hpp:179
dynamics::Joint * mJoint
The Joint that this constraint is associated with.
Definition: JointLimitConstraint.hpp:134
void applyImpulse(double *lambda) override
Apply computed constraint impulse to constrained skeletons.
Definition: JointLimitConstraint.cpp:436
static double mErrorReductionParameter
Global constraint error redection parameter in the range of [0, 1].
Definition: JointLimitConstraint.hpp:171
static void setMaxErrorReductionVelocity(double erv)
Set global error reduction parameter.
Definition: JointLimitConstraint.cpp:134
Eigen::Matrix< double, 6, 1 > mNegativeVel
The desired delta velocity to satisfy the joint limit constraint.
Definition: JointLimitConstraint.hpp:155
~JointLimitConstraint() override=default
Destructor.
static void setErrorReductionParameter(double erp)
Set global error reduction parameter.
Definition: JointLimitConstraint.cpp:108
void excite() override
Excite the constraint.
Definition: JointLimitConstraint.cpp:424
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
class Joint
Definition: Joint.hpp:60
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
Definition: BulletCollisionDetector.cpp:65
ConstraintInfo.
Definition: ConstraintBase.hpp:50