Bullet Collision Detection & Physics Library: btMultiBodySphericalJointLimit.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodySphericalJointLimit.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2018 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
17
18#include "btMultiBodySphericalJointLimit.h"
19#include "btMultiBody.h"
20#include "btMultiBodyLinkCollider.h"
21#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22#include "LinearMath/btTransformUtil.h"
23#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
24#include "LinearMath/btIDebugDraw.h"
25
26btMultiBodySphericalJointLimit::btMultiBodySphericalJointLimit(btMultiBody* body, int link,
27 btScalar swingxRange,
28 btScalar swingyRange,
29 btScalar twistRange,
30 btScalar maxAppliedImpulse)
31 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT),
32 m_desiredVelocity(0, 0, 0),
33 m_desiredPosition(0,0,0,1),
34 m_use_multi_dof_params(false),
35 m_kd(1., 1., 1.),
36 m_kp(0.2, 0.2, 0.2),
37 m_erp(1),
38 m_rhsClamp(SIMD_INFINITY),
39 m_maxAppliedImpulseMultiDof(maxAppliedImpulse, maxAppliedImpulse, maxAppliedImpulse),
40 m_pivotA(m_bodyA->getLink(link).m_eVector),
41 m_pivotB(m_bodyB->getLink(link).m_eVector),
42 m_swingxRange(swingxRange),
43 m_swingyRange(swingyRange),
44 m_twistRange(twistRange)
45
46{
47
48 m_maxAppliedImpulse = maxAppliedImpulse;
49}
50
51
52 void btMultiBodySphericalJointLimit::finalizeMultiDof()
53{
54 allocateJacobiansMultiDof();
55 // note: we rely on the fact that data.m_jacobians are
56 // always initialized to zero by the Constraint ctor
57 int linkDoF = 0;
58 unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
59
60 // row 0: the lower bound
61 // row 0: the lower bound
62 jacobianA(0)[offset] = 1;
63
64 jacobianB(1)[offset] = -1;
65
66 m_numDofsFinalized = m_jacSizeBoth;
67}
68
69
70btMultiBodySphericalJointLimit::~btMultiBodySphericalJointLimit()
71{
72}
73
74 int btMultiBodySphericalJointLimit::getIslandIdA() const
75{
76 if (this->m_linkA < 0)
77 {
78 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
79 if (col)
80 return col->getIslandTag();
81 }
82 else
83 {
84 if (m_bodyA->getLink(m_linkA).m_collider)
85 {
86 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
87 }
88 }
89 return -1;
90}
91
92 int btMultiBodySphericalJointLimit::getIslandIdB() const
93{
94 if (m_linkB < 0)
95 {
96 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
97 if (col)
98 return col->getIslandTag();
99 }
100 else
101 {
102 if (m_bodyB->getLink(m_linkB).m_collider)
103 {
104 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
105 }
106 }
107 return -1;
108}
109
110 void btMultiBodySphericalJointLimit::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
111 btMultiBodyJacobianData& data,
112 const btContactSolverInfo& infoGlobal)
113{
114 // only positions need to be updated -- data.m_jacobians and force
115 // directions were set in the ctor and never change.
116
117 if (m_numDofsFinalized != m_jacSizeBoth)
118 {
119 finalizeMultiDof();
120 }
121
122 //don't crash
123 if (m_numDofsFinalized != m_jacSizeBoth)
124 return;
125
126
127 if (m_maxAppliedImpulse == 0.f)
128 return;
129
130 const btScalar posError = 0;
131 const btVector3 zero(0, 0, 0);
132
133
134 btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
135
136 btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
137 m_bodyA->getJointPosMultiDof(m_linkA)[1],
138 m_bodyA->getJointPosMultiDof(m_linkA)[2],
139 m_bodyA->getJointPosMultiDof(m_linkA)[3]);
140
141 btQuaternion refQuat = m_desiredPosition;
142 btVector3 vTwist(0,0,1);
143
144 btVector3 vConeNoTwist = quatRotate(currentQuat, vTwist);
145 vConeNoTwist.normalize();
146 btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
147 qABCone.normalize();
148 btQuaternion qABTwist = qABCone.inverse() * currentQuat;
149 qABTwist.normalize();
150 btQuaternion desiredQuat = qABTwist;
151
152
153 btQuaternion relRot = currentQuat.inverse() * desiredQuat;
154 btVector3 angleDiff;
155 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
156
157 btScalar limitRanges[3] = {m_swingxRange, m_swingyRange, m_twistRange};
158
160 btQuaternion qMinTwist = qABTwist;
161 btScalar twistAngle = qABTwist.getAngle();
162
163 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
164 {
165 qMinTwist = -(qABTwist);
166 twistAngle = qMinTwist.getAngle();
167 }
168 btVector3 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
169 if (twistAngle > SIMD_EPSILON)
170 vTwistAxis.normalize();
171
172 if (vTwistAxis.dot(vTwist)<0)
173 twistAngle*=-1.;
174
175 angleDiff[2] = twistAngle;
176
177
178 for (int row = 0; row < getNumRows(); row++)
179 {
180 btScalar allowed = limitRanges[row];
181 btScalar damp = 1;
182 if((angleDiff[row]>-allowed)&&(angleDiff[row]<allowed))
183 {
184 angleDiff[row]=0;
185 damp=0;
186
187 } else
188 {
189 if (angleDiff[row]>allowed)
190 {
191 angleDiff[row]-=allowed;
192
193 }
194 if (angleDiff[row]<-allowed)
195 {
196 angleDiff[row]+=allowed;
197
198 }
199 }
200
201
202 int dof = row;
203
204 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
205 btScalar desiredVelocity = this->m_desiredVelocity[row];
206
207 double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
208 btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
209
210 btMatrix3x3 frameAworld;
211 frameAworld.setIdentity();
212 frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
213 btScalar posError = 0;
214 {
215 btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
216 switch (m_bodyA->getLink(m_linkA).m_jointType)
217 {
218 case btMultibodyLink::eSpherical:
219 {
220 btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
221 double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
222 posError = kp*angleDiff[row % 3];
223 double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
224 //should multiply by time step
225 //max_applied_impulse *= infoGlobal.m_timeStep
226
227 double min_applied_impulse = -max_applied_impulse;
228
229
230 if (posError>0)
231 max_applied_impulse=0;
232 else
233 min_applied_impulse=0;
234
235 if (btFabs(posError)>SIMD_EPSILON)
236 {
237 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
238 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
239 zero, zero, zero,//pure angular, so zero out linear parts
240 posError,
241 infoGlobal,
242 min_applied_impulse, max_applied_impulse, true,
243 1.0, false, 0, 0,
244 damp);
245 constraintRow.m_orgConstraint = this;
246 constraintRow.m_orgDofIndex = row;
247 }
248 break;
249 }
250 default:
251 {
252 btAssert(0);
253 }
254 };
255 }
256 }
257}
258
259
260 void btMultiBodySphericalJointLimit::debugDraw(class btIDebugDraw* drawer)
261{
262 btTransform tr;
263 tr.setIdentity();
264 if (m_bodyB)
265 {
266 btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotB);
267 tr.setOrigin(pivotBworld);
268 drawer->drawTransform(tr, 0.1);
269 }
270}
btCollisionObject.h
vTwist
static btVector3 vTwist(1, 0, 0)
btGeneric6DofSpring2Constraint.h
btIDebugDraw.h
MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT
@ MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT
Definition: btMultiBodyConstraint.h:34
btMultiBodyLinkCollider.h
btMultiBodySphericalJointLimit.h
btMultiBody.h
shortestArcQuat
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:940
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:526
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btTransformUtil.h
btAlignedObjectArray< btMultiBodySolverConstraint >
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:448
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:69
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btIDebugDraw::drawTransform
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:163
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:323
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:142
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:57
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:60
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:181
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:73
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:189
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:72
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:35
btMultiBodyConstraint::fillMultiBodyConstraint
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0, btScalar damping=1.0)
Definition: btMultiBodyConstraint.cpp:53
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:59
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:62
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:68
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:125
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:61
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btMultiBodySphericalJointLimit::m_kp
btVector3 m_kp
Definition: btMultiBodySphericalJointLimit.h:31
btMultiBodySphericalJointLimit::m_twistRange
btScalar m_twistRange
Definition: btMultiBodySphericalJointLimit.h:39
btMultiBodySphericalJointLimit::debugDraw
virtual void debugDraw(class btIDebugDraw *drawer)
Definition: btMultiBodySphericalJointLimit.cpp:260
btMultiBodySphericalJointLimit::~btMultiBodySphericalJointLimit
virtual ~btMultiBodySphericalJointLimit()
Definition: btMultiBodySphericalJointLimit.cpp:70
btMultiBodySphericalJointLimit::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodySphericalJointLimit.cpp:110
btMultiBodySphericalJointLimit::btMultiBodySphericalJointLimit
btMultiBodySphericalJointLimit(btMultiBody *body, int link, btScalar swingxRange, btScalar swingyRange, btScalar twistRange, btScalar maxAppliedImpulse)
This file was written by Erwin Coumans.
Definition: btMultiBodySphericalJointLimit.cpp:26
btMultiBodySphericalJointLimit::m_swingxRange
btScalar m_swingxRange
Definition: btMultiBodySphericalJointLimit.h:37
btMultiBodySphericalJointLimit::m_desiredVelocity
btVector3 m_desiredVelocity
Definition: btMultiBodySphericalJointLimit.h:27
btMultiBodySphericalJointLimit::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodySphericalJointLimit.cpp:52
btMultiBodySphericalJointLimit::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodySphericalJointLimit.cpp:74
btMultiBodySphericalJointLimit::m_swingyRange
btScalar m_swingyRange
Definition: btMultiBodySphericalJointLimit.h:38
btMultiBodySphericalJointLimit::m_pivotB
btVector3 m_pivotB
Definition: btMultiBodySphericalJointLimit.h:36
btMultiBodySphericalJointLimit::m_kd
btVector3 m_kd
Definition: btMultiBodySphericalJointLimit.h:30
btMultiBodySphericalJointLimit::m_maxAppliedImpulseMultiDof
btVector3 m_maxAppliedImpulseMultiDof
Definition: btMultiBodySphericalJointLimit.h:34
btMultiBodySphericalJointLimit::m_use_multi_dof_params
bool m_use_multi_dof_params
Definition: btMultiBodySphericalJointLimit.h:29
btMultiBodySphericalJointLimit::m_desiredPosition
btQuaternion m_desiredPosition
Definition: btMultiBodySphericalJointLimit.h:28
btMultiBodySphericalJointLimit::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodySphericalJointLimit.cpp:92
btMultiBody
Definition: btMultiBody.h:51
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
Definition: btMultiBody.cpp:554
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition: btMultiBody.cpp:388
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
Definition: btMultiBody.cpp:470
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition: btMultiBody.cpp:393
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btQuaternion::getAngle
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:468
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:167
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:147
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btContactSolverInfo
Definition: btContactSolverInfo.h:76
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:44
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:30
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:132
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:137
btMultibodyLink::eSpherical
@ eSpherical
Definition: btMultiBodyLink.h:74
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:110