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

Bullet Collision Detection & Physics Library

btMultiBodySphericalJointMotor.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2018 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
17
18 #include "btMultiBodySphericalJointMotor.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
25 btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
26 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true),
27 m_desiredVelocity(0, 0, 0),
28 m_desiredPosition(0,0,0,1),
29 m_kd(1.),
30 m_kp(0.2),
31 m_erp(1),
32 m_rhsClamp(SIMD_INFINITY)
33 {
34
35 m_maxAppliedImpulse = maxMotorImpulse;
36 }
37
38
39 void btMultiBodySphericalJointMotor::finalizeMultiDof()
40 {
41 allocateJacobiansMultiDof();
42 // note: we rely on the fact that data.m_jacobians are
43 // always initialized to zero by the Constraint ctor
44 int linkDoF = 0;
45 unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
46
47 // row 0: the lower bound
48 // row 0: the lower bound
49 jacobianA(0)[offset] = 1;
50
51 m_numDofsFinalized = m_jacSizeBoth;
52 }
53
54
55 btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
56 {
57 }
58
59 int btMultiBodySphericalJointMotor::getIslandIdA() const
60 {
61 if (this->m_linkA < 0)
62 {
63 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
64 if (col)
65 return col->getIslandTag();
66 }
67 else
68 {
69 if (m_bodyA->getLink(m_linkA).m_collider)
70 {
71 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
72 }
73 }
74 return -1;
75 }
76
77 int btMultiBodySphericalJointMotor::getIslandIdB() const
78 {
79 if (m_linkB < 0)
80 {
81 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
82 if (col)
83 return col->getIslandTag();
84 }
85 else
86 {
87 if (m_bodyB->getLink(m_linkB).m_collider)
88 {
89 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
90 }
91 }
92 return -1;
93 }
94
95 void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
96 btMultiBodyJacobianData& data,
97 const btContactSolverInfo& infoGlobal)
98 {
99 // only positions need to be updated -- data.m_jacobians and force
100 // directions were set in the ctor and never change.
101
102 if (m_numDofsFinalized != m_jacSizeBoth)
103 {
104 finalizeMultiDof();
105 }
106
107 //don't crash
108 if (m_numDofsFinalized != m_jacSizeBoth)
109 return;
110
111
112 if (m_maxAppliedImpulse == 0.f)
113 return;
114
115 const btScalar posError = 0;
116 const btVector3 dummy(0, 0, 0);
117
118
119 btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
120
121 btQuaternion desiredQuat = m_desiredPosition;
122 btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
123 m_bodyA->getJointPosMultiDof(m_linkA)[1],
124 m_bodyA->getJointPosMultiDof(m_linkA)[2],
125 m_bodyA->getJointPosMultiDof(m_linkA)[3]);
126
127 btQuaternion relRot = currentQuat.inverse() * desiredQuat;
128 btVector3 angleDiff;
129 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
130
131
132
133 for (int row = 0; row < getNumRows(); row++)
134 {
135 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
136
137 int dof = row;
138
139 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
140 btScalar desiredVelocity = this->m_desiredVelocity[row];
141
142 btScalar velocityError = desiredVelocity - currentVelocity;
143
144 btMatrix3x3 frameAworld;
145 frameAworld.setIdentity();
146 frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
147 btScalar posError = 0;
148 {
149 btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
150 switch (m_bodyA->getLink(m_linkA).m_jointType)
151 {
152 case btMultibodyLink::eSpherical:
153 {
154 btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
155 posError = m_kp*angleDiff[row % 3];
156 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
157 btVector3(0,0,0), dummy, dummy,
158 posError,
159 infoGlobal,
160 -m_maxAppliedImpulse, m_maxAppliedImpulse, true);
161 constraintRow.m_orgConstraint = this;
162 constraintRow.m_orgDofIndex = row;
163 break;
164 }
165 default:
166 {
167 btAssert(0);
168 }
169 };
170 }
171 }
172 }
btMultiBodySphericalJointMotor::m_kp
btScalar m_kp
Definition: btMultiBodySphericalJointMotor.h:30
btTransformUtil.h
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:68
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor
btMultiBodySphericalJointMotor(btMultiBody *body, int link, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
Definition: btMultiBodySphericalJointMotor.cpp:25
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBodySphericalJointMotor::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodySphericalJointMotor.cpp:59
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:56
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:132
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:127
btMultiBodySphericalJointMotor::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodySphericalJointMotor.cpp:77
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:45
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:160
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition: btMultiBody.cpp:383
btMultiBody
Definition: btMultiBody.h:49
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:40
btMultiBodySphericalJointMotor::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodySphericalJointMotor.cpp:39
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:133
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
Definition: btMultiBody.cpp:534
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:524
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:55
btMultiBodySphericalJointMotor.h
btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor
virtual ~btMultiBodySphericalJointMotor()
Definition: btMultiBodySphericalJointMotor.cpp:55
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:51
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:104
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyLinkCollider.h
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:110
btMultibodyLink::eSpherical
Definition: btMultiBodyLink.h:74
btMultiBody.h
btMultiBodySphericalJointMotor::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodySphericalJointMotor.cpp:95
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:434
btGeneric6DofSpring2Constraint.h
btMultiBodySphericalJointMotor::m_desiredVelocity
btVector3 m_desiredVelocity
Definition: btMultiBodySphericalJointMotor.h:27
btCollisionObject.h
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:44
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:34
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:49
btMultiBodySphericalJointMotor::m_desiredPosition
btQuaternion m_desiredPosition
Definition: btMultiBodySphericalJointMotor.h:28
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)
Definition: btMultiBodyConstraint.cpp:52
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:237
btMatrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:314
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition: btMultiBody.cpp:378
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:47
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:46