Bullet Collision Detection & Physics Library
btMultiBodyJointMotor.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 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 "btMultiBodyJointMotor.h"
19 #include "btMultiBody.h"
22 
23 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
24  : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
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 
38 {
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 
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),
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 }
67 {
68 }
69 
71 {
72  if (this->m_linkA < 0)
73  {
75  if (col)
76  return col->getIslandTag();
77  }
78  else
79  {
81  {
83  }
84  }
85  return -1;
86 }
87 
89 {
90  if (m_linkB < 0)
91  {
93  if (col)
94  return col->getIslandTag();
95  }
96  else
97  {
99  {
101  }
102  }
103  return -1;
104 }
105 
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 
114  {
116  }
117 
118  //don't crash
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
155  {
157  {
158  constraintRow.m_contactNormal1.setZero();
159  constraintRow.m_contactNormal2.setZero();
161  constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
162  constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
163 
164  break;
165  }
167  {
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 }
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition: btMultiBody.cpp:378
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btMultiBodyJointMotor::getIslandIdB
virtual int getIslandIdB() const
Definition: btMultiBodyJointMotor.cpp:88
btMultiBodyJointMotor::m_desiredPosition
btScalar m_desiredPosition
Definition: btMultiBodyJointMotor.h:28
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:40
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition: btMultiBody.cpp:383
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:46
btMultiBodyConstraint::getNumRows
int getNumRows() const
Definition: btMultiBodyConstraint.h:104
btMultiBodyJointMotor::m_kp
btScalar m_kp
Definition: btMultiBodyJointMotor.h:30
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btMultiBodyJointMotor::finalizeMultiDof
virtual void finalizeMultiDof()
Definition: btMultiBodyJointMotor.cpp:37
btMultiBodyJointMotor::~btMultiBodyJointMotor
virtual ~btMultiBodyJointMotor()
Definition: btMultiBodyJointMotor.cpp:66
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodyLinkCollider.h
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:41
btMultiBodyJointMotor::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyJointMotor.cpp:106
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:47
btMultiBodyConstraint::m_maxAppliedImpulse
btScalar m_maxAppliedImpulse
Definition: btMultiBodyConstraint.h:56
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
btMultiBodyJointMotor.h
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:524
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btMultiBody
Definition: btMultiBody.h:49
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:434
btMultiBodyConstraint::m_numDofsFinalized
int m_numDofsFinalized
Definition: btMultiBodyConstraint.h:55
btAlignedObjectArray< btMultiBodySolverConstraint >
btMultiBodyJointMotor::m_rhsClamp
btScalar m_rhsClamp
Definition: btMultiBodyJointMotor.h:32
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:34
btMultiBodyConstraint::jacobianA
btScalar * jacobianA(int row)
Definition: btMultiBodyConstraint.h:160
btMultiBodyJointMotor::m_desiredVelocity
btScalar m_desiredVelocity
Definition: btMultiBodyJointMotor.h:27
btMultiBody.h
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:44
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBodyJointMotor::m_erp
btScalar m_erp
Definition: btMultiBodyJointMotor.h:31
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btMultiBodyJointMotor::m_kd
btScalar m_kd
Definition: btMultiBodyJointMotor.h:29
btCollisionObject.h
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:51
btMultiBodyConstraint::jacobianB
btScalar * jacobianB(int row)
Definition: btMultiBodyConstraint.h:168
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:237
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:45
btMultiBodyJointMotor::getIslandIdA
virtual int getIslandIdA() const
Definition: btMultiBodyJointMotor.cpp:70
btMultiBodyJointMotor::btMultiBodyJointMotor
btMultiBodyJointMotor(btMultiBody *body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
This file was written by Erwin Coumans.
Definition: btMultiBodyJointMotor.cpp:23
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926