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

Bullet Collision Detection & Physics Library

btMultiBodyJointMotor.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2013 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 "btMultiBodyJointMotor.h"
19#include "btMultiBody.h"
20#include "btMultiBodyLinkCollider.h"
21#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22
23btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
24 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
25 m_desiredVelocity(desiredVelocity),
26 m_desiredPosition(0),
27 m_kd(1.),
28 m_kp(0),
29 m_erp(1),
30 m_rhsClamp(SIMD_INFINITY)
31{
32 m_maxAppliedImpulse = maxMotorImpulse;
33 // the data.m_jacobians never change, so may as well
34 // initialize them here
35}
36
37 void btMultiBodyJointMotor::finalizeMultiDof()
38{
39 allocateJacobiansMultiDof();
40 // note: we rely on the fact that data.m_jacobians are
41 // always initialized to zero by the Constraint ctor
42 int linkDoF = 0;
43 unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
44
45 // row 0: the lower bound
46 // row 0: the lower bound
47 jacobianA(0)[offset] = 1;
48
49 m_numDofsFinalized = m_jacSizeBoth;
50}
51
52 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
53 //:btMultiBodyConstraint(body,0,link,-1,1,true),
54 : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
55 m_desiredVelocity(desiredVelocity),
56 m_desiredPosition(0),
57 m_kd(1.),
58 m_kp(0),
59 m_erp(1),
60 m_rhsClamp(SIMD_INFINITY)
61{
62 btAssert(linkDoF < body->getLink(link).m_dofCount);
63
64 m_maxAppliedImpulse = maxMotorImpulse;
65}
66btMultiBodyJointMotor::~btMultiBodyJointMotor()
67{
68}
69
70 int btMultiBodyJointMotor::getIslandIdA() const
71{
72 if (this->m_linkA < 0)
73 {
74 btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
75 if (col)
76 return col->getIslandTag();
77 }
78 else
79 {
80 if (m_bodyA->getLink(m_linkA).m_collider)
81 {
82 return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
83 }
84 }
85 return -1;
86}
87
88 int btMultiBodyJointMotor::getIslandIdB() const
89{
90 if (m_linkB < 0)
91 {
92 btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
93 if (col)
94 return col->getIslandTag();
95 }
96 else
97 {
98 if (m_bodyB->getLink(m_linkB).m_collider)
99 {
100 return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
101 }
102 }
103 return -1;
104}
105
106 void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
107 btMultiBodyJacobianData& data,
108 const btContactSolverInfo& infoGlobal)
109{
110 // only positions need to be updated -- data.m_jacobians and force
111 // directions were set in the ctor and never change.
112
113 if (m_numDofsFinalized != m_jacSizeBoth)
114 {
115 finalizeMultiDof();
116 }
117
118 //don't crash
119 if (m_numDofsFinalized != m_jacSizeBoth)
120 return;
121
122 if (m_maxAppliedImpulse == 0.f)
123 return;
124
125 const btScalar posError = 0;
126 const btVector3 dummy(0, 0, 0);
127
128 for (int row = 0; row < getNumRows(); row++)
129 {
130 btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
131
132 int dof = 0;
133 btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
134 btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
135 btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
136
137 btScalar velocityError = (m_desiredVelocity - currentVelocity);
138 btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
139 if (rhs > m_rhsClamp)
140 {
141 rhs = m_rhsClamp;
142 }
143 if (rhs < -m_rhsClamp)
144 {
145 rhs = -m_rhsClamp;
146 }
147
148 fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
149 constraintRow.m_orgConstraint = this;
150 constraintRow.m_orgDofIndex = row;
151 {
152 //expect either prismatic or revolute joint type for now
153 btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
154 switch (m_bodyA->getLink(m_linkA).m_jointType)
155 {
156 case btMultibodyLink::eRevolute:
157 {
158 constraintRow.m_contactNormal1.setZero();
159 constraintRow.m_contactNormal2.setZero();
160 btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
161 constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
162 constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
163
164 break;
165 }
166 case btMultibodyLink::ePrismatic:
167 {
168 btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
169 constraintRow.m_contactNormal1 = prismaticAxisInWorld;
170 constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
171 constraintRow.m_relpos1CrossNormal.setZero();
172 constraintRow.m_relpos2CrossNormal.setZero();
173
174 break;
175 }
176 default:
177 {
178 btAssert(0);
179 }
180 };
181 }
182 }
183}
btCollisionObject.h
MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR
@ MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR
Definition: btMultiBodyConstraint.h:28
btMultiBodyJointMotor.h
btMultiBodyLinkCollider.h
btMultiBody.h
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
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
btAlignedObjectArray< btMultiBodySolverConstraint >
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:448
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
btMultiBodyJointMotor::m_rhsClamp
btScalar m_rhsClamp
Definition: btMultiBodyJointMotor.h:32
btMultiBodyJointMotor::m_desiredVelocity
btScalar m_desiredVelocity
Definition: btMultiBodyJointMotor.h:27
btMultiBodyJointMotor::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyJointMotor.cpp:88
btMultiBodyJointMotor::m_desiredPosition
btScalar m_desiredPosition
Definition: btMultiBodyJointMotor.h:28
btMultiBodyJointMotor::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyJointMotor.cpp:37
btMultiBodyJointMotor::~btMultiBodyJointMotor
virtual ~btMultiBodyJointMotor()
Definition: btMultiBodyJointMotor.cpp:66
btMultiBodyJointMotor::btMultiBodyJointMotor
btMultiBodyJointMotor(btMultiBody *body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
Definition: btMultiBodyJointMotor.cpp:23
btMultiBodyJointMotor::m_kp
btScalar m_kp
Definition: btMultiBodyJointMotor.h:30
btMultiBodyJointMotor::m_erp
btScalar m_erp
Definition: btMultiBodyJointMotor.h:31
btMultiBodyJointMotor::m_kd
btScalar m_kd
Definition: btMultiBodyJointMotor.h:29
btMultiBodyJointMotor::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyJointMotor.cpp:106
btMultiBodyJointMotor::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyJointMotor.cpp:70
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btMultiBody
Definition: btMultiBody.h:51
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
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:119
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
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_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:132
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:137
btMultibodyLink::ePrismatic
@ ePrismatic
Definition: btMultiBodyLink.h:73
btMultibodyLink::eRevolute
@ eRevolute
Definition: btMultiBodyLink.h:72
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:141
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:110
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:93
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96