DART 6.8.5
Loading...
Searching...
No Matches
ConstraintSolver.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_CONSTRAINTSOVER_HPP_
34#define DART_CONSTRAINT_CONSTRAINTSOVER_HPP_
35
36#include <vector>
37
38#include <Eigen/Dense>
39
45
46namespace dart {
47
48namespace dynamics {
49class Skeleton;
50class ShapeNodeCollisionObject;
51} // namespace dynamics
52
53namespace constraint {
54
57{
58public:
65 explicit ConstraintSolver(double timeStep);
66
67 // TODO(JS): Remove timeStep. The timestep can be set by world when a
68 // constraint solver is assigned to a world.
69 // Deprecate
70
73
75 // TODO: implement copy constructor since this class contains a pointer to
76 // allocated memory.
77 ConstraintSolver(const ConstraintSolver& other) = delete;
78
80 virtual ~ConstraintSolver() = default;
81
83 void addSkeleton(const dynamics::SkeletonPtr& skeleton);
84
86 void addSkeletons(const std::vector<dynamics::SkeletonPtr>& skeletons);
87
89 const std::vector<dynamics::SkeletonPtr>& getSkeletons() const;
90
92 void removeSkeleton(const dynamics::SkeletonPtr& skeleton);
93
95 void removeSkeletons(const std::vector<dynamics::SkeletonPtr>& skeletons);
96
98 void removeAllSkeletons();
99
101 void addConstraint(const ConstraintBasePtr& constraint);
102
104 void removeConstraint(const ConstraintBasePtr& constraint);
105
108
110 std::size_t getNumConstraints() const;
111
114
117
119 std::vector<constraint::ConstraintBasePtr> getConstraints();
120
122 std::vector<constraint::ConstConstraintBasePtr> getConstraints() const;
123
126
128 virtual void setTimeStep(double _timeStep);
129
131 double getTimeStep() const;
132
136 DART_DEPRECATED(6.0)
137 void setCollisionDetector(collision::CollisionDetector* collisionDetector);
138
141 const std::shared_ptr<collision::CollisionDetector>& collisionDetector);
142
144 collision::CollisionDetectorPtr getCollisionDetector();
145
147 collision::ConstCollisionDetectorPtr getCollisionDetector() const;
148
151 collision::CollisionGroupPtr getCollisionGroup();
152
155 collision::ConstCollisionGroupPtr getCollisionGroup() const;
156
159 collision::CollisionOption& getCollisionOption();
160
163 const collision::CollisionOption& getCollisionOption() const;
164
166 collision::CollisionResult& getLastCollisionResult();
167
169 const collision::CollisionResult& getLastCollisionResult() const;
170
172 DART_DEPRECATED(6.7)
173 void setLCPSolver(std::unique_ptr<LCPSolver> lcpSolver);
174
176 DART_DEPRECATED(6.7)
177 LCPSolver* getLCPSolver() const;
178
180 void solve();
181
184 virtual void setFromOtherConstraintSolver(const ConstraintSolver& other);
185
186protected:
187 // TODO(JS): Docstring
188 virtual void solveConstrainedGroup(ConstrainedGroup& group) = 0;
189
191 bool containSkeleton(const dynamics::ConstSkeletonPtr& skeleton) const;
192
194 bool checkAndAddSkeleton(const dynamics::SkeletonPtr& skeleton);
195
197 bool containConstraint(const ConstConstraintBasePtr& constraint) const;
198
200 bool checkAndAddConstraint(const ConstraintBasePtr& constraint);
201
203 void updateConstraints();
204
207
210
212 bool isSoftContact(const collision::Contact& contact) const;
213
215
217 collision::CollisionDetectorPtr mCollisionDetector;
218
220 collision::CollisionGroupPtr mCollisionGroup;
221
223 collision::CollisionOption mCollisionOption;
224
226 collision::CollisionResult mCollisionResult;
227
229 double mTimeStep;
230
232 std::vector<dynamics::SkeletonPtr> mSkeletons;
233
236
239
242
245
248
251
254
257
260};
261
262} // namespace constraint
263} // namespace dart
264
265#endif // DART_CONSTRAINT_CONSTRAINTSOVER_HPP_
#define DART_DEPRECATED(version)
Definition Deprecated.hpp:51
std::size_t index
Definition SkelParser.cpp:1617
Definition CollisionDetector.hpp:56
ConstrainedGroup is a group of skeletons that interact each other with constraints.
Definition ConstrainedGroup.hpp:58
ConstraintSolver manages constraints and computes constraint impulses.
Definition ConstraintSolver.hpp:57
std::vector< SoftContactConstraintPtr > mSoftContactConstraints
Soft contact constraints those are automatically created.
Definition ConstraintSolver.hpp:238
void removeSkeletons(const std::vector< dynamics::SkeletonPtr > &skeletons)
Remove multiple skeletons.
Definition ConstraintSolver.cpp:151
std::vector< dynamics::SkeletonPtr > mSkeletons
Skeleton list.
Definition ConstraintSolver.hpp:232
std::vector< constraint::ConstraintBasePtr > getConstraints()
Returns all the constraints added to this ConstraintSolver.
Definition ConstraintSolver.cpp:222
collision::CollisionResult mCollisionResult
Last collision checking result.
Definition ConstraintSolver.hpp:226
void addSkeletons(const std::vector< dynamics::SkeletonPtr > &skeletons)
Add mutiple skeletons.
Definition ConstraintSolver.cpp:119
void clearLastCollisionResult()
Clears the last collision result.
Definition ConstraintSolver.cpp:242
void removeAllConstraints()
Remove all constraints.
Definition ConstraintSolver.cpp:198
bool containConstraint(const ConstConstraintBasePtr &constraint) const
Check if the constraint is contained in this solver.
Definition ConstraintSolver.cpp:425
virtual void solveConstrainedGroup(ConstrainedGroup &group)=0
std::vector< JointCoulombFrictionConstraintPtr > mJointCoulombFrictionConstraints
Joint Coulomb friction constraints those are automatically created.
Definition ConstraintSolver.hpp:250
virtual ~ConstraintSolver()=default
Destructor.
void removeConstraint(const ConstraintBasePtr &constraint)
Remove a constraint.
Definition ConstraintSolver.cpp:181
ConstraintSolver(const ConstraintSolver &other)=delete
Copy constructor.
collision::CollisionGroupPtr getCollisionGroup()
Return collision group of collision objects that are added to this ConstraintSolver.
Definition ConstraintSolver.cpp:305
ConstraintSolver()
Default constructor.
Definition ConstraintSolver.cpp:81
void solveConstrainedGroups()
Solve constrained groups.
Definition ConstraintSolver.cpp:679
std::vector< ContactConstraintPtr > mContactConstraints
Contact constraints those are automatically created.
Definition ConstraintSolver.hpp:235
bool checkAndAddSkeleton(const dynamics::SkeletonPtr &skeleton)
Add skeleton if the constraint is not contained in this solver.
Definition ConstraintSolver.cpp:409
void solve()
Solve constraint impulses and apply them to the skeletons.
Definition ConstraintSolver.cpp:362
virtual void setFromOtherConstraintSolver(const ConstraintSolver &other)
Sets this constraint solver using other constraint solver.
Definition ConstraintSolver.cpp:383
std::size_t getNumConstraints() const
Returns the number of constraints that was manually added to this ConstraintSolver.
Definition ConstraintSolver.cpp:204
void addSkeleton(const dynamics::SkeletonPtr &skeleton)
Add single skeleton.
Definition ConstraintSolver.cpp:99
collision::CollisionDetectorPtr mCollisionDetector
Collision detector.
Definition ConstraintSolver.hpp:217
constraint::ConstraintBasePtr getConstraint(std::size_t index)
Returns a constraint by index.
Definition ConstraintSolver.cpp:210
bool checkAndAddConstraint(const ConstraintBasePtr &constraint)
Add constraint if the constraint is not contained in this solver.
Definition ConstraintSolver.cpp:433
void addConstraint(const ConstraintBasePtr &constraint)
Add a constraint.
Definition ConstraintSolver.cpp:166
collision::CollisionDetectorPtr getCollisionDetector()
Get collision detector.
Definition ConstraintSolver.cpp:292
double getTimeStep() const
Get time step.
Definition ConstraintSolver.cpp:255
virtual void setTimeStep(double _timeStep)
Set time step.
Definition ConstraintSolver.cpp:248
collision::CollisionOption & getCollisionOption()
Returns collision option that is used for collision checkings in this ConstraintSolver to generate co...
Definition ConstraintSolver.cpp:317
std::vector< MimicMotorConstraintPtr > mMimicMotorConstraints
Mimic motor constraints those are automatically created.
Definition ConstraintSolver.hpp:247
std::vector< ServoMotorConstraintPtr > mServoMotorConstraints
Servo motor constraints those are automatically created.
Definition ConstraintSolver.hpp:244
void removeSkeleton(const dynamics::SkeletonPtr &skeleton)
Remove single skeleton.
Definition ConstraintSolver.cpp:132
std::vector< ConstrainedGroup > mConstrainedGroups
Constraint group list.
Definition ConstraintSolver.hpp:259
collision::CollisionResult & getLastCollisionResult()
Return the last collision checking result.
Definition ConstraintSolver.cpp:329
std::vector< ConstraintBasePtr > mManualConstraints
Constraints that manually added.
Definition ConstraintSolver.hpp:253
const std::vector< dynamics::SkeletonPtr > & getSkeletons() const
Returns all the skeletons added to this ConstraintSolver.
Definition ConstraintSolver.cpp:126
collision::CollisionGroupPtr mCollisionGroup
Collision group.
Definition ConstraintSolver.hpp:220
std::vector< ConstraintBasePtr > mActiveConstraints
Active constraints.
Definition ConstraintSolver.hpp:256
bool isSoftContact(const collision::Contact &contact) const
Return true if at least one of colliding body is soft body.
Definition ConstraintSolver.cpp:686
double mTimeStep
Time step.
Definition ConstraintSolver.hpp:229
void setLCPSolver(std::unique_ptr< LCPSolver > lcpSolver)
Set LCP solver.
Definition ConstraintSolver.cpp:342
void removeAllSkeletons()
Remove all skeletons in this constraint solver.
Definition ConstraintSolver.cpp:159
std::vector< JointLimitConstraintPtr > mJointLimitConstraints
Joint limit constraints those are automatically created.
Definition ConstraintSolver.hpp:241
void updateConstraints()
Update constraints.
Definition ConstraintSolver.cpp:448
collision::CollisionOption mCollisionOption
Collision detection option.
Definition ConstraintSolver.hpp:223
bool containSkeleton(const dynamics::ConstSkeletonPtr &skeleton) const
Check if the skeleton is contained in this solver.
Definition ConstraintSolver.cpp:394
LCPSolver * getLCPSolver() const
Get LCP solver.
Definition ConstraintSolver.cpp:351
void setCollisionDetector(collision::CollisionDetector *collisionDetector)
Set collision detector.
Definition ConstraintSolver.cpp:261
void buildConstrainedGroups()
Build constrained groupsContact.
Definition ConstraintSolver.cpp:623
Definition LCPSolver.hpp:45
std::shared_ptr< JointCoulombFrictionConstraint > JointCoulombFrictionConstraintPtr
Definition SmartPointer.hpp:52
std::shared_ptr< const ConstraintBase > ConstConstraintBasePtr
Definition SmartPointer.hpp:45
std::shared_ptr< ContactConstraint > ContactConstraintPtr
Definition SmartPointer.hpp:47
std::shared_ptr< MimicMotorConstraint > MimicMotorConstraintPtr
Definition SmartPointer.hpp:51
std::shared_ptr< ServoMotorConstraint > ServoMotorConstraintPtr
Definition SmartPointer.hpp:50
std::shared_ptr< JointLimitConstraint > JointLimitConstraintPtr
Definition SmartPointer.hpp:49
std::shared_ptr< ConstraintBase > ConstraintBasePtr
Definition SmartPointer.hpp:45
std::shared_ptr< SoftContactConstraint > SoftContactConstraintPtr
Definition SmartPointer.hpp:48
std::shared_ptr< const Skeleton > ConstSkeletonPtr
Definition SmartPointer.hpp:60
std::shared_ptr< Skeleton > SkeletonPtr
Definition SmartPointer.hpp:60
Definition BulletCollisionDetector.cpp:63
Definition SharedLibraryManager.hpp:43