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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodySphericalJointMotor.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 "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
25btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
26 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
27 m_desiredVelocity(0, 0, 0),
28 m_desiredPosition(0,0,0,1),
29 m_use_multi_dof_params(false),
30 m_kd(1., 1., 1.),
31 m_kp(0.2, 0.2, 0.2),
32 m_erp(1),
33 m_rhsClamp(SIMD_INFINITY),
34 m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse),
35 m_damping(1.0, 1.0, 1.0)
36{
37
38 m_maxAppliedImpulse = maxMotorImpulse;
39}
40
41
42 void btMultiBodySphericalJointMotor::finalizeMultiDof()
43{
44 allocateJacobiansMultiDof();
45 // note: we rely on the fact that data.m_jacobians are
46 // always initialized to zero by the Constraint ctor
47 int linkDoF = 0;
48 unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
49
50 // row 0: the lower bound
51 // row 0: the lower bound
52 jacobianA(0)[offset] = 1;
53
54 m_numDofsFinalized = m_jacSizeBoth;
55}
56
57
58btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
59{
60}
61
62 int btMultiBodySphericalJointMotor::getIslandIdA() const
63{
64 if (this->m_linkA < 0)
65 {
66 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
67 if (col)
68 return col->getIslandTag();
69 }
70 else
71 {
72 if (m_bodyA->getLink(m_linkA).m_collider)
73 {
74 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
75 }
76 }
77 return -1;
78}
79
80 int btMultiBodySphericalJointMotor::getIslandIdB() const
81{
82 if (m_linkB < 0)
83 {
84 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
85 if (col)
86 return col->getIslandTag();
87 }
88 else
89 {
90 if (m_bodyB->getLink(m_linkB).m_collider)
91 {
92 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
93 }
94 }
95 return -1;
96}
97
98 void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
99 btMultiBodyJacobianData& data,
100 const btContactSolverInfo& infoGlobal)
101{
102 // only positions need to be updated -- data.m_jacobians and force
103 // directions were set in the ctor and never change.
104
105 if (m_numDofsFinalized != m_jacSizeBoth)
106 {
107 finalizeMultiDof();
108 }
109
110 //don't crash
111 if (m_numDofsFinalized != m_jacSizeBoth)
112 return;
113
114
115 if (m_maxAppliedImpulse == 0.f)
116 return;
117
118 const btScalar posError = 0;
119 const btVector3 dummy(0, 0, 0);
120
121
122 btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
123
124 btQuaternion desiredQuat = m_desiredPosition;
125 btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
126 m_bodyA->getJointPosMultiDof(m_linkA)[1],
127 m_bodyA->getJointPosMultiDof(m_linkA)[2],
128 m_bodyA->getJointPosMultiDof(m_linkA)[3]);
129
130 btQuaternion relRot = currentQuat.inverse() * desiredQuat;
131 btVector3 angleDiff;
132 btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
133
134
135
136 for (int row = 0; row < getNumRows(); row++)
137 {
138 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
139
140 int dof = row;
141
142 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
143 btScalar desiredVelocity = this->m_desiredVelocity[row];
144
145 double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
146 btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
147
148 btMatrix3x3 frameAworld;
149 frameAworld.setIdentity();
150 frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
151 btScalar posError = 0;
152 {
153 btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
154 switch (m_bodyA->getLink(m_linkA).m_jointType)
155 {
156 case btMultibodyLink::eSpherical:
157 {
158 btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
159 double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
160 posError = kp*angleDiff[row % 3];
161 double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
162 fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
163 btVector3(0,0,0), dummy, dummy,
164 posError,
165 infoGlobal,
166 -max_applied_impulse, max_applied_impulse, true,
167 1.0, false, 0, 0,
168 m_damping[row % 3]);
169 constraintRow.m_orgConstraint = this;
170 constraintRow.m_orgDofIndex = row;
171 break;
172 }
173 default:
174 {
175 btAssert(0);
176 }
177 };
178 }
179 }
180}
btCollisionObject.h
btGeneric6DofSpring2Constraint.h
MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR
@ MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR
Definition btMultiBodyConstraint.h:32
btMultiBodyLinkCollider.h
btMultiBodySphericalJointMotor.h
btMultiBody.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
SIMD_INFINITY
#define SIMD_INFINITY
Definition btScalar.h:544
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
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::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
btMultiBodySphericalJointMotor::getIslandIdA
virtual int getIslandIdA() const
Definition btMultiBodySphericalJointMotor.cpp:62
btMultiBodySphericalJointMotor::m_desiredVelocity
btVector3 m_desiredVelocity
Definition btMultiBodySphericalJointMotor.h:27
btMultiBodySphericalJointMotor::m_maxAppliedImpulseMultiDof
btVector3 m_maxAppliedImpulseMultiDof
Definition btMultiBodySphericalJointMotor.h:34
btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor
btMultiBodySphericalJointMotor(btMultiBody *body, int link, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
Definition btMultiBodySphericalJointMotor.cpp:25
btMultiBodySphericalJointMotor::finalizeMultiDof
virtual void finalizeMultiDof()
Definition btMultiBodySphericalJointMotor.cpp:42
btMultiBodySphericalJointMotor::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition btMultiBodySphericalJointMotor.cpp:98
btMultiBodySphericalJointMotor::getIslandIdB
virtual int getIslandIdB() const
Definition btMultiBodySphericalJointMotor.cpp:80
btMultiBodySphericalJointMotor::m_desiredPosition
btQuaternion m_desiredPosition
Definition btMultiBodySphericalJointMotor.h:28
btMultiBodySphericalJointMotor::m_kd
btVector3 m_kd
Definition btMultiBodySphericalJointMotor.h:30
btMultiBodySphericalJointMotor::m_damping
btVector3 m_damping
Definition btMultiBodySphericalJointMotor.h:35
btMultiBodySphericalJointMotor::m_kp
btVector3 m_kp
Definition btMultiBodySphericalJointMotor.h:31
btMultiBodySphericalJointMotor::m_use_multi_dof_params
bool m_use_multi_dof_params
Definition btMultiBodySphericalJointMotor.h:29
btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor
virtual ~btMultiBodySphericalJointMotor()
Definition btMultiBodySphericalJointMotor.cpp:58
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::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition btMultiBody.cpp:393
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition btQuaternion.h:50
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition btQuaternion.h:497
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
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