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
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:110