DART  6.10.1
BallJointConstraint.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_BALLJOINTCONSTRAINT_HPP_
34 #define DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_
35 
36 #include <Eigen/Dense>
37 
39 #include "dart/math/MathTypes.hpp"
40 
41 namespace dart {
42 namespace constraint {
43 
47 {
48 public:
53  dynamics::BodyNode* _body, const Eigen::Vector3d& _jointPos);
54 
61  dynamics::BodyNode* _body1,
62  dynamics::BodyNode* _body2,
63  const Eigen::Vector3d& _jointPos);
64 
66  virtual ~BallJointConstraint();
67 
68  // Documentation inherited
69  const std::string& getType() const override;
70 
72  static const std::string& getStaticType();
73 
74 protected:
75  //----------------------------------------------------------------------------
76  // Constraint virtual functions
77  //----------------------------------------------------------------------------
78 
79  // Documentation inherited
80  void update() override;
81 
82  // Documentation inherited
83  void getInformation(ConstraintInfo* _lcp) override;
84 
85  // Documentation inherited
86  void applyUnitImpulse(std::size_t _index) override;
87 
88  // Documentation inherited
89  void getVelocityChange(double* _vel, bool _withCfm) override;
90 
91  // Documentation inherited
92  void excite() override;
93 
94  // Documentation inherited
95  void unexcite() override;
96 
97  // Documentation inherited
98  void applyImpulse(double* _lambda) override;
99 
100  // Documentation inherited
101  bool isActive() const override;
102 
103  // Documentation inherited
104  dynamics::SkeletonPtr getRootSkeleton() const override;
105 
106  // Documentation inherited
107  void uniteSkeletons() override;
108 
109 private:
112  Eigen::Vector3d mOffset1;
113 
116  Eigen::Vector3d mOffset2;
117 
119  Eigen::Vector3d mViolation;
120 
122  Eigen::Matrix<double, 3, 6> mJacobian1;
123 
125  Eigen::Matrix<double, 3, 6> mJacobian2;
126 
128  double mOldX[3];
129 
131  std::size_t mAppliedImpulseIndex;
132 
133 public:
134  // To get byte-aligned Eigen vectors
135  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
136 };
137 
138 } // namespace constraint
139 } // namespace dart
140 
141 #endif // DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_
BallJointConstraint represents ball joint constraint between a body and the world or between two bodi...
Definition: BallJointConstraint.hpp:47
static const std::string & getStaticType()
Returns constraint type for this class.
Definition: BallJointConstraint.cpp:100
void excite() override
Excite the constraint.
Definition: BallJointConstraint.cpp:292
void unexcite() override
Unexcite the constraint.
Definition: BallJointConstraint.cpp:305
void getInformation(ConstraintInfo *_lcp) override
Fill LCP variables.
Definition: BallJointConstraint.cpp:142
const std::string & getType() const override
Returns a string representing the constraint type.
Definition: BallJointConstraint.cpp:94
Eigen::Matrix< double, 3, 6 > mJacobian1
Linear map between constraint space and Cartesian space for body1.
Definition: BallJointConstraint.hpp:122
Eigen::Vector3d mOffset2
Offset from the origin of body frame 2 to the ball joint position where the offset is expressed in bo...
Definition: BallJointConstraint.hpp:116
Eigen::Vector3d mOffset1
Offset from the origin of body frame 1 to the ball joint position where the offset is expressed in bo...
Definition: BallJointConstraint.hpp:112
BallJointConstraint(dynamics::BodyNode *_body, const Eigen::Vector3d &_jointPos)
Constructor that takes one body and the joint position in the world frame.
Definition: BallJointConstraint.cpp:44
void applyImpulse(double *_lambda) override
Apply computed constraint impulse to constrained skeletons.
Definition: BallJointConstraint.cpp:318
Eigen::Matrix< double, 3, 6 > mJacobian2
Linear map between constraint space and Cartesian space for body2.
Definition: BallJointConstraint.hpp:125
dynamics::SkeletonPtr getRootSkeleton() const override
Definition: BallJointConstraint.cpp:336
void uniteSkeletons() override
Definition: BallJointConstraint.cpp:361
void applyUnitImpulse(std::size_t _index) override
Apply unit impulse to constraint space.
Definition: BallJointConstraint.cpp:179
virtual ~BallJointConstraint()
Destructor.
Definition: BallJointConstraint.cpp:89
bool isActive() const override
Return true if this constraint is active.
Definition: BallJointConstraint.cpp:395
void update() override
Update constraint using updated Skeleton's states.
Definition: BallJointConstraint.cpp:107
double mOldX[3]
Definition: BallJointConstraint.hpp:128
std::size_t mAppliedImpulseIndex
Index of applied impulse.
Definition: BallJointConstraint.hpp:131
void getVelocityChange(double *_vel, bool _withCfm) override
Get velocity change due to the uint impulse.
Definition: BallJointConstraint.cpp:257
Eigen::Vector3d mViolation
Position constraint violation expressed in body frame 1.
Definition: BallJointConstraint.hpp:119
class JointConstraint
Definition: JointConstraint.hpp:48
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
Definition: BulletCollisionDetector.cpp:65
ConstraintInfo.
Definition: ConstraintBase.hpp:50